/*
    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;
}

/* TODO: move to .c file */
/* Produces a view matrix needed for camera work. */
static inline com_mat_t com_mat_look_at(com_vec_t pos, com_vec_t up,
                                        com_vec_t target) {
  com_vec_t const r = com_vec_nrm(com_vec_crs(target, up));
  com_vec_t const u = com_vec_crs(r, target);

  com_mat_t result;

  result.a[0] = r.s.x;
  result.a[1] = u.s.x;
  result.a[2] = -target.s.x;
  result.a[3] = 0;
  result.a[4] = r.s.y;
  result.a[5] = u.s.y;
  result.a[6] = -target.s.y;
  result.a[7] = 0;
  result.a[8] = r.s.z;
  result.a[9] = u.s.z;
  result.a[10] = -target.s.z;
  result.a[11] = 0;
  result.a[12] = -com_vec_dot(r, pos);
  result.a[13] = -com_vec_dot(u, pos);
  result.a[14] = com_vec_dot(target, pos);
  result.a[15] = COM_FIXED_FRACUNIT;

  return result;
}

/* TODO: move to .c file */
/* Produces a projection matrix needed for camera work. */
static inline com_mat_t com_mat_perspective(uint16_t rwidth, uint16_t rheight,
                                            com_fixed_t nearz, com_fixed_t farz,
                                            com_fixed_t fov) {
  com_mat_t result = {0};

  // com_fixed_t const aspect = com_fixed_div(rwidth, rheight);

  // const float f = 1.0f / tanf(camera->fov * 0.5f);
  // const float fn = 1.0f / (CAMERA_NEAR_Z - camera->far_z);

  // result.row[0].x = f / aspect;
  // result.row[1].y = f;
  // result.row[2].z = (CAMERA_NEAR_Z + camera->far_z) * fn;
  // result.row[2].w = -1.0f;
  // result.row[3].z = 2.0f * CAMERA_NEAR_Z * camera->far_z * fn;

  // return result;
}

#endif