//usr/bin/cc -Wall -Wextra -Wpedantic matrix.c -o matrix && ./matrix; exit
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#define EPSILON 1E-6

#define OK 0
#define NO_INVERSE 1

typedef float matrix3[9];
typedef float matrix4[16];

#define PRINT_MATRIX(matrix, size) { \
  uint8_t __i = 0; \
  while (__i < size * size) { \
    printf("%f ", matrix[__i]); \
    if (++__i % size == 0) printf("\n"); \
  } \
}

//     a b c
// det(d e f) = a * (ei - hf) - b * (di - gf) + c * (dh - ge)
//     g h i
float determinant3(matrix3 matrix) {
  return matrix[0] * (matrix[4] * matrix[8] - matrix[7] * matrix[5])
       - matrix[1] * (matrix[3] * matrix[8] - matrix[6] * matrix[5])
       + matrix[2] * (matrix[3] * matrix[7] - matrix[6] * matrix[4])
       ;
}

/*
 * To compute an inverse:
 * 1. First check the determinant is not zero
 * 2. Compute the cofactor matrix
 * 3. Transpose the cofactor matrix to get the adjugate matrix
 * 4. Divide the adjugate matrix by the determinant
 */
uint8_t inverse3(matrix3 matrix) {
  // Compute determinant to check if matrix is invertible
  float det = determinant3(matrix);
  if (abs(det) < EPSILON) return NO_INVERSE;
  // Compute adjugate matrix (transpose of the cofactors)
  matrix3 adjugate = { 0, 0, 0, 0, 0, 0, 0, 0, 0 };
  adjugate[0] =  (matrix[4] * matrix[8] - matrix[7] * matrix[5]);
  adjugate[3] = -(matrix[3] * matrix[8] - matrix[6] * matrix[5]);
  adjugate[6] =  (matrix[3] * matrix[7] - matrix[6] * matrix[4]);
  adjugate[1] = -(matrix[1] * matrix[8] - matrix[7] * matrix[2]);
  adjugate[4] =  (matrix[0] * matrix[8] - matrix[6] * matrix[2]);
  adjugate[7] = -(matrix[0] * matrix[7] - matrix[6] * matrix[1]);
  adjugate[2] =  (matrix[1] * matrix[5] - matrix[4] * matrix[2]);
  adjugate[5] = -(matrix[0] * matrix[5] - matrix[3] * matrix[2]);
  adjugate[8] =  (matrix[0] * matrix[4] - matrix[3] * matrix[1]);
  // Divide the cofactor matrix by the dererminant
  float idet = 1 / det;
  matrix[0] = adjugate[0] * idet;
  matrix[1] = adjugate[1] * idet;
  matrix[2] = adjugate[2] * idet;
  matrix[3] = adjugate[3] * idet;
  matrix[4] = adjugate[4] * idet;
  matrix[5] = adjugate[5] * idet;
  matrix[6] = adjugate[6] * idet;
  matrix[7] = adjugate[7] * idet;
  matrix[8] = adjugate[8] * idet;

  return OK;
}

void multiplication3(matrix3 lhs, matrix3 rhs) {
  matrix3 result = { 0, 0, 0, 0, 0, 0, 0, 0, 0 };
  result[0] = lhs[0] * rhs[0] + lhs[1] * rhs[3] + lhs[2] * rhs[6];
  result[1] = lhs[0] * rhs[1] + lhs[1] * rhs[4] + lhs[2] * rhs[7];
  result[2] = lhs[0] * rhs[2] + lhs[1] * rhs[5] + lhs[2] * rhs[8];
  result[3] = lhs[3] * rhs[0] + lhs[4] * rhs[3] + lhs[5] * rhs[6];
  result[4] = lhs[3] * rhs[1] + lhs[4] * rhs[4] + lhs[5] * rhs[7];
  result[5] = lhs[3] * rhs[2] + lhs[4] * rhs[5] + lhs[5] * rhs[8];
  result[6] = lhs[6] * rhs[0] + lhs[7] * rhs[3] + lhs[8] * rhs[6];
  result[7] = lhs[6] * rhs[1] + lhs[7] * rhs[4] + lhs[8] * rhs[7];
  result[8] = lhs[6] * rhs[2] + lhs[7] * rhs[5] + lhs[8] * rhs[8];
  memcpy(&lhs[0], &result[0], sizeof(matrix3));
}

