/*
    Fixed point implementation of 4 dimensional matrix.
    Some optimizing cases are present, such as reordered matrices and assumed
   identity components.

    Column-major ordering is assumed unless stated otherwise:

    a e k o
    b f l p
    c g m q
    d h n r

    In memory: a b c d e f g h ...
*/

#ifndef COM_MAT_H
#define COM_MAT_H

#include "fixed.h"
#include "vec.h"
#include <stdint.h>
#include <stdio.h>

typedef union {
  com_fixed_t com_def_alignedas(64) a[4 * 4];
} com_mat_t;

static inline com_mat_t com_mat_identity(void) {
  com_mat_t result = {0};

  result.a[0 * 4 + 0] = COM_FIXED_FRACUNIT;
  result.a[1 * 4 + 1] = COM_FIXED_FRACUNIT;
  result.a[2 * 4 + 2] = COM_FIXED_FRACUNIT;
  result.a[3 * 4 + 3] = COM_FIXED_FRACUNIT;

  return result;
}

/* https://michalpitr.substack.com/p/optimizing-matrix-multiplication */
static inline com_mat_t com_mat_mul(com_mat_t a, com_mat_t b) {
  // com_mat_t result = {0};

  // for (int c = 0; c < 4; ++c) {
  //   for (int k = 0; k < 4; ++k) {
  //     for (int r = 0; r < 4; ++r) {
  //       result.a[r + c * 4] += com_fixed_mul(a.a[r + k * 4], b.a[k + c * 4]);
  //     }
  //   }
  // }

  com_mat_t result;

  for (int c = 0; c < 4; ++c) {
    for (int r = 0; r < 4; ++r) {
      result.a[r + c * 4] = (((int64_t)a.a[r + 0 * 4] * b.a[0 + c * 4]) +
                             ((int64_t)a.a[r + 1 * 4] * b.a[1 + c * 4]) +
                             ((int64_t)a.a[r + 2 * 4] * b.a[2 + c * 4]) +
                             ((int64_t)a.a[r + 3 * 4] * b.a[3 + c * 4])) >>
                            COM_FIXED_FRACBITS;
    }
  }

  return result;
}

/* This case might be slightly more optimized, as we can reorder one frequently
 * reused matrix, such as VP.
 */
/* Note: second a matrix is assumed to be row-major, reverse of typical. */
static inline com_mat_t com_mat_mul_reodered(com_mat_t a, com_mat_t b) {
  com_mat_t result;

  for (int c = 0; c < 4; ++c) {
    for (int r = 0; r < 4; ++r) {
      result.a[r + c * 4] = (((int64_t)a.a[0 + r * 4] * b.a[0 + c * 4]) +
                             ((int64_t)a.a[1 + r * 4] * b.a[1 + c * 4]) +
                             ((int64_t)a.a[2 + r * 4] * b.a[2 + c * 4]) +
                             ((int64_t)a.a[3 + r * 4] * b.a[3 + c * 4])) >>
                            COM_FIXED_FRACBITS;
    }
  }

  return result;
}

/* Projects vertex position to a screen via reordered row-major MVP matrix,
 * which implies division by w in-place */
static inline com_vec_t com_mat_vec_project(com_mat_t a, com_vec_t b) {
  com_fixed_t t[4];
  com_vec_t result;

  for (int c = 0; c < 4; ++c) {
    t[c] =
        (((int64_t)a.a[c * 4 + 0] * b.s.x) + ((int64_t)a.a[c * 4 + 1] * b.s.y) +
         ((int64_t)a.a[c * 4 + 2] * b.s.z) + a.a[c * 4 + 3]) >>
        COM_FIXED_FRACBITS;
  }

  /* Creates perspective effect, could be skipped for orthographic */
  result.a[0] = com_fixed_div(t[0], t[3]);
  result.a[1] = com_fixed_div(t[1], t[3]);
  result.a[2] = com_fixed_div(t[2], t[3]);

  return result;
}

/* Slightly optimized case of assumed identity scaling, might be useful for MVP
 * calculations, if model matrix does not scale. View matrix is always
 * unscaled as well.
 */
// static inline com_mat_t com_mat_mul_no_scale(com_mat_t a, com_mat_t b) {
//   com_mat_t result;

//   /* TODO: calc the rest */

//   result.a[0 * 4 + 3] = 0;
//   result.a[1 * 4 + 3] = 0;
//   result.a[2 * 4 + 3] = 0;
//   result.a[3 * 4 + 3] = COM_FIXED_FRACUNIT;

//   return result;
// }

/* Reorder between column and row major, it's also called transposing */
static inline com_mat_t com_mat_reoder(com_mat_t a) {
  com_mat_t result;

  for (int r = 0; r < 4; ++r) {
    for (int c = 0; c < 4; ++c) {
      result.a[c * 4 + r] = a.a[c + r * 4];
    }
  }

  return result;
}

#endif