/*---------------------------------------------------------------------------*
  Project:  Horizon
  File:     math_Matrix33.ipp

  Copyright (C)2009-2010 Nintendo Co., Ltd.  All rights reserved.

  These coded instructions, statements, and computer programs contain
  proprietary information of Nintendo of America Inc. and/or Nintendo
  Company Ltd., and are protected by Federal copyright law.  They may
  not be disclosed to third parties or copied or duplicated in any form,
  in whole or in part, without the prior written consent of Nintendo.

  $Revision: 13623 $
 *---------------------------------------------------------------------------*/
#include <nn/math/math_Vector3.h>

namespace nn { 
namespace math {
namespace ARMv6 {

#include <nn/hw/ARM/code32.h>
/*!
    @name    行列
    @{
*/

template<typename TMatrix>
inline asm TMatrix* 
MTX33MultAsm_ORG(TMatrix*, const TMatrix*, const TMatrix*)
{
    VPUSH       {d8-d13}            // レジスタの保存

    //templateの為、いちおう行列のあるオフセット値を計算する...
    MOV         r3,#__cpp(offsetof(TMatrix,f))
    ADD         r1,r1,r3
    ADD         r2,r2,r3
    MOV         r3,#__cpp(TMatrix::COLUMN_COUNT)*4

    VLDMIA      r2,{s9-s11}         // レジスタ[S9-S11]に行列p2の１行目
    VLDMIA      r1,{s0-s2}          // レジスタ[S0-S2]に行列p1の１行目
    ADD         r1,r1,r3
    ADD         r2,r2,r3
    VLDMIA      r2,{s12-s14}        // レジスタ[S12-S14]に行列p2の２行目
    VLDMIA      r1,{s3-s5}          // レジスタ[S3-S5]に行列p1の２行目
    ADD         r1,r1,r3
    ADD         r2,r2,r3
    VLDMIA      r2,{s15-s17}        // レジスタ[S15-S17]に行列p2の３行目
    VLDMIA      r1,{s6-s8}          // レジスタ[S6-S8]に行列p1の３行目

    VMUL.F32    s18,s9,s0
    VMUL.F32    s19,s10,s0
    VMUL.F32    s20,s11,s0

    VMUL.F32    s21,s9,s3
    VMUL.F32    s22,s10,s3
    VMUL.F32    s23,s11,s3

    VMUL.F32    s24,s9,s6
    VMUL.F32    s25,s10,s6
    VMUL.F32    s26,s11,s6

    VMLA.F32    s18,s12,s1
    VMLA.F32    s19,s13,s1
    VMLA.F32    s20,s14,s1

    VMLA.F32    s21,s12,s4
    VMLA.F32    s22,s13,s4
    VMLA.F32    s23,s14,s4

    VMLA.F32    s24,s12,s7
    VMLA.F32    s25,s13,s7
    VMLA.F32    s26,s14,s7

    VMLA.F32    s18,s15,s2
    VMLA.F32    s19,s16,s2
    VMLA.F32    s20,s17,s2

    VMLA.F32    s21,s15,s5
    VMLA.F32    s22,s16,s5
    VMLA.F32    s23,s17,s5

    VMLA.F32    s24,s15,s8
    VMLA.F32    s25,s16,s8
    VMLA.F32    s26,s17,s8

    VSTMIA      r0,{s18-s20}        // 結果をストア
    VPOP        {d8-d9}             // 分割POPした方が速い
    ADD         r1,r0,r3
    VSTMIA      r1,{s21-s23}        // 結果をストア
    VPOP        {d10-d11}           // 細切れにした方が速い
    ADD         r1,r1,r3
    VSTMIA      r1,{s24-s26}        // 結果をストア
    VPOP        {d12-d13}           // ブツ切りした方が速い
    BX          lr                  // 戻る
}

template<typename TMatrix>
inline asm TMatrix* 
MTX33MultAsm(TMatrix*, const TMatrix*, const TMatrix*)
{


    //templateの為、いちおう行列のあるオフセット値を計算する...
    MOV         r3,#__cpp(offsetof(TMatrix,f))
    ADD         r1,r1,r3
    ADD         r2,r2,r3
    MOV         r3,#__cpp(TMatrix::COLUMN_COUNT)*4

    CMP         r3,#3*4
    BNE         LABELX

    VPUSH       {d8}                  // レジスタの保存
    VLDMIA      r2!,{s10-s15}         // レジスタ[ S10-S15]に行列p2の１,２行目

    VLDR.F32    s16,[r1,#3*4*0+4*0]  // 行列p1[0][0]
    VLDR.F32    s17,[r1,#3*4*1+4*0]  // 行列p1[1][0]

    VMUL.F32    s0,s10,s16
    VMUL.F32    s1,s11,s16
    VMUL.F32    s2,s12,s16
    VLDR.F32    s16,[r1,#3*4*2+4*0]  // 行列p1[2][0]

    VMUL.F32    s3,s10,s17
    VMUL.F32    s4,s11,s17
    VMUL.F32    s5,s12,s17
    VLDR.F32    s17,[r1,#3*4*0+4*1]  // 行列p1[0][1]

    VMUL.F32    s6,s10,s16
    VMUL.F32    s7,s11,s16
    VMUL.F32    s8,s12,s16
    VLDR.F32    s16,[r1,#3*4*1+4*1]  // 行列p1[1][1]

    VLDMIA      r2,{s10-s12}         // レジスタ[ S10-S12]に行列p2の３行目
    VMLA.F32    s0,s13,s17
    VMLA.F32    s1,s14,s17
    VMLA.F32    s2,s15,s17
    VLDR.F32    s17,[r1,#3*4*2+4*1]  // 行列p1[2][1]
                
    VMLA.F32    s3,s13,s16
    VMLA.F32    s4,s14,s16
    VMLA.F32    s5,s15,s16
    VLDR.F32    s16,[r1,#3*4*0+4*2]  // 行列p1[0][2]
                
    VMLA.F32    s6,s13,s17
    VMLA.F32    s7,s14,s17
    VMLA.F32    s8,s15,s17
    VLDR.F32    s17,[r1,#3*4*1+4*2]  // 行列p1[1][2]

    VMLA.F32    s0,s10,s16
    VMLA.F32    s1,s11,s16
    VMLA.F32    s2,s12,s16
    VLDR.F32    s16,[r1,#3*4*2+4*2]  // 行列p1[2][2]
                
    VMLA.F32    s3,s10,s17
    VMLA.F32    s4,s11,s17
    VMLA.F32    s5,s12,s17
                
    VMLA.F32    s6,s10,s16
    VMLA.F32    s7,s11,s16
    VMLA.F32    s8,s12,s16

    VPOP        {d8}                // POP

    VSTMIA      r0,{s0-s8}          // 結果をストア
    BX          lr                  // 戻る

LABELX
    VPUSH       {d8-d13}            // レジスタの保存
    VLDMIA      r2,{s9-s11}         // レジスタ[ S9-S11]に行列p2の１行目
    VLDMIA      r1,{s18-s20}        // レジスタ[S18-S20]に行列p1の１行目
    ADD         r1,r1,r3
    ADD         r2,r2,r3
    VLDMIA      r2,{s12-s14}        // レジスタ[S12-S14]に行列p2の２行目
    VLDMIA      r1,{s21-s23}        // レジスタ[S21-S23]に行列p1の２行目
    ADD         r1,r1,r3
    ADD         r2,r2,r3
    VLDMIA      r2,{s15-s17}        // レジスタ[S15-S17]に行列p2の３行目
    VLDMIA      r1,{s24-s26}        // レジスタ[S24-S26]に行列p1の３行目

    VMUL.F32    s0,s9,s18
    VMUL.F32    s1,s10,s18
    VMUL.F32    s2,s11,s18

    VMUL.F32    s3,s9,s21
    VMUL.F32    s4,s10,s21
    VMUL.F32    s5,s11,s21

    VMUL.F32    s6,s9,s24
    VMUL.F32    s7,s10,s24
    VMUL.F32    s8,s11,s24

    VMLA.F32    s0,s12,s19
    VMLA.F32    s1,s13,s19
    VMLA.F32    s2,s14,s19
                
    VMLA.F32    s3,s12,s22
    VMLA.F32    s4,s13,s22
    VMLA.F32    s5,s14,s22
                
    VMLA.F32    s6,s12,s25
    VMLA.F32    s7,s13,s25
    VMLA.F32    s8,s14,s25

    VMLA.F32    s0,s15,s20
    VMLA.F32    s1,s16,s20
    VMLA.F32    s2,s17,s20
                
    VMLA.F32    s3,s15,s23
    VMLA.F32    s4,s16,s23
    VMLA.F32    s5,s17,s23
                
    VMLA.F32    s6,s15,s26
    VMLA.F32    s7,s16,s26
    VMLA.F32    s8,s17,s26

    VPOP        {d8-d13}            // POP

    ADD         r1,r0,r3
    ADD         r2,r1,r3
    VSTMIA      r0,{s0-s2}          // 結果をストア
    VSTMIA      r1,{s3-s5}          // 結果をストア
    VSTMIA      r2,{s6-s8}          // 結果をストア
    BX          lr                  // 戻る

}

/*!
    @}
*/

#include <nn/hw/ARM/codereset.h>
/*!
    @name    ユーティリティ
    @{
*/

/*!--------------------------------------------------------------------------*
  @brief        ベクトルを行列で変換します。

  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。pV と同じベクトルを指していても構いません。
  @param[in]    pM    変換行列へのポインタ。
  @param[in]    pV    元となるベクトルへのポインタ。

  @return       pOut を返します。
 *---------------------------------------------------------------------------*/
NN_MATH_INLINE VEC3*
VEC3TransformC(VEC3* pOut, const MTX33* pM, const VEC3* pV)
{
    NN_NULL_ASSERT(pOut);
    NN_NULL_ASSERT(pM);
    NN_NULL_ASSERT(pV);

    VEC3 vTmp;
    VEC3* pDst = (pOut == pV) ? &vTmp : pOut;
    pDst->x = pM->f._00 * pV->x + pM->f._01 * pV->y + pM->f._02 * pV->z;
    pDst->y = pM->f._10 * pV->x + pM->f._11 * pV->y + pM->f._12 * pV->z;
    pDst->z = pM->f._20 * pV->x + pM->f._21 * pV->y + pM->f._22 * pV->z;

    if (pDst == &vTmp)
    {
        pOut->x = pDst->x;
        pOut->y = pDst->y;
        pOut->z = pDst->z;
    }

    return pOut;
}

/*!
    @}
*/

/* ------------------------------------------------------------------------
        MTX33
   ------------------------------------------------------------------------ */

/*!
    @name    行列
    @{
*/

/*!--------------------------------------------------------------------------*
  @brief        3x3行列の積を計算します。

  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。p1, p2 と同じ行列を指していても構いません。
  @param[in]    p1    左の行列へのポインタ。
  @param[in]    p2    右の行列へのポインタ。

  @return       pOutを返します。
 *---------------------------------------------------------------------------*/
template<typename TMatrix>
inline TMatrix*
MTX33MultC(TMatrix* pOut, const TMatrix* __restrict p1, const TMatrix* __restrict p2)
{
    NN_NULL_ASSERT( p1 );
    NN_NULL_ASSERT( p2 );
    NN_NULL_ASSERT( pOut );

    TMatrix mTmp;
    TMatrix* __restrict pDst = (pOut == p1 || pOut == p2) ? &mTmp : pOut;

    pDst->f._00 = p1->f._00 * p2->f._00 + p1->f._01 * p2->f._10 + p1->f._02 * p2->f._20;
    pDst->f._01 = p1->f._00 * p2->f._01 + p1->f._01 * p2->f._11 + p1->f._02 * p2->f._21;
    pDst->f._02 = p1->f._00 * p2->f._02 + p1->f._01 * p2->f._12 + p1->f._02 * p2->f._22;

    pDst->f._10 = p1->f._10 * p2->f._00 + p1->f._11 * p2->f._10 + p1->f._12 * p2->f._20;
    pDst->f._11 = p1->f._10 * p2->f._01 + p1->f._11 * p2->f._11 + p1->f._12 * p2->f._21;
    pDst->f._12 = p1->f._10 * p2->f._02 + p1->f._11 * p2->f._12 + p1->f._12 * p2->f._22;

    pDst->f._20 = p1->f._20 * p2->f._00 + p1->f._21 * p2->f._10 + p1->f._22 * p2->f._20;
    pDst->f._21 = p1->f._20 * p2->f._01 + p1->f._21 * p2->f._11 + p1->f._22 * p2->f._21;
    pDst->f._22 = p1->f._20 * p2->f._02 + p1->f._21 * p2->f._12 + p1->f._22 * p2->f._22;

    if (pDst == &mTmp)
    {
        pOut->f._00 = pDst->f._00; pOut->f._01 = pDst->f._01; pOut->f._02 = pDst->f._02;
        pOut->f._10 = pDst->f._10; pOut->f._11 = pDst->f._11; pOut->f._12 = pDst->f._12;
        pOut->f._20 = pDst->f._20; pOut->f._21 = pDst->f._21; pOut->f._22 = pDst->f._22;
    }

    return pOut;
}

/*!--------------------------------------------------------------------------*
  @brief        3x3行列をコピーします。

  @param[out]   pOut  コピー先の行列のポインタ
  @param[in]    p     コピー元の行列のポインタ

  @return       pOutを返します。
 *---------------------------------------------------------------------------*/
NN_MATH_INLINE MTX33*
MTX33CopyC(MTX33* pOut, const MTX33* p)
{
    if (pOut != p)
    {
        *pOut = *p;
    }

    return pOut;
}



/*!--------------------------------------------------------------------------*
  @brief        行列を実数倍して、別の行列を足します。

  @param[out]   pOut  計算結果を受け取るバッファへのポインタ。p1, p2 と同じ行列を指していても構いません。
  @param[in]    t     掛ける数。
  @param[in]    p1    元の行列へのポインタ。
  @param[in]    p2    足す行列へのポインタ。

  @return       pOut を返します。
 *---------------------------------------------------------------------------*/
NN_MATH_INLINE MTX33*
MTX33MAddC(MTX33* pOut, f32 t, const MTX33* p1, const MTX33* p2)
{
    pOut->f._00 = t * p1->f._00 + p2->f._00;
    pOut->f._01 = t * p1->f._01 + p2->f._01;
    pOut->f._02 = t * p1->f._02 + p2->f._02;

    pOut->f._10 = t * p1->f._10 + p2->f._10;
    pOut->f._11 = t * p1->f._11 + p2->f._11;
    pOut->f._12 = t * p1->f._12 + p2->f._12;

    pOut->f._20 = t * p1->f._20 + p2->f._20;
    pOut->f._21 = t * p1->f._21 + p2->f._21;
    pOut->f._22 = t * p1->f._22 + p2->f._22;

    return pOut;
}
NN_MATH_INLINE MTX33*
MTX33MAddC_FAST(MTX33* pOut, f32 t, const MTX33* p1, const MTX33* p2)
{
    register f32 m00, m01, m02, m10, m11, m12, m20, m21, m22;

    m00 = p2->f._00;
    m01 = p2->f._01;
    m02 = p2->f._02;
          
    m10 = p2->f._10;
    m11 = p2->f._11;
    m12 = p2->f._12;
          
    m20 = p2->f._20;
    m21 = p2->f._21;
    m22 = p2->f._22;

    m00 += t * p1->f._00;
    m01 += t * p1->f._01;
    m02 += t * p1->f._02;
    
    m10 += t * p1->f._10;
    m11 += t * p1->f._11;
    m12 += t * p1->f._12;
    
    m20 += t * p1->f._20;
    m21 += t * p1->f._21;
    m22 += t * p1->f._22;
    
    pOut->f._00 = m00;
    pOut->f._01 = m01;
    pOut->f._02 = m02;

    pOut->f._10 = m10;
    pOut->f._11 = m11;
    pOut->f._12 = m12;

    pOut->f._20 = m20;
    pOut->f._21 = m21;
    pOut->f._22 = m22;

    return pOut;

}

/*!
    @}
*/
}  // namespace ARMv6
}  // namespace math
}  // namespace nn