//
// Generated By: dol2asm
// Translation Unit: d/d_npc_lib
//

#include "d/d_npc_lib.h"
#include "dol2asm.h"
#include "SSystem/SComponent/c_math.h"

//
// Forward References:
//

extern "C" void __ct__16dNpcLib_lookat_cFv();
extern "C" void init__16dNpcLib_lookat_cFP8J3DModelPiP5csXyzP5csXyz();
extern "C" void action__16dNpcLib_lookat_cF4cXyz4cXyzP10fopAc_ac_cPA4_fi();
extern "C" void dbView__16dNpcLib_lookat_cFv();
extern "C" void setPrm__16dNpcLib_lookat_cFv();
extern "C" void update__16dNpcLib_lookat_cFv();
extern "C" void limitter__16dNpcLib_lookat_cFPssss();
extern "C" void __dt__16dNpcLib_lookat_cFv();

//
// External References:
//

extern "C" void __dt__4cXyzFv();
extern "C" void transS__14mDoMtx_stack_cFRC4cXyz();
extern "C" void XYZrotS__14mDoMtx_stack_cFRC5csXyz();
extern "C" void ZXYrotM__14mDoMtx_stack_cFRC5csXyz();
extern "C" void __ct__4cXyzFv();
extern "C" void __dt__5csXyzFv();
extern "C" void __ct__5csXyzFv();
extern "C" void __mi__4cXyzCFRC3Vec();
extern "C" void normalize__4cXyzFv();
extern "C" void isZero__4cXyzCFv();
extern "C" void __apl__5csXyzFR5csXyz();
extern "C" void cM_atan2s__Fff();
extern "C" void __dl__FPv();
extern "C" void __destroy_arr();
extern "C" void __construct_array();
extern "C" void _savegpr_22();
extern "C" void _savegpr_23();
extern "C" void _savegpr_25();
extern "C" void _restgpr_22();
extern "C" void _restgpr_23();
extern "C" void _restgpr_25();
extern "C" u8 now__14mDoMtx_stack_c[48];
extern "C" u8 Zero__5csXyz[4];
extern "C" extern u8 data_80451164[4];

//
// Declarations:
//

/* 80251314-8025140C 24BC54 00F8+00 0/0 0/0 4/4 .text            __ct__16dNpcLib_lookat_cFv */
dNpcLib_lookat_c::dNpcLib_lookat_c() {}

/* 8025140C-80251534 24BD4C 0128+00 0/0 0/0 2/2 .text
 * init__16dNpcLib_lookat_cFP8J3DModelPiP5csXyzP5csXyz          */
void dNpcLib_lookat_c::init(J3DModel* i_mdl_p, int* param_1, csXyz* param_2, csXyz* param_3) {
    int i;
    for (i = 0; i < 4; i++) {
        field_0xbc[i] = -1;
    }

    mCount = 0;
    for (i = 0; i < 3; i++) {
        int a_jntNum = param_1[i];
        if (a_jntNum < 0) {
            break;
        }

        mDoMtx_stack_c::copy(i_mdl_p->getAnmMtx(a_jntNum));
        mDoMtx_stack_c::multVecZero(&field_0x04[mCount]);
        field_0xbc[i] = a_jntNum;
        field_0x94[i] = param_2[i];
        field_0x7c[i] = param_3[i];
        mCount++;
    }
}

/* ############################################################################################## */
/* 80454DE8-80454DEC 0033E8 0004+00 3/3 0/0 0/0 .sdata2          @3864 */
SECTION_SDATA2 static u8 lit_3864[4] = {
    0x00,
    0x00,
    0x00,
    0x00,
};

/* 80454DEC-80454DF0 0033EC 0004+00 1/1 0/0 0/0 .sdata2          @3865 */
SECTION_SDATA2 static f32 lit_3865 = 1.0f;

/* 80454DF0-80454DF8 0033F0 0008+00 2/2 0/0 0/0 .sdata2          @3866 */
SECTION_SDATA2 static f64 lit_3866 = 0.5;

/* 80454DF8-80454E00 0033F8 0008+00 2/2 0/0 0/0 .sdata2          @3867 */
SECTION_SDATA2 static f64 lit_3867 = 3.0;

/* 80454E00-80454E08 003400 0008+00 2/2 0/0 0/0 .sdata2          @3868 */
SECTION_SDATA2 static u8 lit_3868[8] = {
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
};

/* 80454E08-80454E10 003408 0004+04 1/1 0/0 0/0 .sdata2          @3869 */
SECTION_SDATA2 static f32 lit_3869[1 + 1 /* padding */] = {
    0.5f,
    /* padding */
    0.0f,
};

/* 80454E10-80454E18 003410 0008+00 2/2 0/0 0/0 .sdata2          @3873 */
SECTION_SDATA2 static f64 lit_3873 = 4503601774854144.0 /* cast s32 to float */;

/* 80251534-80251B60 24BE74 062C+00 0/0 0/0 2/2 .text
 * action__16dNpcLib_lookat_cF4cXyz4cXyzP10fopAc_ac_cPA4_fi     */
