//
// Generated by dtk
// Translation Unit: J3DCluster.cpp
//

#include "JSystem/J3DGraphAnimator/J3DCluster.h"
#include "JSystem/J3DGraphAnimator/J3DModel.h"
#include "JSystem/J3DGraphAnimator/J3DSkinDeform.h"
#include "JSystem/J3DGraphAnimator/J3DAnimation.h"
#include "JSystem/JKernel/JKRHeap.h"
#include "dolphin/os/OS.h"
#include "string.h"

/* 802F37C4-802F37E4       .text clear__13J3DDeformDataFv */
void J3DDeformData::clear() {
    mClusterNum = 0;
    mClusterPointer = NULL;
    mClusterKeyNum = 0;
    mClusterKeyPointer = NULL;
    mClusterName = NULL;
    mClusterKeyName = NULL;
}

/* 802F37E4-802F3814       .text __ct__13J3DDeformDataFv */
J3DDeformData::J3DDeformData() {
    clear();
}

/* 802F3814-802F3838       .text deform__13J3DDeformDataFP8J3DModel */
void J3DDeformData::deform(J3DModel* model) {
    deform(model->getVertexBuffer());
}

/* 802F3838-802F3900       .text deform__13J3DDeformDataFP15J3DVertexBuffer */
void J3DDeformData::deform(J3DVertexBuffer* vtx) {
    vtx->swapVtxPosArrayPointer();
    vtx->swapVtxNrmArrayPointer();

    for (u16 i = 0; i < mClusterNum; i++)
        mClusterPointer[i].getDeformer()->deform(vtx, i);

    DCStoreRange(vtx->getVtxPosArrayPointer(0), vtx->getVertexData()->getVtxNum() * 12);
    DCStoreRange(vtx->getVtxNrmArrayPointer(0), vtx->getVertexData()->getNrmNum() * 12);
    vtx->setCurrentVtxPos(vtx->getVtxPosArrayPointer(0));
    vtx->setCurrentVtxNrm(vtx->getVtxNrmArrayPointer(0));
}

/* 802F3900-802F3920       .text clear__11J3DDeformerFv */
void J3DDeformer::clear() {
    mDeformData = NULL;
    mAnmCluster = NULL;
    mWeightList = NULL;
    field_0x0c = NULL;
    mFlags = 3;
}

/* 802F3920-802F3A08       .text deform__11J3DDeformerFP15J3DVertexBufferUs */
void J3DDeformer::deform(J3DVertexBuffer* vtx, u16 idx) {
    u16 keyIdx = 0;
    if (mAnmCluster) {
        for (u16 i = 0; i < idx; i++)
            keyIdx += mDeformData->getClusterPointer(i)->mKeyNum;

        for (u16 i = 0; i < mDeformData->getClusterPointer(idx)->mKeyNum; i++)
            mWeightList[i] = mAnmCluster->getWeight(keyIdx++);

        deform(vtx, idx, mWeightList);
    }
}

