/*
 * Skin plug-in
 */

/****************************************************************************
 *                                                                          *
 *  Module  :   matblend.c                                                  *
 *                                                                          *
 *  Purpose :   Matrix-blending pipeline for PS2                            *
 *                                                                          *
 ****************************************************************************/

/****************************************************************************
 Includes
 */

#if (defined(__R5900__) || defined(DOXYGEN))

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <math.h>
#include <float.h>

#include <rpcriter.h>
#include "rpplugin.h"
#include <rpdbgerr.h>

#include <rwcore.h>
#include <rpworld.h>
#include <rprandom.h>
#include <rpspline.h>

#include "rpskin310.h"
#include "skinpriv.h"
#include "matblend.h"
#include "SKY2_SkinGeneric/SkinGeneric.h"

static const char   rcsid[] __RWUNUSED__ =
    "@@(#)$Id: matblend.c,v 1.16 2001/09/26 12:39:44 johns Exp $";

/****************************************************************************
 Local Defines
 */
#define _EPSILON        ((RwReal)(0.001))

/****************************************************************************
 Globals (across program)
 */

RpMatBlendState MatBlend = { (RxPipeline *) NULL,
                             (RxPipeline *) NULL,
                             (RxPipeline *) NULL,
                             (RxWorldLightingCallBack) NULL };

/*==========================================================================*/
/*                                                                          */
/*                               Functions                                  */
/*                                                                          */
/*==========================================================================*/

/****************************************************************************
 _rpSkinMBPipelineDestroy
 Pipeline Destruction.
 Inputs :   None
 Outputs:   None
 */
void
_rpSkinMBPipelineDestroy(void)
{
    RWFUNCTION(RWSTRING("_rpSkinMBPipelineDestroy"));

#ifdef SKY2_DRVMODEL_H
    if (NULL != MatBlend.AtomicGenericSkinPipe)
    {
        RxPipelineDestroy(MatBlend.AtomicGenericSkinPipe);
        MatBlend.AtomicGenericSkinPipe = (RxPipeline *) NULL;
    }
    if (NULL != MatBlend.AtomicPS2AllSkinPipe)
    {
        RxPipelineDestroy(MatBlend.AtomicPS2AllSkinPipe);
        MatBlend.AtomicPS2AllSkinPipe = (RxPipeline *) NULL;
    }
    if (NULL != MatBlend.AtomicPS2AllMatSkinPipe)
    {
        RxPipelineDestroy(MatBlend.AtomicPS2AllMatSkinPipe);
        MatBlend.AtomicPS2AllMatSkinPipe = (RxPipeline *) NULL;
    }
    MatBlend.lightingCallBack = (RxWorldLightingCallBack)NULL;
#endif /*  SKY2_DRVMODEL_H */

    RWRETURNVOID();
}

/****************************************************************************
 _rpSkinMBPluginAttach
 Initialise the matrix blending pipelines (PS2Manager and PS2All versions).
 Inputs :   None
 Outputs:   RwBool              - TRUE on success
 */
RwBool
_rpSkinMBPluginAttach(void)
{
#ifdef SKY2_DRVMODEL_H

    RxPipeline *ps2ManPipe;
    RxPipeline *ps2AllMatPipe;
    RxPipeline *ps2AllPipe;

    RWFUNCTION(RWSTRING("_rpSkinMBPluginAttach"));

    ps2ManPipe    = RpSkinGenericPipeCreate();
    ps2AllMatPipe = RpSkinPS2AllMatPipeCreate();
    ps2AllPipe    = RpSkinPS2AllPipeCreate(ps2AllMatPipe);

    if ((NULL != ps2ManPipe) &&
        (NULL != ps2AllPipe) &&
        (NULL != ps2AllMatPipe) )
    {
        MatBlend.AtomicGenericSkinPipe   = ps2ManPipe;
        MatBlend.AtomicPS2AllSkinPipe    = ps2AllPipe;
        MatBlend.AtomicPS2AllMatSkinPipe = ps2AllMatPipe;
        MatBlend.lightingCallBack        = (RxWorldLightingCallBack)NULL;

        RWRETURN(TRUE);
    }

    if (NULL != ps2ManPipe)    RxPipelineDestroy(ps2ManPipe);
    if (NULL != ps2AllPipe)    RxPipelineDestroy(ps2AllPipe);
    if (NULL != ps2AllMatPipe) RxPipelineDestroy(ps2AllMatPipe);

#endif /* SKY2_DRVMODEL_H */
    RWRETURN(FALSE);
}

