#include "matrix3x3.h"

// ================= Inversion ================== //

int sp_matrix3x3_invert(SPMatrix3x3* matrix)
{
    const float determinant = sp_matrix3x3_get_determinant(matrix);

    if (-SP_EPSYLON <= determinant && determinant <= SP_EPSYLON) {
        return 0;
    }

    const float r1c1 = matrix->r2c2 * matrix->r3c3 - matrix->r2c3 * matrix->r3c2;
    const float r1c2 = matrix->r1c3 * matrix->r3c2 - matrix->r1c2 * matrix->r3c3;
    const float r1c3 = matrix->r1c2 * matrix->r2c3 - matrix->r1c3 * matrix->r2c2;

    const float r2c1 = matrix->r2c3 * matrix->r3c1 - matrix->r2c1 * matrix->r3c3;
    const float r2c2 = matrix->r1c1 * matrix->r3c3 - matrix->r1c3 * matrix->r3c1;
    const float r2c3 = matrix->r1c3 * matrix->r2c1 - matrix->r1c1 * matrix->r2c3;

    const float r3c1 = matrix->r2c1 * matrix->r3c2 - matrix->r2c2 * matrix->r3c1;
    const float r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2;
    const float r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1;

    matrix->r1c1 = r1c1 / determinant;
    matrix->r1c2 = r1c2 / determinant;
    matrix->r1c3 = r1c3 / determinant;

    matrix->r2c1 = r2c1 / determinant;
    matrix->r2c2 = r2c2 / determinant;
    matrix->r2c3 = r2c3 / determinant;

    matrix->r3c1 = r3c1 / determinant;
    matrix->r3c2 = r3c2 / determinant;
    matrix->r3c3 = r3c3 / determinant;

    return 1;
}

int dp_matrix3x3_invert(DPMatrix3x3* matrix)
{
    const double determinant = dp_matrix3x3_get_determinant(matrix);

    if (-DP_EPSYLON <= determinant && determinant <= DP_EPSYLON) {
        return 0;
    }

    const double r1c1 = matrix->r2c2 * matrix->r3c3 - matrix->r2c3 * matrix->r3c2;
    const double r1c2 = matrix->r1c3 * matrix->r3c2 - matrix->r1c2 * matrix->r3c3;
    const double r1c3 = matrix->r1c2 * matrix->r2c3 - matrix->r1c3 * matrix->r2c2;

    const double r2c1 = matrix->r2c3 * matrix->r3c1 - matrix->r2c1 * matrix->r3c3;
    const double r2c2 = matrix->r1c1 * matrix->r3c3 - matrix->r1c3 * matrix->r3c1;
    const double r2c3 = matrix->r1c3 * matrix->r2c1 - matrix->r1c1 * matrix->r2c3;

    const double r3c1 = matrix->r2c1 * matrix->r3c2 - matrix->r2c2 * matrix->r3c1;
    const double r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2;
    const double r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1;

    matrix->r1c1 = r1c1 / determinant;
    matrix->r1c2 = r1c2 / determinant;
    matrix->r1c3 = r1c3 / determinant;

    matrix->r2c1 = r2c1 / determinant;
    matrix->r2c2 = r2c2 / determinant;
    matrix->r2c3 = r2c3 / determinant;

    matrix->r3c1 = r3c1 / determinant;
    matrix->r3c2 = r3c2 / determinant;
    matrix->r3c3 = r3c3 / determinant;

    return 1;
}

// ================ Make Inverted =============== //

int sp_matrix3x3_make_inverted(const SPMatrix3x3* matrix, SPMatrix3x3* result)
{
    const float determinant = sp_matrix3x3_get_determinant(matrix);

    if (-SP_EPSYLON <= determinant && determinant <= SP_EPSYLON) {
        return 0;
    }

    const float r1c1 = matrix->r2c2 * matrix->r3c3 - matrix->r2c3 * matrix->r3c2;
    const float r1c2 = matrix->r1c3 * matrix->r3c2 - matrix->r1c2 * matrix->r3c3;
    const float r1c3 = matrix->r1c2 * matrix->r2c3 - matrix->r1c3 * matrix->r2c2;

    const float r2c1 = matrix->r2c3 * matrix->r3c1 - matrix->r2c1 * matrix->r3c3;
    const float r2c2 = matrix->r1c1 * matrix->r3c3 - matrix->r1c3 * matrix->r3c1;
    const float r2c3 = matrix->r1c3 * matrix->r2c1 - matrix->r1c1 * matrix->r2c3;

    const float r3c1 = matrix->r2c1 * matrix->r3c2 - matrix->r2c2 * matrix->r3c1;
    const float r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2;
    const float r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1;

    result->r1c1 = r1c1 / determinant;
    result->r1c2 = r1c2 / determinant;
    result->r1c3 = r1c3 / determinant;

    result->r2c1 = r2c1 / determinant;
    result->r2c2 = r2c2 / determinant;
    result->r2c3 = r2c3 / determinant;

    result->r3c1 = r3c1 / determinant;
    result->r3c2 = r3c2 / determinant;
    result->r3c3 = r3c3 / determinant;

    return 1;
}