/* 802F3A08-802F3FA8       .text deform__11J3DDeformerFP15J3DVertexBufferUsPf */
void J3DDeformer::deform(J3DVertexBuffer* vtx, u16 idx, f32* weightList) {
    /* Nonmatching */
    if (!!(mFlags & 2) && vtx->getVertexData()->getVtxPosType() == GX_F32) {
        J3DCluster* cluster = mDeformData->getClusterPointer(idx);
        s32 posNum = cluster->mPosNum;
        s32 keyNum = cluster->mKeyNum;

        u16 keyStart = 0;
        for (u16 i = 0; i < idx; i++)
            keyStart += mDeformData->getClusterPointer(i)->mKeyNum + 1;

        J3DClusterKey* key = mDeformData->getClusterKeyPointer(keyStart);
        normalizeWeight(keyNum, weightList);

        f32* vtxPosDst = (f32*)vtx->getVtxPosArrayPointer(0);
        f32* vtxPosSrc = (f32*)mDeformData->getVtxPos();

        for (s32 i = 0; i < posNum; i++) {
            f32* vtx = &vtxPosDst[cluster->mPosDstIdx[i] * 3];
            vtx[0] = 0.0f;
            vtx[1] = 0.0f;
            vtx[2] = 0.0f;
        }

        f32 sign[2] = { 1.0f, -1.0f };
        for (u16 i = 0; i < posNum; i++) {
            for (u16 j = 0; j < keyNum; j++) {
                u16 flag = (u16)key[j].mPosFlag[i];
                f32* src = &vtxPosSrc[(flag & 0x1FFF) * 3];
                f32* dst = &vtxPosDst[cluster->mPosDstIdx[i] * 3];
                dst[0] += weightList[j] * src[0] * sign[(flag >> 15) & 1];
                dst[1] += weightList[j] * src[1] * sign[(flag >> 14) & 1];
                dst[2] += weightList[j] * src[2] * sign[(flag >> 13) & 1];
            }
        }

        if (!!(mFlags & 1) && cluster->mFlags != 0 && vtx->getVertexData()->getVtxNrmType() == GX_F32) {
            f32* vtxNrmDst = (f32*)vtx->getVtxNrmArrayPointer(0);
            f32* vtxNrmSrc = mDeformData->getVtxNrm();
            f32* nrmBuf = field_0x0c;

            for (u16 i = 0; i < cluster->mNrmNum; i++) {
                f32* dst = &nrmBuf[i * 3];
                dst[0] = 0.0f;
                dst[1] = 0.0f;
                dst[2] = 0.0f;

                for (u16 j = 0; j < cluster->mKeyNum; j++) {
                    u16 flag = key[j].mNrmFlag[i];
                    f32* src = &vtxNrmSrc[(flag & 0x1FFF) * 3];

                    f32 srcX = src[0];
                    f32 srcY = src[1];
                    f32 srcZ = src[2];

                    if (flag & 0x8000)
                        srcX = -srcX;
                    if (flag & 0x4000)
                        srcY = -srcY;
                    if (flag & 0x2000)
                        srcZ = -srcZ;

                    dst[0] += srcX * weightList[j];
                    dst[1] += srcY * weightList[j];
                    dst[2] += srcZ * weightList[j];
                }

                normalize(dst);
            }

            for (u16 i = 0; i < cluster->mClusterVertexNum; i++) {
                J3DClusterVertex* vertex = &cluster->mClusterVertex[i];
                f32 pos[3];
                pos[0] = 0.0f;
                pos[1] = 0.0f;
                pos[2] = 0.0f;

                f32 weight = 1.0f / (float)vertex->mNum;
                for (u16 j = 0; j < vertex->mNum; j++) {
                    f32* src = &nrmBuf[vertex->mSrcIdx[j] * 3];
                    pos[0] += weight * src[0];
                    pos[1] += weight * src[1];
                    pos[2] += weight * src[2];
                }
                normalize(pos);

                for (u16 j = 0; j < vertex->mNum; j++) {
                    u16 srcIdx = vertex->mSrcIdx[j];
                    u16 dstIdx = vertex->mDstIdx[j];

                    if (dstIdx == 0xFFFF)
                        continue;

                    f32* src = &nrmBuf[srcIdx * 3];
                    f32 dot = pos[0]*src[0] + pos[1]*src[1] + pos[2]*src[2];

                    f32 angle;
                    if (dot >= 1.0f) {
                        angle = 0.0f;
                    } else if (dot > -1.0f) {
                        angle = acos(dot);
                        angle = (angle * 180.0f) / 3.1415f;
                    } else {
                        angle = 180.0f;
                    }

                    if (angle <= cluster->mMinAngle) {
                        vtxNrmDst[dstIdx * 3 + 0] = pos[0];
                        vtxNrmDst[dstIdx * 3 + 1] = pos[1];
                        vtxNrmDst[dstIdx * 3 + 2] = pos[2];
                    } else if (angle > cluster->mMaxAngle) {
                        vtxNrmDst[dstIdx * 3 + 0] = src[0];
                        vtxNrmDst[dstIdx * 3 + 1] = src[1];
                        vtxNrmDst[dstIdx * 3 + 2] = src[2];
                    } else {
                        f32 weight = (angle - cluster->mMinAngle) / (cluster->mMaxAngle - cluster->mMinAngle);
                        f32 inv = 1.0f - weight;

                        vtxNrmDst[dstIdx * 3 + 0] = inv * pos[0] + weight * src[0];
                        vtxNrmDst[dstIdx * 3 + 1] = inv * pos[1] + weight * src[1];
                        vtxNrmDst[dstIdx * 3 + 2] = inv * pos[2] + weight * src[2];
                    }
                }
            }
        }
    }
}