/**
 * \ingroup rpskin310
 * \ref RpSkinSetPS2Lighting 
 *
 * This function allows you to overload the lighting of the 
 * skinning pipeline (both the PS2Manager.csl and PS2All.csl
 * pipelines are overloaded by this function).
 *
 * If NULL is passed to this function, the default lighting
 * functionality will be reinstated.
 *
 * This function should be called after unlocking the pipeline containing
 * this node.
 *
 * \param newLightingFunc The \ref RxWorldLightingCallBack to use
 *
 * \see RxPipelineNodePS2ManagerSetLighting
 */
void
RpSkinSetPS2Lighting(RxWorldLightingCallBack newLightingFunc)
{
    RxPipelineNode     *plnode;
    RxNodeDefinition   *manager =
        RxNodeDefinitionGetPS2Manager(rxOBJTYPE_ATOMIC);

    RWAPIFUNCTION(RWSTRING("RpSkinSetPS2Lighting"));

    plnode = RxPipelineFindNodeByName(MatBlend.AtomicGenericSkinPipe,
                                      manager->name,
                                      (RxPipelineNode *) NULL,
                                      (RwInt32 *) NULL);

    RWASSERT(plnode != NULL);

    RxPipelineNodePS2ObjAllInOneSetLighting(plnode, newLightingFunc);

    /* The skinning PS2All ObjectSetupCB gets the callback
     * through the global gSkinLightingCallBack */
    MatBlend.lightingCallBack = newLightingFunc;

    RWRETURNVOID();
}

/**
 * \ingroup rpskin310
 * \ref RpSkinGetPS2AllPipeline
 *
 * This function allows you to get a version of the skinning atomic object
 * pipeline based on the PS2All.csl node. This can then be applied to skinned
 * atomics with \ref RpAtomicSetPipeline
 *
 * \return A pointer to the PS2All skinning pipeline on success, otherwise NULL
 *
 * \see RxNodeDefinitionGetPS2All
 * \see RpAtomicSetPipeline
 */
RxPipeline *
RpSkinGetPS2AllPipeline(void)
{
    RWAPIFUNCTION(RWSTRING("RpSkinGetPS2AllPipeline"));

    RWRETURN(MatBlend.AtomicPS2AllSkinPipe);
}


/****************************************************************************
 _rpSkinMBPluginAttach
 Initialise an atomic's matrix weight data for dispatch to the VU.  Weights
 are stored in a 32 bit RwReal, with the least significant 16 bits used to
 encode the matrix index.
 Inputs :   RpAtomic *           - Pointer to a skin atomic.
 Outputs:   RwBool               - TRUE on success
 */