int dp_matrix3x3_make_inverted(const DPMatrix3x3* matrix, DPMatrix3x3* result)
{
    const double determinant = dp_matrix3x3_get_determinant(matrix);

    if (-DP_EPSYLON <= determinant && determinant <= DP_EPSYLON) {
        return 0;
    }

    const double r1c1 = matrix->r2c2 * matrix->r3c3 - matrix->r2c3 * matrix->r3c2;
    const double r1c2 = matrix->r1c3 * matrix->r3c2 - matrix->r1c2 * matrix->r3c3;
    const double r1c3 = matrix->r1c2 * matrix->r2c3 - matrix->r1c3 * matrix->r2c2;

    const double r2c1 = matrix->r2c3 * matrix->r3c1 - matrix->r2c1 * matrix->r3c3;
    const double r2c2 = matrix->r1c1 * matrix->r3c3 - matrix->r1c3 * matrix->r3c1;
    const double r2c3 = matrix->r1c3 * matrix->r2c1 - matrix->r1c1 * matrix->r2c3;

    const double r3c1 = matrix->r2c1 * matrix->r3c2 - matrix->r2c2 * matrix->r3c1;
    const double r3c2 = matrix->r1c2 * matrix->r3c1 - matrix->r1c1 * matrix->r3c2;
    const double r3c3 = matrix->r1c1 * matrix->r2c2 - matrix->r1c2 * matrix->r2c1;

    result->r1c1 = r1c1 / determinant;
    result->r1c2 = r1c2 / determinant;
    result->r1c3 = r1c3 / determinant;

    result->r2c1 = r2c1 / determinant;
    result->r2c2 = r2c2 / determinant;
    result->r2c3 = r2c3 / determinant;

    result->r3c1 = r3c1 / determinant;
    result->r3c2 = r3c2 / determinant;
    result->r3c3 = r3c3 / determinant;

    return 1;
}

// ============= Weighed Sum of two ============= //

void sp_matrix3x3_get_weighted_sum2(
    const float weight1, const SPMatrix3x3* matrix1,
    const float weight2, const SPMatrix3x3* matrix2,
    SPMatrix3x3* sum
)
{
    sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2;
    sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2;
    sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2;

    sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2;
    sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2;
    sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2;

    sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2;
    sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2;
    sum->r3c3 = matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2;
}

void dp_matrix3x3_get_weighted_sum2(
    const double weight1, const DPMatrix3x3* matrix1,
    const double weight2, const DPMatrix3x3* matrix2,
    DPMatrix3x3* sum
)
{
    sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2;
    sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2;
    sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2;

    sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2;
    sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2;
    sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2;

    sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2;
    sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2;
    sum->r3c3 = matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2;
}

// ============ Weighed Sum of three ============ //

void sp_matrix3x3_get_weighted_sum3(
    const float weight1, const SPMatrix3x3* matrix1,
    const float weight2, const SPMatrix3x3* matrix2,
    const float weight3, const SPMatrix3x3* matrix3,
    SPMatrix3x3* sum
)
{
    sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3;
    sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3;
    sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2 + matrix3->r1c3 * weight3;

    sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3;
    sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3;
    sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2 + matrix3->r2c3 * weight3;

    sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2 + matrix3->r3c1 * weight3;
    sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2 + matrix3->r3c2 * weight3;
    sum->r3c3 = matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2 + matrix3->r3c3 * weight3;
}

void dp_matrix3x3_get_weighted_sum3(
    const double weight1, const DPMatrix3x3* matrix1,
    const double weight2, const DPMatrix3x3* matrix2,
    const double weight3, const DPMatrix3x3* matrix3,
    DPMatrix3x3* sum
)
{
    sum->r1c1 = matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2 + matrix3->r1c1 * weight3;
    sum->r1c2 = matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2 + matrix3->r1c2 * weight3;
    sum->r1c3 = matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2 + matrix3->r1c3 * weight3;

    sum->r2c1 = matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2 + matrix3->r2c1 * weight3;
    sum->r2c2 = matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2 + matrix3->r2c2 * weight3;
    sum->r2c3 = matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2 + matrix3->r2c3 * weight3;

    sum->r3c1 = matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2 + matrix3->r3c1 * weight3;
    sum->r3c2 = matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2 + matrix3->r3c2 * weight3;
    sum->r3c3 = matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2 + matrix3->r3c3 * weight3;
}