/* 802F3FA8-802F4064       .text normalize__11J3DDeformerFPf */
void J3DDeformer::normalize(f32* vec) {
    f32 inv = 1.0f / sqrtf(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]);
    vec[0] *= inv;
    vec[1] *= inv;
    vec[2] *= inv;
}

/* 802F4064-802F40C0       .text normalizeWeight__11J3DDeformerFiPf */
void J3DDeformer::normalizeWeight(int count, f32* weight) {
    f32 sum = 0.0f;
    for (u16 i = 0; i < count; i++)
        sum += weight[i];
    sum = 1.0f / sum;
    for (u16 i = 0; i < count; i++)
        weight[i] *= sum;
}

/* 802F40C0-802F40F0       .text __ct__13J3DSkinDeformFv */
J3DSkinDeform::J3DSkinDeform() {
    mPosUseMtx = NULL;
    mNrmUseMtx = NULL;
    mNrmMtx = NULL;
    mFlags = 3;
    field_0x14 = 1;
}

/* 802F40F0-802F44E8       .text initMtxIndexArray__13J3DSkinDeformFP12J3DModelData */
int J3DSkinDeform::initMtxIndexArray(J3DModelData* modelData) {
    /* Nonmatching - regalloc */

    if (mPosUseMtx != NULL && mNrmUseMtx != NULL)
        return J3DErrType_Success;

    mPosUseMtx = new u16[modelData->getVtxNum()];
    if (mPosUseMtx == NULL)
        return J3DErrType_OutOfMemory;

    for (u32 i = 0; i < modelData->getVtxNum(); i++)
        mPosUseMtx[i] = 0xFFFF;

    if (modelData->getNrmNum() != 0) {
        mNrmUseMtx = new u16[modelData->getNrmNum()];
        if (mNrmUseMtx == NULL)
            return J3DErrType_OutOfMemory;

        for (u32 i = 0; i < modelData->getNrmNum(); i++)
            mNrmUseMtx[i] = 0;
    } else {
        mNrmUseMtx = NULL;
    }

    mNrmMtx = new(0x20) Mtx33[modelData->getDrawMtxNum()];
    if (mNrmMtx == NULL)
        return J3DErrType_OutOfMemory;

    for (u16 i = 0; i < modelData->getShapeNum(); i++) {
        int size[4] = { 0, 1, 1, 2 };

        int pnmtxIdxOffs = -1;
        int posOffs = -1;
        int nrmOffs = -1;
        int vtxSize = 0;
        for (GXVtxDescList* desc = modelData->getShapeNodePointer(i)->getVtxDesc(); desc->attr != GX_VA_NULL; desc++) {
            switch (desc->attr) {
            case GX_VA_PNMTXIDX:
                pnmtxIdxOffs = vtxSize;
                break;
            case GX_VA_POS:
                posOffs = vtxSize;
                if (desc->type != GX_INDEX16) {
                    OSReport(" Invlid Data : CPU Pipeline process GX_INDEX16 Data Only\n");
                }
                break;
            case GX_VA_NRM:
                nrmOffs = vtxSize;
                if (desc->type != GX_INDEX16) {
                    OSReport(" Invlid Data : CPU Pipeline process GX_INDEX16 Data Only\n");
                }
                break;
            case GX_VA_TEX0:
                if (desc->type != GX_INDEX16) {
                    OSReport(" Invlid Data : CPU Pipeline process GX_INDEX16 Data Only\n");
                }
                break;
            }

            vtxSize += size[desc->type];
        }

        for (u16 j = 0; j < modelData->getShapeNodePointer(i)->getMtxGroupNum(); j++) {
            J3DShapeMtx* shapeMtx = modelData->getShapeNodePointer(i)->getShapeMtx(j);

            u8* displayListStart = modelData->getShapeNodePointer(i)->getShapeDraw(j)->getDisplayList();
            for (u8* dl = displayListStart; (dl - displayListStart) < modelData->getShapeNodePointer(i)->getShapeDraw(j)->getDisplayListSize(); ) {
                u8 cmd = dl[0];
                if (cmd != GX_TRIANGLEFAN && cmd != GX_TRIANGLESTRIP)
                    break;

                u16 vtxCount = *(u16*)&dl[1];

                u16 useMtxIdxBuf[10];
                for (s32 k = 0; k < vtxCount; k++) {
                    u8* vtx = &dl[3 + vtxSize * k];
                    u8 pnmtxIdx = (u32)*(u8*)&vtx[pnmtxIdxOffs] / 3;
                    u16 posIdx = *(u16*)&vtx[posOffs];
                    u16 nrmIdx = *(u16*)&vtx[nrmOffs];

                    u16 useMtxIdx = shapeMtx->getUseMtxIndex(pnmtxIdx);
                    if (useMtxIdx == 0xFFFF) {
                        useMtxIdx = useMtxIdxBuf[pnmtxIdx];
                    } else if (pnmtxIdxOffs != -1) {
                        useMtxIdxBuf[pnmtxIdx] = useMtxIdx;
                    }

                    mPosUseMtx[posIdx] = useMtxIdx;
                    if (nrmOffs != -1)
                        mNrmUseMtx[nrmIdx] = useMtxIdx;
                }

                dl += vtxSize * vtxCount;
                dl += 3;
            }

            if (nrmOffs == -1) {
                modelData->getShapeNodePointer(i)->onFlag(J3DShpFlag_EnableLod);
                modelData->getShapeNodePointer(i)->offFlag(J3DShpFlag_SkinNrmCpu);
            }
        }
    }

    for (u32 i = 0; i < modelData->getVtxNum(); i++) {
        if (mPosUseMtx[i] == 0xFFFF) {
            field_0x14 = 0;
            mPosUseMtx[i] = 0;
        }
    }

    return J3DErrType_Success;
}