RwBool
_rpSkinMBInitAtomic(RpAtomic * atomic)
{
    RpSkin             *pSkin = *RPSKINATOMICGETDATA(atomic);

#ifdef SKY2_DRVMODEL_H
    RwInt32             i;
    RwUInt32            u;
    RwUInt32            m;
#endif /* SKY2_DRVMODEL_H */

    RWFUNCTION(RWSTRING("_rpSkinMBInitAtomic"));

    RpAtomicSetPipeline(atomic, MatBlend.AtomicGenericSkinPipe);

    if (pSkin == NULL)
    {
        RWRETURN(FALSE);
    }

#ifdef SKY2_DRVMODEL_H

    for (i = 0; i < pSkin->totalVertices; i++)
    { /*--------------- Index and weight for Bone 1 ----------------*/
        u = *((RwUInt32 *) & (pSkin->pMatrixWeightsMap[i].w0));
        u &= 0xFFFFFC00;
        m = (pSkin->pMatrixIndexMap[i] & 0xFF);
        RWASSERT((m != 0xFF)
                 && (pSkin->pMatrixWeightsMap[i].w0 > 0.0f));
        u |= (m + 1) * 4;
        *((RwUInt32 *) & (pSkin->pMatrixWeightsMap[i].w0)) = u;
  /*--------------- Index and weight for Bone 2 ----------------*/
        u = *((RwUInt32 *) & (pSkin->pMatrixWeightsMap[i].w1));
        u &= 0xFFFFFC00;
        m = ((pSkin->pMatrixIndexMap[i] >> 8) & 0xFF);
        if ((m == 0xFF) || (pSkin->pMatrixWeightsMap[i].w1 == 0.0f))
        {
            u = 0;             /* No matrix - force to 0 */
        }
        else
        {
            u |= (m + 1) * 4;
        }
        *((RwUInt32 *) & (pSkin->pMatrixWeightsMap[i].w1)) = u;
  /*--------------- Index and weight for Bone 3 ----------------*/
        u = *((RwUInt32 *) & (pSkin->pMatrixWeightsMap[i].w2));
        u &= 0xFFFFFC00;
        m = ((pSkin->pMatrixIndexMap[i] >> 16) & 0xFF);
        if ((m == 0xFF) || (pSkin->pMatrixWeightsMap[i].w2 == 0.0f))
        {
            u = 0;             /* No matrix - force to 0 */
        }
        else
        {
            u |= (m + 1) * 4;
        }
        *((RwUInt32 *) & (pSkin->pMatrixWeightsMap[i].w2)) = u;
  /*--------------- Index and weight for Bone 4 ----------------*/
        u = *((RwUInt32 *) & (pSkin->pMatrixWeightsMap[i].w3));
        u &= 0xFFFFFC00;
        m = ((pSkin->pMatrixIndexMap[i] >> 24) & 0xFF);
        if ((m == 0xFF) || (pSkin->pMatrixWeightsMap[i].w3 == 0.0f))
        {
            u = 0;             /* No matrix - force to 0 */
        }
        else
        {
            u |= (m + 1) * 4;
        }
        *((RwUInt32 *) & (pSkin->pMatrixWeightsMap[i].w3)) = u;
    }

#endif /* SKY2_DRVMODEL_H */

    pSkin->pPlatformData = NULL;

    RWRETURN(TRUE);
}

void
_rpSkinMBDestroyAtomic(RpSkin * pSkin)
{
    RWFUNCTION(RWSTRING("_rpSkinMBDestroyAtomic"));

    if (pSkin && pSkin->pPlatformData)
    {
        RwFree(pSkin->pPlatformData);
        pSkin->pPlatformData = NULL;
    }

    RWRETURNVOID();
}

/****************************************************************************
 _rpSkinMBWeightsInstancing
 Vertices weights and indices instance function
 Inputs :  *mesh                - Pointer on the current mesh
           *pSkin               - Pointer on the current skin
           *destMBI             - Pointer on the weights/indices cache
 Outputs:   None
 */
void
_rpSkinMBWeightsInstancing(const RpMesh * mesh,
                           RpSkin * pSkin,
                           RwReal * destMBI)
{
    const RxVertexIndex *meshIndices = mesh->indices;
    RwMatrixWeights    *srcMBI = pSkin->pMatrixWeightsMap;
    RwInt32             numIndices = mesh->numIndices;
    RwInt32             i;

    RWFUNCTION(RWSTRING("_rpSkinMBWeightsInstancing"));

    RWASSERT(NULL != mesh);

        /*--- Filling cluster number 5 ---*/
    for (i = 0; i < numIndices; i++)
    {
        destMBI[0] = srcMBI[*meshIndices].w0;
        destMBI[1] = srcMBI[*meshIndices].w1;
        destMBI[2] = srcMBI[*meshIndices].w2;
        destMBI[3] = srcMBI[*meshIndices].w3;
        meshIndices++;
        destMBI += 4;
    }

    RWRETURNVOID();
}