/*
 *     a b c d
 *     e f g h
 * det(i j k l) =   a * (f * (kp - ol) - g * (jp - nl) + h * (jo - nk))
 *     m n o p    - b * (e * (kp - ol) - g * (ip - ml) + h * (io - mk))
 *                + c * (e * (jp - nl) - f * (ip - ml) + h * (in - mj))
 *                - d * (e * (jo - nk) - f * (io - mk) + g * (in - mj))
 */
float determinant4(matrix4 m) {
  return
      m[0] * (m[5] * (m[10] * m[15] - m[14] * m[11]) - m[6] * (m[9] * m[15] - m[13] * m[11]) + m[7] * (m[9] * m[14] - m[13] * m[10]))
    - m[1] * (m[4] * (m[10] * m[15] - m[14] * m[11]) - m[6] * (m[8] * m[15] - m[12] * m[11]) + m[7] * (m[8] * m[14] - m[12] * m[10]))
    + m[2] * (m[4] * ( m[9] * m[15] - m[13] * m[11]) - m[5] * (m[8] * m[15] - m[12] * m[11]) + m[7] * (m[8] * m[13] - m[12] *  m[9]))
    - m[3] * (m[4] * ( m[9] * m[14] - m[13] * m[10]) - m[5] * (m[8] * m[14] - m[12] * m[10]) + m[6] * (m[8] * m[13] - m[12] *  m[9]))
    ;
}