/* 802F44E8-802F4734       .text changeFastSkinDL__13J3DSkinDeformFP12J3DModelData */
void J3DSkinDeform::changeFastSkinDL(J3DModelData* modelData) {
    /* Nonmatching */
    for (u16 i = 0; i < modelData->getShapeNum(); i++) {
        int size[4] = { 0, 1, 1, 2 };
        int pnmtxIdxOffs = -1;
        int vtxSize = 0;

        for (GXVtxDescList* desc = modelData->getShapeNodePointer(i)->getVtxDesc(); desc->attr != GX_VA_NULL; desc++) {
            if (desc->attr == GX_VA_PNMTXIDX) {
                pnmtxIdxOffs = vtxSize;
            }

            vtxSize += size[desc->type];
        }

        if (pnmtxIdxOffs != -1) {
            for (u16 j = 0; j < modelData->getShapeNodePointer(i)->getMtxGroupNum(); j++) {
                J3DShapeDraw* shapeDraw = modelData->getShapeNodePointer(i)->getShapeDraw(j);
                u8* displayListStart = shapeDraw->getDisplayList();
                u8* dl = displayListStart;
                u8* dst = displayListStart;

                for (; (dl - displayListStart) < shapeDraw->getDisplayListSize(); ) {
                    u8 cmd = dl[0];
                    *dst = cmd;
                    dst += 1;

                    if (cmd != GX_TRIANGLEFAN && cmd != GX_TRIANGLESTRIP)
                        break;

                    u32 vtxCount = *(u16*)&dl[1];
                    *(u16*)dst = vtxCount;
                    dst += 2;

                    u32 vtxSizeDst = vtxSize - 1; // remove GX_VA_PNMTXIDX
                    for (s32 k = 0; k < vtxCount; k++) {
                        memcpy(dst, &dl[vtxSize * k + 3 + 1], vtxSizeDst);
                        dst += vtxSizeDst;
                    }

                    dl += vtxSize * vtxCount + 3;
                }

                u32 newSize = ((u32)dst + 0x1F & ~0x1F) - (u32)displayListStart;

                for (; (dst - displayListStart) < modelData->getShapeNodePointer(i)->getShapeDraw(j)->getDisplayListSize();) {
                    *dst++ = 0;
                }

                shapeDraw->setDisplayListSize(newSize);
                DCStoreRange(displayListStart, shapeDraw->getDisplayListSize());
            }
        }
    }

    for (u16 i = 0; i < modelData->getShapeNum(); i++) {
        J3DShape* shape = modelData->getShapeNodePointer(i);
        GXVtxDescList* desc = shape->getVtxDesc();
        GXVtxDescList* descDst = desc;
        for (; desc->attr != GX_VA_NULL; desc++) {
            if (desc->attr != GX_VA_PNMTXIDX) {
                *descDst = *desc;
                descDst++;
            }
        }
        descDst->attr = GX_VA_NULL;
        descDst->type = GX_NONE;
        shape->makeVcdVatCmd();
    }
}

