// 秒寻科技
// 矩阵运算，包括转置、相乘和求逆
// 周涛
// 2023-04-08

#include "../includes/includes.h"

// 转置，就是将原阵的行变成目标阵的列
// pfSourceM:原阵
// pfTargetM:目标阵
// byRow:矩阵行
// byCol:矩阵列
void matrix_transpose(flt32 *pfSourceM, flt32 *pfTargetM, uint8 byRow, uint8 byCol)
{
    flt32 fTmp = 0;

    for (int i = 0; i < byRow; i++)
    {
        for (int j = 0; j < byCol; j++)
        {
            fTmp = pfSourceM[i * byCol + j];
            pfTargetM[j * byRow + i] = fTmp;
        }
    }

    return;
}

// 矩阵相乘，M1*M2 = pfResultM
// pfResultM:结果阵，结果阵为byRow1行和byCol1列
uint8 matrix_multi(flt32 *pfM1, uint8 byRow1, uint8 byCol1, flt32 *pfM2, uint8 byRow2, uint8 byCol2, flt32 *pfResultM)
{
    flt32 fM1Row = 0;
    flt32 fM2Col = 0;

    if (byRow1 != byCol2 || byCol1 != byRow2)
    {
        return 1;
    }

    for (int i = 0; i < byRow1; i++) // 遍历M1的每一行
    {
        for (int j = 0; j < byCol2; j++) // 遍历M2的每一列
        {
            pfResultM[i * byCol2 + j] = 0; // 初始化结果矩阵的当前元素为0

            // 遍历M1的行和M2的列
            for (int k = 0; k < byCol1; k++)
            {
                fM1Row = pfM1[i * byCol1 + k];                // M1的一行
                fM2Col = pfM2[k * byCol2 + j];                // M2的一列
                pfResultM[i * byCol2 + j] += fM1Row * fM2Col; // 结果阵的一个元素
            }
        }
    }

    return 0;
}

// 矩阵求逆
// pfM1:原阵
// pfM2:逆阵
// bySize:矩阵大小
uint8 matrix_inversion(flt32 *pfM1, flt32 *pfM2, uint8 bySize)
{
    flt32 fMainCell = 0;
    uint8 byNonZeroRow = 0;
    flt32 det = 1;
    flt32 fDivisorFactor = 0; // 除数因子
    flt32 fHedgingFactor = 0; // 对消因子

    // 将逆阵初始化成单位矩阵
    matrix_gen_unit_matrix(pfM2, bySize);

    //// 用高斯-约当消元法计算矩阵的逆（就是将原阵通过乘加化为单元阵）
    for (uint8 i = 0; i < bySize; i++) // 遍历原阵一行中的每个元素
    {
        // 寻找主元，确保主元不为0
        fMainCell = pfM1[i * bySize + i];
        if (fMainCell == 0)
        {
            byNonZeroRow = i + 1;
            while (byNonZeroRow < bySize && pfM1[byNonZeroRow * bySize + i] == 0)
            {
                byNonZeroRow++;
            }

            if (byNonZeroRow >= bySize)
            {
                return 1; // 失败，该矩阵无法求逆
            }

            // 交换行，原阵
            matrix_exchange_row(pfM1, bySize, i, byNonZeroRow);
            // 交换行，逆阵
            matrix_exchange_row(pfM2, bySize, i, byNonZeroRow);
        }

        // 将主元化为1
        fDivisorFactor = pfM1[i * bySize + i];
        det *= fDivisorFactor;
        for (uint8 j = 0; j < bySize; j++)
        {
            pfM1[i * bySize + j] /= fDivisorFactor;
            pfM2[i * bySize + j] /= fDivisorFactor;
        }

        matrix_print(pfM1, 4, 4);
        printf("上面是原阵\n");
        matrix_print(pfM2, 4, 4);
        printf("上面是逆阵\n");

        // 将主元(i行)下方的元素置0-消元过程
        for (int k = i + 1; k < bySize; k++)
        {
            fHedgingFactor = pfM1[k * bySize + i]; // 对消因子
            for (int l = 0; l < bySize; l++)
            {
                pfM1[k * bySize + l] -= fHedgingFactor * pfM1[i * bySize + l]; // 原阵对消
                pfM2[k * bySize + l] -= fHedgingFactor * pfM2[i * bySize + l]; // 逆阵重算
            }
        }

        // 将主元(i行)上方的元素置为0（上三角化过程）
        for (int k = 0; k < i; k++) // 注意k的上边界一定要是i
        {
            fHedgingFactor = pfM1[k * bySize + i];
            for (int j = 0; j < bySize; j++)
            {
                pfM1[k * bySize + j] -= fHedgingFactor * pfM1[i * bySize + j]; // 原阵对消
                pfM2[k * bySize + j] -= fHedgingFactor * pfM2[i * bySize + j]; // 逆阵重算
            }
        }
    }

    // 如果行列式为0，则矩阵不可逆
    if (!det)
    {
        return 1;
    }

    return 0;
}

void matrix_gen_unit_matrix(flt32 *pfM, uint8 bySize)
{
    for (uint8 i = 0; i < bySize; i++)
    {
        for (uint8 j = 0; j < bySize; j++)
        {
            if (i == j)
            {
                pfM[i * bySize + j] = 1;
            }
            else
            {
                pfM[i * bySize + j] = 0;
            }
        }
    }

    return;
}

// 交换矩阵中两行的函数（用第二行换第一行）
// pfM:矩阵
// byCol:列
// byRow1:第一行
// byRow2：第二行
void matrix_exchange_row(flt32 *pfM, uint8 byCol, uint8 byRow1, uint8 byRow2)
{
    flt32 fTmp;
    for (int k = 0; k < byCol; k++)
    {
        fTmp = pfM[byRow1 + k];
        pfM[byRow1 + k] = pfM[byRow2 + k];
        pfM[byRow2 + k] = fTmp;
    }

    return;
}

// 打印矩阵
void matrix_print(flt32 *pfM, uint8 byRow, uint8 byCol)
{
    for (int i = 0; i < byRow; i++)
    {
        for (int j = 0; j < byCol; j++)
        {
            printf("%f ", pfM[i * byCol + j]);
        }
        printf("\n");
    }

    return;
}