/****************************************************************************
 _rpSkinMBMatUpdating
 Skin matrices update function
 Inputs :   *atomic                - Pointer on the current atomic
           *pSkin                  - Pointer on the current skin
 Outputs:   RwMatrix *             - Pointer on the Matrix cache
 */
RwMatrix           *
_rpSkinMBMatUpdating(RpAtomic * atomic, RpSkin * pSkin)
{
    RpHAnimHierarchy   *pCurrentHierarchy; /* Current skin hierarchy */

    RWFUNCTION(RWSTRING("_rpSkinMBMatUpdating"));

    RpSkinAtomicGlobals.SkinMatrixCacheSkin = pSkin;
    RpSkinAtomicGlobals.SkinMatrixCacheRenderFrame =
        RWSRCGLOBAL(renderFrame);

    /*
     * Test for a hierarchy or skeleton. In theory, both shouldn't exist
     * simultaneously, but if they do then the hierarchy takes precedence.
     */

    pCurrentHierarchy = pSkin->pCurrentHierarchy;
    if (pCurrentHierarchy)
    {
        RwMatrix           *pMBA = RpSkinAtomicGlobals.SkinMatrixCache;
        const RwInt32       numNodes = pCurrentHierarchy->numNodes;
        const RwMatrix * const pMatrixArray = pCurrentHierarchy->pMatrixArray;
        const RpSkinBoneInfo * const pBoneInfo = pSkin->pBoneInfo;

        if (pCurrentHierarchy-> flags & rpHANIMHIERARCHYNOMATRICES)
        {
            RwMatrix            inverseAtomicLTM;
            RwMatrix            prodMatrix;
            RwInt32             i;
            const RpHAnimNodeInfo * const pNodeInfo =
                                pCurrentHierarchy->pNodeInfo;

            inverseAtomicLTM.flags = 0;
            prodMatrix.flags = 0;

            RwMatrixInvert(&inverseAtomicLTM,
                           RwFrameGetLTM(RpAtomicGetFrame(atomic)));

            for (i = 0; i < numNodes; i++)
            {
                RwFrame            * const pFrame = pNodeInfo[i].pFrame;

                SkinMatrixMultiplyMacro5900(&prodMatrix,
                                            &pBoneInfo[i].boneToSkinMat,
                                            RwFrameGetLTM(pFrame));
                SkinMatrixMultiplyMacro5900(&pMBA[i], &prodMatrix,
                                            &inverseAtomicLTM);
            }
            RWRETURN(pMBA);
        }
        else
        {
            if (pCurrentHierarchy->flags &
                rpHANIMHIERARCHYLOCALSPACEMATRICES)
            {
                RwInt32 i;

                for (i = 0; i < numNodes; i++)
                {
                    SkinMatrixMultiplyMacro5900(&pMBA[i],
                                                &pBoneInfo[i].boneToSkinMat,
                                                &pMatrixArray[i]);
                }

                RWRETURN(pMBA);
            }
            else
            {
                RwMatrix inverseAtomicLTM;
                RwMatrix prodMatrix;
                RwInt32 i;

                inverseAtomicLTM.flags = 0;
                prodMatrix.flags = 0;

                RwMatrixInvert(&inverseAtomicLTM,
                               RwFrameGetLTM(RpAtomicGetFrame(atomic)));

                for (i = 0; i < numNodes; i++)
                {
                    SkinMatrixMultiplyMacro5900(&prodMatrix,
                                                &pBoneInfo[i].boneToSkinMat,
                                                &pMatrixArray[i]);

                    SkinMatrixMultiplyMacro5900(&pMBA[i],
                                                &prodMatrix,
                                                &inverseAtomicLTM);
                }

                RWRETURN(pMBA);
            }

/* The following assembly version of the above has been removed pending a fix.
 * This fails in a sky295 optimized build.
 */
#if (0)
            if (pCurrentHierarchy->flags &
                rpHANIMHIERARCHYLOCALSPACEMATRICES)
            {
/* *INDENT-OFF* */
                asm volatile ("                         ;
                       .set noreorder                   ;
                        mult %3, %3, %5                 ;
                        add  %3, %2, %3                 ;
                SKMacLoop1:                             ;
                        lqc2 vf1, 0x00(%0)              ;
                        lqc2 vf2, 0x10(%0)              ;
                        lqc2 vf3, 0x20(%0)              ;
                        lqc2 vf4, 0x30(%0)              ;
                                                        ;
                        lqc2 vf5, 0x00(%1)              ;
                        lqc2 vf6, 0x10(%1)              ;
                        lqc2 vf7, 0x20(%1)              ;
                        lqc2 vf8, 0x30(%1)              ;
                                                        ;
                        vmulax.xyzw  ACC, vf5, vf1      ;
                        vmadday.xyzw ACC, vf6, vf1      ;
                        vmaddz.xyzw  vf9, vf7, vf1      ;
                                                        ;
                        vmulax.xyzw  ACC, vf5, vf2      ;
                        vmadday.xyzw ACC, vf6, vf2      ;
                        vmaddz.xyzw vf10, vf7, vf2      ;
                        vmulax.xyzw  ACC, vf5, vf3      ;
                        vmadday.xyzw ACC, vf6, vf3      ;
                        vmaddz.xyzw vf11, vf7, vf3      ;
                        vmulax.xyzw  ACC, vf5, vf4      ;
                        vmadday.xyzw ACC, vf6, vf4      ;
                        vmaddaz.xyzw ACC, vf7, vf4      ;
                        vmaddw.xyzw vf12, vf8, vf0      ;
                                                        ;
                        sqc2 vf9,  0x00(%2)             ;
                        sqc2 vf10, 0x10(%2)             ;
                        sqc2 vf11, 0x20(%2)             ;
                        sqc2 vf12, 0x30(%2)             ;
                                                        ;
                        addi %2, %2, 0x40               ;
                        add  %0, %0, %4                 ;
                        add  %1, %1, %5                 ;
                        bne  %3, %2, SKMacLoop1         ;
                        nop
                    .set reorder"   :: "r" (&pBoneInfo[0].boneToSkinMat),
                                        "r" (&pMatrixArray[0]),
                                        "r" (&pMBA[0]),
                                        "r" (numFrames),
                                        "r" (sizeof(RpSkinBoneInfo)),
                                        "r" (sizeof(RwMatrix))
                                    :   "cc", "memory" );
/* *INDENT-ON* */

                RWRETURN(pMBA);
            }
            else
            {
                RwMatrix            inverseAtomicLTM;

                RwMatrixInvert(&inverseAtomicLTM,
                               RwFrameGetLTM(RpAtomicGetFrame(atomic)));


/* *INDENT-OFF* */
                        asm volatile ("                 ;
                    .set noreorder                      ;
                        mult %4, %4, %6                 ;
                        lqc2 vf13, 0x00(%2)             ;
                        lqc2 vf14, 0x10(%2)             ;
                        lqc2 vf15, 0x20(%2)             ;
                        lqc2 vf16, 0x30(%2)             ;
                        add  %4, %3, %4                 ;
                SKMacLoop2:                             ;
                        lqc2 vf1, 0x00(%0)              ;
                        lqc2 vf2, 0x10(%0)              ;
                        lqc2 vf3, 0x20(%0)              ;
                        lqc2 vf4, 0x30(%0)              ;
                                                        ;
                        lqc2 vf5, 0x00(%1)              ;
                        lqc2 vf6, 0x10(%1)              ;
                        lqc2 vf7, 0x20(%1)              ;
                        lqc2 vf8, 0x30(%1)              ;
                                                        ;
                        vmulax.xyzw  ACC, vf5, vf1      ;
                        vmadday.xyzw ACC, vf6, vf1      ;
                        vmaddz.xyzw  vf9, vf7, vf1      ;
                                                        ;
                        vmulax.xyzw  ACC, vf5, vf2      ;
                        vmadday.xyzw ACC, vf6, vf2      ;
                        vmaddz.xyzw vf10, vf7, vf2      ;
                                                        ;
                        vmulax.xyzw  ACC, vf5, vf3      ;
                        vmadday.xyzw ACC, vf6, vf3      ;
                        vmaddz.xyzw vf11, vf7, vf3      ;
                                                        ;
                        vmulax.xyzw  ACC, vf5, vf4      ;
                        vmadday.xyzw ACC, vf6, vf4      ;
                        vmaddaz.xyzw ACC, vf7, vf4      ;
                        vmaddw.xyzw vf12, vf8, vf0      ;
                                                        ;
                        vmulax.xyzw  ACC, vf13, vf9     ;
                        vmadday.xyzw ACC, vf14, vf9     ;
                        vmaddz.xyzw  vf1, vf15, vf9     ;
                                                        ;
                        vmulax.xyzw  ACC, vf13, vf10    ;
                        vmadday.xyzw ACC, vf14, vf10    ;
                        vmaddz.xyzw  vf2, vf15, vf10    ;
                                                        ;
                        vmulax.xyzw  ACC, vf13, vf11    ;
                        vmadday.xyzw ACC, vf14, vf11    ;
                        vmaddz.xyzw  vf3, vf15, vf11    ;
                                                        ;
                        vmulax.xyzw  ACC, vf13, vf12    ;
                        vmadday.xyzw ACC, vf14, vf12    ;
                        vmaddaz.xyzw ACC, vf15, vf12    ;
                        vmaddw.xyzw  vf4, vf16, vf0     ;
                                                        ;
                        sqc2 vf1, 0x00(%3)              ;
                        sqc2 vf2, 0x10(%3)              ;
                        sqc2 vf3, 0x20(%3)              ;
                        sqc2 vf4, 0x30(%3)              ;
                                                        ;
                        addi %3, %3, 0x40               ;
                        add  %0, %0, %5                 ;
                        add  %1, %1, %6                 ;
                        bne  %4, %3, SKMacLoop2         ;
                        nop
                    .set reorder"    :: "r" (&pBoneInfo[0].boneToSkinMat),
                                        "r" (&pMatrixArray[0]),
                                        "r" (&inverseAtomicLTM),
                                        "r" (&pMBA[0]),
                                        "r" (numFrames),
                                        "r" (sizeof(RpSkinBoneInfo)),
                                        "r" (sizeof(RwMatrix))
                                    :   "cc", "memory" );
/* *INDENT-ON* */

                RWRETURN(pMBA);
            }

#endif /* (0) */

        }
    }

    /* Is there a skeleton instead? */
    if (pSkin->pCurrentSkeleton)
    {
        RWRETURN(pSkin->pCurrentSkeleton->pMatrixArray);
    }

    RWRETURN((RwMatrix *)NULL);
}

/****************************************************************************
 rpSkinGetPipeline
 Retrieves a pointer to the default skin pipeline
 Inputs :   None
 Outputs:   RxPipeline                 - Non NULL on Success
 */
RxPipeline         *
_rpSkinGetDefaultPipeline(void)
{
    RxPipeline         *result = MatBlend.AtomicGenericSkinPipe;

    RWFUNCTION(RWSTRING("_rpSkinGetDefaultPipeline"));

    RWRETURN(result);
}

#endif /* (defined(__R5900__) || defined(DOXYGEN)) */