/* 802F4734-802F4850       .text calcNrmMtx__13J3DSkinDeformFP8J3DModel */
void J3DSkinDeform::calcNrmMtx(J3DModel* model) {
    J3DModelData* modelData = model->getModelData();
    for (u16 i = 0; i < modelData->getDrawMtxNum(); i++) {
        if (modelData->getDrawMtxFlag(i) == 0) {
            if (model->getScaleFlag(modelData->getDrawMtxIndex(i)) == 1) {
                J3DPSMtx33CopyFrom34(model->getAnmMtx(modelData->getDrawMtxIndex(i)), mNrmMtx[i]);
            } else {
                J3DPSCalcInverseTranspose(model->getAnmMtx(modelData->getDrawMtxIndex(i)), mNrmMtx[i]);
            }
        } else {
            if (model->getEnvScaleFlag(modelData->getDrawMtxIndex(i)) == 1) {
                J3DPSMtx33CopyFrom34(model->getWeightAnmMtx(modelData->getDrawMtxIndex(i)), mNrmMtx[i]);
            } else {
                J3DPSCalcInverseTranspose(model->getWeightAnmMtx(modelData->getDrawMtxIndex(i)), mNrmMtx[i]);
            }
        }
    }
}

/* 802F4850-802F4974       .text deformVtxPos_F32__13J3DSkinDeformCFP8J3DModel */
void J3DSkinDeform::deformVtxPos_F32(J3DModel* model) const {
    /* Nonmatching */
    Mtx* mtxArr[2];
    mtxArr[0] = model->mpNodeMtx;
    mtxArr[1] = model->mpWeightEnvMtx;
    model->getVertexBuffer()->swapTransformedVtxPos();

    s32 vtxNum = model->getModelData()->getVtxNum();
    Vec* curVtxPos = (Vec*)model->getCurrentVtxPos();
    Vec* transformedVtxPos = (Vec*)model->getVertexBuffer()->getTransformedVtxPos(0);

    for (s32 i = 0; i < vtxNum; i++) {
        Mtx* mtx = mtxArr[model->getModelData()->getDrawMtxFlag(mPosUseMtx[i])];
        J3DPSMulMtxVec(mtx[model->getModelData()->getDrawMtxIndex(mPosUseMtx[i])], &curVtxPos[i], &transformedVtxPos[i]);
    }

    DCStoreRange(model->getVertexBuffer()->getTransformedVtxPos(0), model->getModelData()->getVtxNum() * sizeof(Vec));
    model->setCurrentVtxPos(transformedVtxPos);
}

