#include "matrix.h"
#include <stdio.h>

void
print_matrix(float m[9])
{
        printf(" [%+.2f  %+.2f  %+.2f] \n", m[0], m[1], m[2]);
        printf(" [%+.2f  %+.2f  %+.2f] \n", m[3], m[4], m[5]);
        printf(" [%+.2f  %+.2f  %+.2f] \n\n", m[6], m[7], m[8]);
}

void
multiply_matrices(const float a[9], const float b[9], float out[9])
{
        // zero out target matrix
        for (int i = 0; i < 9; i++) {
                out[i] = 0;
        }

        for (int i = 0; i < 3; i++) {
                for (int j = 0; j < 3; j++) {
                        for (int k = 0; k < 3; k++) {
                                out[i * 3 + j] += a[i * 3 + k] * b[k * 3 + j];
                        }
                }
        }
}

void
invert_matrix(const float in[9], float out[9])
{
        float det = in[0] * (in[4] * in[8] - in[5] * in[7]) -
                    in[1] * (in[3] * in[8] - in[5] * in[6]) +
                    in[2] * (in[3] * in[7] - in[4] * in[7]);
        out[0] = (in[4] * in[8] - in[7] * in[5]) / det;
        out[1] = (in[7] * in[2] - in[1] * in[8]) / det;
        out[2] = (in[1] * in[5] - in[4] * in[2]) / det;
        out[3] = (in[6] * in[5] - in[3] * in[8]) / det;
        out[4] = (in[0] * in[8] - in[6] * in[5]) / det;
        out[5] = (in[3] * in[2] - in[0] * in[5]) / det;
        out[6] = (in[3] * in[7] - in[6] * in[4]) / det;
        out[7] = (in[6] * in[1] - in[0] * in[7]) / det;
        out[8] = (in[0] * in[4] - in[3] * in[1]) / det;
}

void
transpose_matrix(const float in[9], float out[9])
{
        for (int i = 0; i < 3; ++i)
                for (int j = 0; j < 3; ++j)
                        out[i + j * 3] = in[j + i * 3];
}