// NONMATCHING - regswap, equivalent
void dNpcLib_lookat_c::action(cXyz param_0, cXyz param_1, fopAc_ac_c* param_2, Mtx param_3,
                              int param_4) {
    cXyz spA0;

    Mtx sp90;
    cMtx_copy(param_3, sp90);
    sp90[2][3] = 0.0f;
    sp90[1][3] = 0.0f;
    sp90[0][3] = 0.0f;

    mDoMtx_stack_c::copy(sp90);
    mDoMtx_stack_c::inverse();
    cMtx_copy(mDoMtx_stack_c::get(), sp90);

    for (int i = 0; i < mCount; i++) {
        spA0 = field_0x04[i] - param_2->current.pos;
        mDoMtx_stack_c::transS(param_2->current.pos);
        mDoMtx_stack_c::concat(sp90);
        mDoMtx_stack_c::multVec(&spA0, &field_0x04[i]);
    }

    spA0 = param_0 - param_2->current.pos;
    mDoMtx_stack_c::transS(param_2->current.pos);
    mDoMtx_stack_c::concat(sp90);
    mDoMtx_stack_c::multVec(&spA0, &param_0);

    spA0 = param_1 - param_2->current.pos;
    mDoMtx_stack_c::transS(param_2->current.pos);
    mDoMtx_stack_c::concat(sp90);
    mDoMtx_stack_c::multVec(&spA0, &field_0x04[mCount]);

    setPrm();
    update();

    cXyz spAC;
    cXyz spB8;

    f32 tmp = 1.0f;
    for (int i = mCount - 1; i >= 0 && param_4 == 1; i--) {
        spAC = param_0 - field_0x04[i];

        if (!spAC.isZero()) {
            spAC.normalize();

            spB8 = field_0x04[mCount] - field_0x04[i];
            if (!spB8.isZero()) {
                spB8.normalize();

                s16 svar7 = -cM_atan2s(spAC.y, spAC.absXZ());
                s16 svar8 = cM_atan2s(spAC.x, spAC.z);

                s16 svar9 = -cM_atan2s(spB8.y, spB8.absXZ());
                s16 svar10 = cM_atan2s(spB8.x, spB8.z);

                field_0x4c[i].x += (s16)(tmp * (f32)(svar7 - svar9));
                field_0x4c[i].y += (s16)(tmp * (f32)(svar8 - svar10));

                limitter(&field_0x4c[i].x, field_0x34[i].x, field_0x94[i].x, field_0x7c[i].x);
                limitter(&field_0x4c[i].y, field_0x34[i].y, field_0x94[i].y, field_0x7c[i].y);
            }
        }

        tmp *= 0.5f;
        update();
    }

    csXyz sp114(csXyz::Zero);
    for (int i = 0; i < 4; i++) {
        sp114 += field_0x4c[i];
        field_0x64[i].x = sp114.x;
        field_0x64[i].y = sp114.y;
        field_0x64[i].z = 0;
    }
}

/* 80251B60-80251B64 24C4A0 0004+00 0/0 0/0 2/2 .text            dbView__16dNpcLib_lookat_cFv */
void dNpcLib_lookat_c::dbView() {}

/* ############################################################################################## */
/* 80454E18-80454E20 003418 0004+04 1/1 0/0 0/0 .sdata2          @3970 */
SECTION_SDATA2 static f32 lit_3970[1 + 1 /* padding */] = {
    2.0f / 5.0f,
    /* padding */
    0.0f,
};

/* 80251B64-80251EF8 24C4A4 0394+00 1/1 0/0 0/0 .text            setPrm__16dNpcLib_lookat_cFv */
void dNpcLib_lookat_c::setPrm() {
    // NONMATCHING
}

/* 80251EF8-80252018 24C838 0120+00 1/1 0/0 0/0 .text            update__16dNpcLib_lookat_cFv */
void dNpcLib_lookat_c::update() {
    cXyz sp50;
    csXyz sp58(csXyz::Zero);
    Mtx m;

    for (int i = 0; i < mCount; i++) {
        mDoMtx_stack_c::XYZrotS(field_0x34[i]);
        cMtx_copy(mDoMtx_stack_c::get(), m);

        mDoMtx_stack_c::transS(field_0x04[i]);
        sp58.x += field_0x4c[i].x;
        sp58.y += field_0x4c[i].y;

        mDoMtx_stack_c::ZXYrotM(sp58);
        mDoMtx_stack_c::concat(m);

        sp50.set(0.0f, 0.0f, field_0xac[i]);
        mDoMtx_stack_c::multVec(&sp50, &field_0x04[i + 1]);
    }
}


/* 80252018-80252094 24C958 007C+00 1/1 0/0 0/0 .text   limitter__16dNpcLib_lookat_cFPssss */
int dNpcLib_lookat_c::limitter(s16* o_value, s16 param_1, s16 param_2, s16 param_3) {
    int limit = param_1 + *o_value;
    if (param_2 <= limit) {
        if (param_2 <= param_1) {
            *o_value = 0;
        } else {
            *o_value -= (s16)(limit - param_2);
        }
    }

    if (limit <= param_3) {
        if (param_1 <= param_3) {
            *o_value = 0;
        } else {
            *o_value -= (s16)(limit - param_3);
        }
    }

    return 1;
}