/* 802F4974-802F4AB4       .text deformVtxPos_S16__13J3DSkinDeformCFP8J3DModel */
void J3DSkinDeform::deformVtxPos_S16(J3DModel* model) const {
    /* Nonmatching */
    Mtx* mtxArr[2];
    mtxArr[0] = model->mpNodeMtx;
    mtxArr[1] = model->mpWeightEnvMtx;

    J3DGQRSetup7(model->getModelData()->getVertexData().getVtxPosFrac(), 7, model->getModelData()->getVertexData().getVtxPosFrac(), 7);
    model->getVertexBuffer()->swapTransformedVtxPos();

    s32 vtxNum = model->getModelData()->getVtxNum();
    SVec* curVtxPos = (SVec*)model->getCurrentVtxPos();
    SVec* transformedVtxPos = (SVec*)model->getVertexBuffer()->getTransformedVtxPos(0);

    for (s32 i = 0; i < vtxNum; i++) {
        Mtx* mtx = mtxArr[model->getModelData()->getDrawMtxFlag(mPosUseMtx[i])];
        J3DPSMulMtxVec(mtx[model->getModelData()->getDrawMtxIndex(mPosUseMtx[i])], &curVtxPos[i], &transformedVtxPos[i]);
    }

    DCStoreRange(model->getVertexBuffer()->getTransformedVtxPos(0), model->getModelData()->getVtxNum() * sizeof(SVec));
    model->setCurrentVtxPos(transformedVtxPos);
}

/* 802F4AB4-802F4BB8       .text deformVtxNrm_F32__13J3DSkinDeformCFP8J3DModel */
void J3DSkinDeform::deformVtxNrm_F32(J3DModel* model) const {
    /* Nonmatching */
    model->getVertexBuffer()->swapTransformedVtxNrm();

    s32 vtxNum = model->getModelData()->getNrmNum();
    Vec* curVtxNrm = (Vec*)model->getCurrentVtxNrm();
    Vec* transformedVtxNrm = (Vec*)model->getVertexBuffer()->getTransformedVtxNrm(0);

    for (s32 i = 0; i < vtxNum; i++) {
        J3DPSMulMtxVec(mNrmMtx[mNrmUseMtx[i]], &curVtxNrm[i], &transformedVtxNrm[i]);
    }

    DCStoreRange(model->getVertexBuffer()->getTransformedVtxNrm(0), model->getModelData()->getNrmNum() * sizeof(Vec));
    model->setCurrentVtxNrm(transformedVtxNrm);
}

/* 802F4BB8-802F4CD8       .text deformVtxNrm_S16__13J3DSkinDeformCFP8J3DModel */
void J3DSkinDeform::deformVtxNrm_S16(J3DModel* model) const {
    /* Nonmatching */
    J3DGQRSetup7(model->getModelData()->getVertexData().getVtxNrmFrac(), 7, model->getModelData()->getVertexData().getVtxNrmFrac(), 7);
    model->getVertexBuffer()->swapTransformedVtxNrm();

    s32 vtxNum = model->getModelData()->getNrmNum();
    SVec* curVtxNrm = (SVec*)model->getCurrentVtxNrm();
    SVec* transformedVtxNrm = (SVec*)model->getVertexBuffer()->getTransformedVtxNrm(0);

    for (s32 i = 0; i < vtxNum; i++) {
        J3DPSMulMtxVec(mNrmMtx[mNrmUseMtx[i]], &curVtxNrm[i], &transformedVtxNrm[i]);
    }

    DCStoreRange(model->getVertexBuffer()->getTransformedVtxNrm(0), model->getModelData()->getNrmNum() * sizeof(SVec));
    model->setCurrentVtxNrm(transformedVtxNrm);
}

/* 802F4CD8-802F4D78       .text deform__13J3DSkinDeformFP8J3DModel */
void J3DSkinDeform::deform(J3DModel* model) {
    if (model->checkFlag(J3DMdlFlag_SkinPosCpu)) {
        if (model->getModelData()->getVertexData().getVtxPosType() == GX_F32)
            deformVtxPos_F32(model);
        else
            deformVtxPos_S16(model);
    }

    if (model->checkFlag(J3DMdlFlag_SkinNrmCpu)) {
        calcNrmMtx(model);
        if (model->getModelData()->getVertexData().getVtxNrmType() == GX_F32)
            deformVtxNrm_F32(model);
        else
            deformVtxNrm_S16(model);
    }
}