// 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 <OgnGetMatrix4RotationDatabase.h>
#include <omni/graph/core/PreUsdInclude.h>
#include <pxr/base/gf/rotation.h>
#include <omni/graph/core/PostUsdInclude.h>


namespace omni
{
namespace graph
{
namespace nodes
{

namespace
{
    void getRotation(const double input[16], double result[3])
    {
        pxr::GfVec3d rotation = reinterpret_cast<const pxr::GfMatrix4d*>(input)->DecomposeRotation(pxr::GfVec3d::XAxis(), pxr::GfVec3d::YAxis(), pxr::GfVec3d::ZAxis());
        memcpy(result, rotation.data(), sizeof(double) * 3);
    }
}

class OgnGetMatrix4Rotation
{
public:
    static bool compute(OgnGetMatrix4RotationDatabase& db)
    {
        const auto& matrixInput = db.inputs.matrix();
        auto rotationOutput = db.outputs.rotation();
        bool failed = true;

        // Singular case
        if (auto matrix = matrixInput.get<double[16]>())
        {           
            if (auto output = rotationOutput.get<double[3]>())
            {
                getRotation(*matrix, *output);
                failed = false;
            }
        }
        // Array case
        else if (auto matrices = matrixInput.get<double[][16]>())
        {
            if (auto output = rotationOutput.get<double[][3]>())
            {
                output->resize(matrices->size());
                for (size_t i = 0; i < matrices.size(); i++)
                {
                    getRotation((*matrices)[i], (*output)[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, output with depth %s", 
                    matrixInput.type().arrayDepth, rotationOutput.type().arrayDepth);
            return false;
        }

        return true;
    }
    
    static void onConnectionTypeResolve(const NodeObj& node) {
        auto start = node.iNode->getAttribute(node, OgnGetMatrix4RotationAttributes::inputs::matrix.m_name);
        auto result = node.iNode->getAttribute(node, OgnGetMatrix4RotationAttributes::outputs::rotation.m_name);
        auto startType = start.iAttribute->getResolvedType(start);

        std::array<AttributeObj, 2> attrs { start, result };
        std::array<uint8_t, 2> tupleCounts { 16, 3 };
        std::array<uint8_t, 2> arrayDepths { startType.arrayDepth, startType.arrayDepth };
        std::array<AttributeRole, 2> rolesBuf { AttributeRole::eNone, AttributeRole::eVector };
        node.iNode->resolvePartiallyCoupledAttributes(node, attrs.data(), tupleCounts.data(), 
                                                    arrayDepths.data(), rolesBuf.data(), attrs.size());
    }
};

REGISTER_OGN_NODE()

}
}
}