// ============ Weighed Sum of four ============= //

void sp_matrix3x3_get_weighted_sum4(
    const float weight1, const SPMatrix3x3* matrix1,
    const float weight2, const SPMatrix3x3* matrix2,
    const float weight3, const SPMatrix3x3* matrix3,
    const float weight4, const SPMatrix3x3* matrix4,
    SPMatrix3x3* sum
)
{
    sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4);
    sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4);
    sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4);

    sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4);
    sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4);
    sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4);

    sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4);
    sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4);
    sum->r3c3 = (matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2) + (matrix3->r3c3 * weight3 + matrix4->r3c3 * weight4);
}

void dp_matrix3x3_get_weighted_sum4(
    const double weight1, const DPMatrix3x3* matrix1,
    const double weight2, const DPMatrix3x3* matrix2,
    const double weight3, const DPMatrix3x3* matrix3,
    const double weight4, const DPMatrix3x3* matrix4,
    DPMatrix3x3* sum
)
{
    sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4);
    sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4);
    sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4);

    sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4);
    sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4);
    sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4);

    sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4);
    sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4);
    sum->r3c3 = (matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2) + (matrix3->r3c3 * weight3 + matrix4->r3c3 * weight4);
}

// ============ Weighed Sum of five ============= //

void sp_matrix3x3_get_weighted_sum5(
    const float weight1, const SPMatrix3x3* matrix1,
    const float weight2, const SPMatrix3x3* matrix2,
    const float weight3, const SPMatrix3x3* matrix3,
    const float weight4, const SPMatrix3x3* matrix4,
    const float weight5, const SPMatrix3x3* matrix5,
    SPMatrix3x3* sum
)
{
    sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5;
    sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5;
    sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4) + matrix5->r1c3 * weight5;

    sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5;
    sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5;
    sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4) + matrix5->r2c3 * weight5;

    sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4) + matrix5->r3c1 * weight5;
    sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4) + matrix5->r3c2 * weight5;
    sum->r3c3 = (matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2) + (matrix3->r3c3 * weight3 + matrix4->r3c3 * weight4) + matrix5->r3c3 * weight5;
}

void dp_matrix3x3_get_weighted_sum5(
    const double weight1, const DPMatrix3x3* matrix1,
    const double weight2, const DPMatrix3x3* matrix2,
    const double weight3, const DPMatrix3x3* matrix3,
    const double weight4, const DPMatrix3x3* matrix4,
    const double weight5, const DPMatrix3x3* matrix5,
    DPMatrix3x3* sum
)
{
    sum->r1c1 = (matrix1->r1c1 * weight1 + matrix2->r1c1 * weight2) + (matrix3->r1c1 * weight3 + matrix4->r1c1 * weight4) + matrix5->r1c1 * weight5;
    sum->r1c2 = (matrix1->r1c2 * weight1 + matrix2->r1c2 * weight2) + (matrix3->r1c2 * weight3 + matrix4->r1c2 * weight4) + matrix5->r1c2 * weight5;
    sum->r1c3 = (matrix1->r1c3 * weight1 + matrix2->r1c3 * weight2) + (matrix3->r1c3 * weight3 + matrix4->r1c3 * weight4) + matrix5->r1c3 * weight5;

    sum->r2c1 = (matrix1->r2c1 * weight1 + matrix2->r2c1 * weight2) + (matrix3->r2c1 * weight3 + matrix4->r2c1 * weight4) + matrix5->r2c1 * weight5;
    sum->r2c2 = (matrix1->r2c2 * weight1 + matrix2->r2c2 * weight2) + (matrix3->r2c2 * weight3 + matrix4->r2c2 * weight4) + matrix5->r2c2 * weight5;
    sum->r2c3 = (matrix1->r2c3 * weight1 + matrix2->r2c3 * weight2) + (matrix3->r2c3 * weight3 + matrix4->r2c3 * weight4) + matrix5->r2c3 * weight5;

    sum->r3c1 = (matrix1->r3c1 * weight1 + matrix2->r3c1 * weight2) + (matrix3->r3c1 * weight3 + matrix4->r3c1 * weight4) + matrix5->r3c1 * weight5;
    sum->r3c2 = (matrix1->r3c2 * weight1 + matrix2->r3c2 * weight2) + (matrix3->r3c2 * weight3 + matrix4->r3c2 * weight4) + matrix5->r3c2 * weight5;
    sum->r3c3 = (matrix1->r3c3 * weight1 + matrix2->r3c3 * weight2) + (matrix3->r3c3 * weight3 + matrix4->r3c3 * weight4) + matrix5->r3c3 * weight5;
}