/*
    Fixed point implementation of 3 dimensional vectors.
    SSE2 extension availability is assumed, making it more viable.
    For mul and div we don't use com_fixed functions to make fewer bitshifts.
*/

#ifndef COM_VEC_H
#define COM_VEC_H

#include "def.h"
#include "fix.h"
#include <stdint.h>

typedef struct {
  com_def_alignedas(16) com_def_vector(com_fix_t, a, 3);
} com_vec_t;

static inline com_vec_t com_vec_from(com_fix_t x, com_fix_t y, com_fix_t z) {
  return (com_vec_t){.a = {x, y, z}};
}

static inline com_vec_t com_vec_scalar(com_fix_t s) {
  return (com_vec_t){.a = {s, s, s}};
}

static inline com_vec_t com_vec_identity(void) {
  return (com_vec_t){
      .a = {COM_FIX_FRACUNIT, COM_FIX_FRACUNIT, COM_FIX_FRACUNIT}};
}

static inline com_vec_t com_vec_add(com_vec_t a, com_vec_t b) {
  return (com_vec_t){.a = {com_fix_add(a.a[0], b.a[0]),
                           com_fix_add(a.a[1], b.a[1]),
                           com_fix_add(a.a[2], b.a[2])}};
}

static inline com_vec_t com_vec_sub(com_vec_t a, com_vec_t b) {
  return (com_vec_t){.a = {com_fix_sub(a.a[0], b.a[0]),
                           com_fix_sub(a.a[1], b.a[1]),
                           com_fix_sub(a.a[2], b.a[2])}};
}

static inline com_vec_t com_vec_mul(com_vec_t a, com_vec_t b) {
  return (com_vec_t){.a = {com_fix_mul(a.a[0], b.a[0]),
                           com_fix_mul(a.a[1], b.a[1]),
                           com_fix_mul(a.a[2], b.a[2])}};
}

/* Note: this does not clamp for over/underflow cases */
static inline com_vec_t com_vec_div(com_vec_t a, com_vec_t b) {
  return (com_vec_t){.a = {com_fix_div(a.a[0], b.a[0]),
                           com_fix_div(a.a[1], b.a[1]),
                           com_fix_div(a.a[2], b.a[2])}};
}

/* Scale vector by a fixed point number */
static inline com_vec_t com_vec_scl(com_vec_t a, com_fix_t b) {
  return (com_vec_t){.a = {com_fix_mul(a.a[0], b), com_fix_mul(a.a[1], b),
                           com_fix_mul(a.a[2], b)}};
}

/* Shows how much given vectors are correlated in direction to each other */
/* Resulted range depends on input, it's in -1 to 1 for normalized
 * input and otherwise is -ab to +ab */
static inline com_fix_t com_vec_dot(com_vec_t a, com_vec_t b) {
  return (((int64_t)a.a[0] * b.a[0]) + ((int64_t)a.a[1] * b.a[1]) +
          ((int64_t)a.a[2] * b.a[2])) >>
         COM_FIX_FRACBITS;
}

/* Cross product produces a perpendicular for normalized vectors, or 0 for
 * parallel vectors */
static inline com_vec_t com_vec_crs(com_vec_t a, com_vec_t b) {
  int64_t const cx = ((int64_t)a.a[1] * b.a[2]) - ((int64_t)a.a[2] - b.a[1]);
  int64_t const cy = ((int64_t)a.a[2] * b.a[0]) - ((int64_t)a.a[0] - b.a[2]);
  int64_t const cz = ((int64_t)a.a[0] * b.a[1]) - ((int64_t)a.a[1] - b.a[0]);
  return (com_vec_t){.a = {(com_fix_t)(cx >> COM_FIX_FRACBITS),
                           (com_fix_t)(cy >> COM_FIX_FRACBITS),
                           (com_fix_t)(cz >> COM_FIX_FRACBITS)}};
}

/* Normalize vector, making it a unit one (of length 1). */
/* Pretty expensive, make sure you actually need it. */
static inline com_vec_t com_vec_nrm(com_vec_t a) {
  com_fix_t const n = com_fix_sqrt(com_vec_dot(a, a));
  // return com_vec_scl(a, com_fix_div(COM_FIX_FRACUNIT, n));
  return (com_vec_t){.a = {com_fix_div(a.a[0], n), com_fix_div(a.a[1], n),
                           com_fix_div(a.a[2], n)}};
}

static inline com_fix_t com_vec_min(com_vec_t a) {
  return com_fix_min(com_fix_min(a.a[0], a.a[1]), a.a[2]);
}

static inline com_fix_t com_vec_max(com_vec_t a) {
  return com_fix_max(com_fix_max(a.a[0], a.a[1]), a.a[2]);
}

static inline void com_vec_print(com_vec_t a) {
  com_fix_print(a.a[0]);
  com_fix_print(a.a[1]);
  com_fix_print(a.a[2]);
}

#endif