uint8_t inverse4(matrix4 m) {
  // Compute determinant to check if matrix is invertible
  float det = determinant4(m);
  if (abs(det) < EPSILON) return NO_INVERSE;
  // Compute adjugate matrix (transpose of the cofactors)
  //          a b c d     A11 A12 A13 A14
  // cofactor(e f g h) = (A21 A22 A23 A24)
  //          i j k l     A31 A32 A33 A34
  //          m n o p     A41 A42 A43 A44
  //
  //   A11 =  a * (f * (k * p - o * l) - g * (j * p - n * l) + h * (j * o - n * k))
  //   A12 = -b * (e * (k * p - o * l) - g * (i * p - m * l) + h * (i * o - m * k))
  //   A13 =  c * (e * (j * p - n * l) - f * (i * p - m * l) + h * (i * n - m * j))
  //   A14 = -d * (e * (j * o - n * k) - f * (i * o - m * k) + g * (i * n - m * j))
  //   A21 =  e * (b * (k * p - o * l) - c * (j * p - n * l) + d * (j * o - n * k))
  //   A22 = -f * (a * (k * p - o * l) - c * (i * p - m * l) + d * (i * o - m * k))
  //   A23 =  g * (a * (j * p - n * l) - b * (i * p - m * l) + d * (i * n - m * j))
  //   A24 = -h * (a * (j * o - n * k) - b * (i * o - m * k) + c * (i * n - m * j))
  //   A31 =  i * (b * (g * p - o * h) - c * (f * p - n * h) + d * (f * o - n * g))
  //   A32 = -j * (a * (g * p - o * h) - c * (e * p - m * h) + d * (e * o - m * g))
  //   A33 =  k * (a * (f * p - n * h) - b * (e * p - m * h) + d * (e * n - m * f))
  //   A34 = -l * (a * (f * o - n * g) - b * (e * o - m * g) + c * (e * n - m * f))
  //   A41 =  m * (b * (g * l - k * h) - c * (f * l - j * h) + d * (f * k - j * g))
  //   A42 = -n * (a * (g * l - k * h) - c * (e * l - i * h) + d * (e * k - i * g))
  //   A43 =  o * (a * (f * l - j * h) - b * (e * l - i * h) + d * (e * j - i * f))
  //   A44 = -p * (a * (f * k - g * j) - b * (e * k - i * g) + c * (e * j - i * f))
  matrix4 adjugate = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
  adjugate[0]  =  (m[5] * (m[10] * m[15] - m[14] * m[11]) - m[6] * (m[9] * m[15] - m[13] * m[11]) + m[7] * (m[9] * m[14] - m[13] * m[10]));
  adjugate[4]  = -(m[4] * (m[10] * m[15] - m[14] * m[11]) - m[6] * (m[8] * m[15] - m[12] * m[11]) + m[7] * (m[8] * m[14] - m[12] * m[10]));
  adjugate[8]  =  (m[4] * ( m[9] * m[15] - m[13] * m[11]) - m[5] * (m[8] * m[15] - m[12] * m[11]) + m[7] * (m[8] * m[13] - m[12] *  m[9]));
  adjugate[12] = -(m[4] * ( m[9] * m[14] - m[13] * m[10]) - m[5] * (m[8] * m[14] - m[12] * m[10]) + m[6] * (m[8] * m[13] - m[12] *  m[9]));
  adjugate[1]  = -(m[1] * (m[10] * m[15] - m[14] * m[11]) - m[2] * (m[9] * m[15] - m[13] * m[11]) + m[3] * (m[9] * m[14] - m[13] * m[10]));
  adjugate[5]  =  (m[0] * (m[10] * m[15] - m[14] * m[11]) - m[2] * (m[8] * m[15] - m[12] * m[11]) + m[3] * (m[8] * m[14] - m[12] * m[10]));
  adjugate[9]  = -(m[0] * ( m[9] * m[15] - m[13] * m[11]) - m[1] * (m[8] * m[15] - m[12] * m[11]) + m[3] * (m[8] * m[13] - m[12] *  m[9]));
  adjugate[13] =  (m[0] * ( m[9] * m[14] - m[13] * m[10]) - m[1] * (m[8] * m[14] - m[12] * m[10]) + m[2] * (m[8] * m[13] - m[12] *  m[9]));
  adjugate[2]  =  (m[1] * ( m[6] * m[15] - m[14] *  m[7]) - m[2] * (m[5] * m[15] - m[13] *  m[7]) + m[3] * (m[5] * m[14] - m[13] *  m[6]));
  adjugate[6]  = -(m[0] * ( m[6] * m[15] - m[14] *  m[7]) - m[2] * (m[4] * m[15] - m[12] *  m[7]) + m[3] * (m[4] * m[14] - m[12] *  m[6]));
  adjugate[10] =  (m[0] * ( m[5] * m[15] - m[13] *  m[7]) - m[1] * (m[4] * m[15] - m[12] *  m[7]) + m[3] * (m[4] * m[13] - m[12] *  m[5]));
  adjugate[14] = -(m[0] * ( m[5] * m[14] - m[13] *  m[6]) - m[1] * (m[4] * m[14] - m[12] *  m[6]) + m[2] * (m[4] * m[13] - m[12] *  m[5]));
  adjugate[3]  = -(m[1] * ( m[6] * m[11] - m[10] *  m[7]) - m[2] * (m[5] * m[11] -  m[9] *  m[7]) + m[3] * (m[5] * m[10] -  m[9] *  m[6]));
  adjugate[7]  =  (m[0] * ( m[6] * m[11] - m[10] *  m[7]) - m[2] * (m[4] * m[11] -  m[8] *  m[7]) + m[3] * (m[4] * m[10] -  m[8] *  m[6]));
  adjugate[11] = -(m[0] * ( m[5] * m[11] -  m[9] *  m[7]) - m[1] * (m[4] * m[11] -  m[8] *  m[7]) + m[3] * (m[4] *  m[9] -  m[8] *  m[5]));
  adjugate[15] =  (m[0] * ( m[5] * m[10] -  m[6] *  m[9]) - m[1] * (m[4] * m[10] -  m[8] *  m[6]) + m[2] * (m[4] *  m[9] -  m[8] *  m[5]));
  // Divide the cofactor matrix by the dererminant
  float idet = 1 / det;
  m[0] = adjugate[0] * idet;
  m[1] = adjugate[1] * idet;
  m[2] = adjugate[2] * idet;
  m[3] = adjugate[3] * idet;
  m[4] = adjugate[4] * idet;
  m[5] = adjugate[5] * idet;
  m[6] = adjugate[6] * idet;
  m[7] = adjugate[7] * idet;
  m[8] = adjugate[8] * idet;
  m[9] = adjugate[9] * idet;
  m[10] = adjugate[10] * idet;
  m[11] = adjugate[11] * idet;
  m[12] = adjugate[12] * idet;
  m[13] = adjugate[13] * idet;
  m[14] = adjugate[14] * idet;
  m[15] = adjugate[15] * idet;

  return OK;
}

