#include "fixed.h"

fixed_t int_to_fixed(i32 i) { return i << 16; }

i32 fixed_to_int(fixed_t f) { return f >> 16; }

fixed_t float_to_fixed(f32 f) { return (fixed_t)(f * 65536.0f); }

f32 fixed_to_float(fixed_t f) { return (f32)f / 65536.0f; }

fixed_t fixed_add(fixed_t a, fixed_t b) { return a + b; }

fixed_t fixed_sub(fixed_t a, fixed_t b) { return a - b; }

fixed_t fixed_mul(fixed_t a, fixed_t b) {
  i32 a_hi = a >> 16;
  u32 a_lo = (u32)a & 0xFFFFU;
  i32 b_hi = b >> 16;
  u32 b_lo = (u32)b & 0xFFFFU;

  i32 p0 = (i32)(a_lo * b_lo) >> 16;
  i32 p1 = a_hi * (i32)b_lo;        
  i32 p2 = (i32)a_lo * b_hi;        
  i32 p3 = (a_hi * b_hi) << 16;     

  return p0 + p1 + p2 + p3;
}

fixed_t fixed_div(fixed_t a, fixed_t b) {
  i32 negative;
  u32 ua, ub, quotient, remainder;
  i32 i;

  if (b == 0)
    return 0;

  negative = ((a < 0) ^ (b < 0));

  ua = (a < 0) ? -a : a;
  ub = (b < 0) ? -b : b;

  quotient = 0;
  remainder = 0;

  for (i = 0; i < 32; i++) {
    remainder <<= 1;
    if (ua & 0x80000000U) {
      remainder |= 1;
    }
    ua <<= 1;

    if (remainder >= ub) {
      remainder -= ub;
      quotient |= 1;
    }

    if (i < 31) {
      quotient <<= 1;
    }
  }

  return negative ? -(i32)quotient : (i32)quotient;
}

i32 fixed_eq(fixed_t a, fixed_t b) { return a == b; }

i32 fixed_ne(fixed_t a, fixed_t b) { return a != b; }

i32 fixed_lt(fixed_t a, fixed_t b) { return a < b; }

i32 fixed_le(fixed_t a, fixed_t b) { return a <= b; }

i32 fixed_gt(fixed_t a, fixed_t b) { return a > b; }

i32 fixed_ge(fixed_t a, fixed_t b) { return a >= b; }

fixed_t fixed_neg(fixed_t f) { return -f; }

fixed_t fixed_abs(fixed_t f) { return (f < 0) ? -f : f; }