/*
    Doom-inspired Q15.16 format used for most of everything.
    https://hackmd.io/9uRB9YbTSBW4Qz_2b8TyDw#Examples-2-DOOM
*/

#ifndef COM_FIXED_H
#define COM_FIXED_H

#include <stdint.h>
#include <stdio.h>

#define COM_FIXED_FRACBITS 16
#define COM_FIXED_FRACUNIT (1 << COM_FIXED_FRACBITS)
#define COM_FIXED_FRACHALF (COM_FIXED_FRACUNIT >> 1)
#define COM_FIXED_FRACQRTR (COM_FIXED_FRACHALF >> 1)

#define COM_FIXED_PI (com_fixed_t)205887
#define COM_FIXED_PI2 (com_fixed_t)411774
#define COM_FIXED_PIHALF (com_fixed_t)102943
#define COM_FIXED_PIONEANDAHALF (COM_FIXED_PI + COM_FIXED_PIHALF)

typedef int32_t com_fixed_t;

extern const uint16_t com_fixed_sin_lut[128];

void com_fixed_print(com_fixed_t a);

void com_fixed_run_tests(void);
void com_fixed_run_bench(void);

static inline com_fixed_t com_fixed_add(com_fixed_t a, com_fixed_t b) {
  return a + b;
}

static inline com_fixed_t com_fixed_sub(com_fixed_t a, com_fixed_t b) {
  return a - b;
}

static inline com_fixed_t com_fixed_mul(com_fixed_t a, com_fixed_t b) {
  return (com_fixed_t)(((int64_t)a * (int64_t)b) >> COM_FIXED_FRACBITS);
}

/* Note: this does not clamp for over/underflow cases */
static inline com_fixed_t com_fixed_div(com_fixed_t a, com_fixed_t b) {
  return (com_fixed_t)(((int64_t)a << COM_FIXED_FRACBITS) / (int64_t)b);
}

/* Is supposed to be only used for debugging, not in real code. */
/* Because of that we default to maximum precision here. */
static inline double com_fixed_as_float(com_fixed_t a) {
  return (a >> COM_FIXED_FRACBITS) * (double)1.0 +
         (a & 0xFFFF) * (double)(1.0 / COM_FIXED_FRACUNIT);
}

/* Branchless floor operation. */
/* TODO: Check against simpler branching implementation. */
static inline com_fixed_t com_fixed_floor(com_fixed_t const a) {
  int32_t const sign = a >> 31;
  int32_t const frac_mask = (1 << COM_FIXED_FRACBITS) - 1;
  int32_t const has_fraction = ((a & frac_mask) != 0);
  int32_t const truncated = a & ~frac_mask;
  return truncated - ((sign & has_fraction) << COM_FIXED_FRACBITS);
}

/* Approximated square root by Newton-Raphson in 2 iterations over small LUT. */
com_fixed_t com_fixed_sqrt(com_fixed_t a);

/* Approximated using LUT. */
/* TODO: test whether linearly interpolated version could be default. */
/*       things like camera movement can be jarring if done without.*/
/* https://namoseley.wordpress.com/2015/07/26/sincos-generation-using-table-lookup-and-iterpolation/
 */
static inline com_fixed_t com_fixed_sin(com_fixed_t q) {
  /* Wrap to (-Pi2,+Pi2) */
  q %= COM_FIXED_PI2;

  /* Wrap to [0,+Pi2) */
  if (q < 0)
    q += COM_FIXED_PI2;

  /* Limit to [0,+1) */
  q = com_fixed_div(q, COM_FIXED_PI2);

  /* Handle cases of 3rd and 4th quadrant separately. */
  if (q >= COM_FIXED_FRACHALF) {
    q %= COM_FIXED_FRACHALF;
    uint16_t const idx = q >= COM_FIXED_FRACQRTR ? 255 - (q >> 7) : q >> 7;
    return -com_fixed_sin_lut[idx];
  } else {
    uint16_t const idx = q >= COM_FIXED_FRACQRTR ? 255 - (q >> 7) : q >> 7;
    return com_fixed_sin_lut[idx];
  }
}

/* Implemented over sin, as to share one single LUT. */
/* Additionally, optimizer probably can collapse some math for combined sincos
 * case. */
static inline com_fixed_t com_fixed_cos(com_fixed_t a) {
  return com_fixed_sin(a + COM_FIXED_PIHALF);
}

/* Combines calculation of both in a more optimal way. */
static inline void com_fixed_sincos(com_fixed_t q, com_fixed_t *s,
                                    com_fixed_t *c) {
  q %= COM_FIXED_PI2;
  if (q < 0)
    q += COM_FIXED_PI2;
  q = com_fixed_div(q, COM_FIXED_PI2);

  /* Sadly, compilers are dumb and branching case is determined to be
   * significantly faster in profiling. */
#define CASE(m_sin_sign, m_cos_sign)                                           \
  uint16_t const idx = q >= COM_FIXED_FRACQRTR ? 255 - (q >> 7) : q >> 7;      \
  *s = m_sin_sign * com_fixed_sin_lut[idx];                                    \
  *c = m_cos_sign * com_fixed_sin_lut[((uint8_t)127 - (uint8_t)idx) % 128]

  /* Handle cases of 2rd and 3th quadrant separately, for minus cos. */
  if (q >= COM_FIXED_FRACHALF / 2 &&
      q < COM_FIXED_FRACHALF + COM_FIXED_FRACHALF / 2) {
    /* Handle cases of 3rd and 4th quadrant separately, for minus sin. */
    if (q >= COM_FIXED_FRACHALF) {
      q %= COM_FIXED_FRACHALF;
      CASE(-1, -1);
    } else {
      CASE(+1, -1);
    }
  } else {
    /* Handle cases of 3rd and 4th quadrant separately, for minus sin. */
    if (q >= COM_FIXED_FRACHALF) {
      q %= COM_FIXED_FRACHALF;
      CASE(-1, 1);
    } else {
      CASE(+1, 1);
    }
  }

#undef CASE
}

static inline com_fixed_t com_fixed_tan(com_fixed_t a) {
  com_fixed_t s, c;
  com_fixed_sincos(a, &s, &c);
  return com_fixed_div(s, c);
}

#endif