void multiplication4(matrix4 lhs, matrix4 rhs) {
  matrix4 result = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
   result[0] =  lhs[0] * rhs[0] +  lhs[1] * rhs[4] +  lhs[2] *  rhs[8] +  lhs[3] * rhs[12];
   result[1] =  lhs[0] * rhs[1] +  lhs[1] * rhs[5] +  lhs[2] *  rhs[9] +  lhs[3] * rhs[13];
   result[2] =  lhs[0] * rhs[2] +  lhs[1] * rhs[6] +  lhs[2] * rhs[10] +  lhs[3] * rhs[14];
   result[3] =  lhs[0] * rhs[3] +  lhs[1] * rhs[7] +  lhs[2] * rhs[11] +  lhs[3] * rhs[15];
   result[4] =  lhs[4] * rhs[0] +  lhs[5] * rhs[4] +  lhs[6] *  rhs[8] +  lhs[7] * rhs[12];
   result[5] =  lhs[4] * rhs[1] +  lhs[5] * rhs[5] +  lhs[6] *  rhs[9] +  lhs[7] * rhs[13];
   result[6] =  lhs[4] * rhs[2] +  lhs[5] * rhs[6] +  lhs[6] * rhs[10] +  lhs[7] * rhs[14];
   result[7] =  lhs[4] * rhs[3] +  lhs[5] * rhs[7] +  lhs[6] * rhs[11] +  lhs[7] * rhs[15];
   result[8] =  lhs[8] * rhs[0] +  lhs[9] * rhs[4] + lhs[10] *  rhs[8] + lhs[11] * rhs[12];
   result[9] =  lhs[8] * rhs[1] +  lhs[9] * rhs[5] + lhs[10] *  rhs[9] + lhs[11] * rhs[13];
  result[10] =  lhs[8] * rhs[2] +  lhs[9] * rhs[6] + lhs[10] * rhs[10] + lhs[11] * rhs[14];
  result[11] =  lhs[8] * rhs[3] +  lhs[9] * rhs[7] + lhs[10] * rhs[11] + lhs[11] * rhs[15];
  result[12] = lhs[12] * rhs[0] + lhs[13] * rhs[4] + lhs[14] *  rhs[8] + lhs[15] * rhs[12];
  result[13] = lhs[12] * rhs[1] + lhs[13] * rhs[5] + lhs[14] *  rhs[9] + lhs[15] * rhs[13];
  result[14] = lhs[12] * rhs[2] + lhs[13] * rhs[6] + lhs[14] * rhs[10] + lhs[15] * rhs[14];
  result[15] = lhs[12] * rhs[3] + lhs[13] * rhs[7] + lhs[14] * rhs[11] + lhs[15] * rhs[15];
  memcpy(&lhs[0], &result[0], sizeof(matrix4));
}


int main(/*int argc, char **argv*/) {
  matrix4 matrix = { 1, 2, 3, 4, 5, 6, 7, 2, 2, 10, 11, 12, 13, 14, 2, 15 };
  matrix4 matrixInverse;
  memcpy(&matrixInverse[0], &matrix[0], sizeof(matrix4));

  if (inverse4(matrixInverse)) {
    fprintf(stderr, "cannot invert matrix\n");
  } else {
    multiplication4(matrixInverse, matrix);
    PRINT_MATRIX(matrixInverse, 4);
  }

  return 0;
}