include/fixedpoint.h

#ifndef FIXEDPOINT_H
#define FIXEDPOINT_H

#include "mc04types.h"
#include "mpc_const.h"

enum {
        BITS_IN_BYTE = 8,
        ARCH_BITS = sizeof(float32_t) * BITS_IN_BYTE,
        FILTER = 0xffffffff >> (ARCH_BITS - FRAC_BITS),

};

//typedef long long unsigned int uint64_t;
//typedef long long int int64_t;

static inline int32_t fip_mul(int32_t a, int32_t b)
{
        int64_t c;
        c = (uint64_t) a *(uint64_t) b;
//      asm ("smull %0,%H0,%1,%2" : "=&r"(c) : "r"(a), "r"(b));

        return (int32_t) (c >> FRAC_BITS);
}

static inline int32_t fip_add(int32_t a, int32_t b)
{
        return a + b;
}

static inline int32_t fip_sub(int32_t a, int32_t b)
{
        return a - b;
}

static inline int32_t fip_div(int32_t n, int32_t d)
{
        int64_t cl;
        int64_t nl = (int64_t) n;
        int64_t dl = (int64_t) d;

        cl = (nl << ARCH_BITS) / dl;
        return (int32_t) (cl >> (ARCH_BITS - FRAC_BITS));
}

static inline float32_t fip_fip2real(int32_t a)
{
        const float32_t factor = (float32_t) (1 << FRAC_BITS);
        int32_t ci = a >> FRAC_BITS;
        float32_t cd = (float32_t) (a & FILTER) / factor;
        return ci + cd;
}

static inline int32_t fip_real2fip(float32_t a)
{
        const float32_t factor = (float32_t) (1 << FRAC_BITS);
        int32_t ci = (int32_t) a;
        int32_t cd = (int32_t) ((a - (float32_t) ci) * factor);
        return (ci << FRAC_BITS) + cd;
}

#endif