#include <stdint.h>
#ifndef FIXED_POINT_H
#define FIXED_POINT_H

typedef struct
  {
    int32_t raw;
  } fp32_t;

/* Fixed Point Arithmetic bit count constants */
#define NUM_FRAC_BITS 14
#define NUM_INT_BITS (31 - NUM_FRAC_BITS)
#define CONVERSION_FACTOR (1 << NUM_FRAC_BITS) /* f = 2^q, (2^14) */

/* Fixed Point Arithmetic conversion operations */
/* Converts an integer n to a fixed point number */
inline fp32_t
fp_from_int (int32_t n)
{
  return (fp32_t){ n * CONVERSION_FACTOR };
}

/* Handles conversion of fixed point to integer,
   with truncation */
inline int32_t
fp_floor (fp32_t x)
{
  return x.raw / CONVERSION_FACTOR;
}

/* Handles conversion of fixed point to integer,
   with rounding */
inline int32_t
fp_round (fp32_t x)
{
  if (x.raw >= 0)
    return (x.raw + CONVERSION_FACTOR / 2) / CONVERSION_FACTOR;
  else
    return (x.raw - CONVERSION_FACTOR / 2) / CONVERSION_FACTOR;
}

/* Add two fixed points */
inline fp32_t
fp_add (fp32_t x, fp32_t y)
{
  return (fp32_t){ x.raw + y.raw };
}

/* Subtract two fixed points */
inline fp32_t
fp_sub (fp32_t x, fp32_t y)
{
  return (fp32_t){ x.raw - y.raw };
}



/* Multiple two fixed points */
inline fp32_t
fp_mul (fp32_t x, fp32_t y)
{
  return (fp32_t){ ((int64_t)x.raw) * y.raw / CONVERSION_FACTOR };
}

/* Divide two fixed points */
inline fp32_t
fp_div (fp32_t x, fp32_t y)
{
  return (fp32_t){ ((int64_t)x.raw) * CONVERSION_FACTOR / y.raw };
}

/* Multiply fixed point and integer */
inline fp32_t
fp_mul_int (fp32_t x, int32_t n)
{
  return (fp32_t){ x.raw * n };
}

/* Divide fixed point by integer */
inline fp32_t
fp_div_int (fp32_t x, int32_t n)
{
  return (fp32_t){ x.raw / n };
}

/* Add fixed point to integer */
inline fp32_t
fp_add_int (fp32_t x, int32_t n)
{
  return (fp32_t){ x.raw + n * CONVERSION_FACTOR };
}

/* Subtract integer from fixed point */
inline fp32_t
fp_sub_int (fp32_t x, int32_t n)
{
  return (fp32_t){ x.raw - n * CONVERSION_FACTOR };
}

#endif //FIXED_POINT_H