#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_CONST (1 << NUM_FRAC_BITS) /* f = 2^q, (2^20) */

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

/* Handles conversion of fixed point to integer. First version truncates, second one rounds */
inline int32_t
fp_floor (fp32_t x)
{
  return x.raw / CONVERSION_CONST;
}

inline int32_t
fp_round (fp32_t x)
{
  if (x.raw >= 0)
    return (x.raw + CONVERSION_CONST / 2) / CONVERSION_CONST;
  else
    return (x.raw - CONVERSION_CONST / 2) / CONVERSION_CONST;
}

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

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

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

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

/* Divide two fixed points */
inline fp32_t
fp_div (fp32_t x, fp32_t y)
{
  return (fp32_t){ ((int64_t)x.raw) * CONVERSION_CONST / 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 };
}

#endif //FIXED_POINT_H