#include <atmel_start.h>
#include <math.h>
#include <COAST.h>

uint16_t __xMR_FN_CALL float_to_linear(float val)
{
        int16_t exp = -16;

        if (val)
                exp = ceilf(log2f(fabsf(val)) - log2f(1023));
        else
                return 0;

        if (exp < -16)
                exp = -16;
        if (exp > 15)
                exp = 15;

        int16_t man = floorf(val / powf(2, exp));
        if (man < -1024)
                man = -1024;
        if (man > 1023)
                man = 1023;

        return ((exp & 0x1f) << 11) | (man & 0x7ff);
}


uint16_t __xMR_FN_CALL float_to_linear16(float val)
{
        if (val < 0)
                return 0;

        // fixed 2^-10 exponent (0x16 in 5-bit two's complement)
        uint32_t res = val / powf(2, -10);
        return __USAT(res, 16);
}


float __xMR_FN_CALL linear_to_float(uint16_t val)
{
        int16_t exp = ((val & 0x8000)?0xffe0:0) | ((val >> 11) & 0x1f);
        int16_t man = ((val & 0x0400)?0xf800:0) | (val & 0x7ff);

        return (man * powf(2, exp));
}
