/*
 * Copyright (c) 2019 Foundries.io
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <kernel.h>
#include <stdlib.h>
#include "lwm2m_util.h"

#define SHIFT_LEFT(v, o, m) (((v) << (o)) & (m))
#define SHIFT_RIGHT(v, o, m) (((v) >> (o)) & (m))

/* convert from float32 to binary32 */
int lwm2m_f32_to_b32(float32_value_t *f32, u8_t *b32, size_t len)
{
	s32_t e = -1, v, f = 0;
	int i;

	if (len != 4) {
		return -EINVAL;
	}

	/* handle zero value special case */
	if (f32->val1 == 0 && f32->val2 == 0) {
		memset(b32, 0, len);
		return 0;
	}

	/* sign handled later */
	v = abs(f32->val1);

	/* add whole value to fraction */
	while (v > 0) {
		f >>= 1;

		if (v & 1) {
			f |= (1 << 23);
		}

		v >>= 1;
		e++;
	}

	/* sign handled later */
	v = abs(f32->val2);

	/* add decimal to fraction */
	i = e;
	while (v > 0 && i < 23) {
		v *= 2;
		if (!f && e < 0 && v < LWM2M_FLOAT32_DEC_MAX) {
			/* handle -e */
			e--;
			continue;
		} else if (v >= LWM2M_FLOAT32_DEC_MAX) {
			v -= LWM2M_FLOAT32_DEC_MAX;
			f |= 1 << (22 - i);
		}

		if (v == 0) {
			break;
		}

		i++;
	}

	/* adjust exponent for bias */
	e += 127;

	memset(b32, 0, len);

	/* sign: bit 31 */
	if (f32->val1 == 0) {
		b32[0] = f32->val2 < 0 ? 0x80 : 0;
	} else {
		b32[0] = f32->val1 < 0 ? 0x80 : 0;
	}

	/* exponent: bits 30-23 */
	b32[0] |= e >> 1;
	b32[1] = (e & 1) << 7;

	/* fraction: bits 22-0 */
	/* NOTE: ignore the "hidden" bit 23 in fraction */
	b32[1] |= (f >> 16) & 0x7F;
	b32[2] = (f >> 8) & 0xFF;
	b32[3] = f & 0xFF;

	return 0;
}

/* convert from float64 to binary64 */
int lwm2m_f64_to_b64(float64_value_t *f64, u8_t *b64, size_t len)
{
	s64_t v, f = 0;
	s32_t e = -1;
	int i;

	if (len != 8) {
		return -EINVAL;
	}

	/* handle zero value special case */
	if (f64->val1 == 0LL && f64->val2 == 0LL) {
		memset(b64, 0, len);
		return 0;
	}

	/* sign handled later */
	v = abs(f64->val1);

	/* add whole value to fraction */
	while (v > 0) {
		f >>= 1;

		if (v & 1) {
			f |= ((s64_t)1 << 52);
		}

		v >>= 1;
		e++;
	}

	/* sign handled later */
	v = abs(f64->val2);

	/* add decimal to fraction */
	i = e;
	while (v > 0 && i < 52) {
		v *= 2;
		if (!f && e < 0 && v < LWM2M_FLOAT64_DEC_MAX) {
			/* handle -e */
			e--;
			continue;
		} else if (v >= LWM2M_FLOAT64_DEC_MAX) {
			v -= LWM2M_FLOAT64_DEC_MAX;
			f |= (s64_t)1 << (51 - i);
		}

		if (v == 0) {
			break;
		}

		i++;
	}

	/* adjust exponent for bias */
	e += 1023;

	memset(b64, 0, len);

	/* sign: bit 63 */
	if (f64->val1 == 0) {
		b64[0] = f64->val2 < 0 ? 0x80 : 0;
	} else {
		b64[0] = f64->val1 < 0 ? 0x80 : 0;
	}

	/* exponent: bits 62-52 */
	b64[0] |= (e >> 4);
	b64[1] = ((e & 0xF) << 4);

	/* fraction: bits 51-0 */
	/* NOTE: ignore the "hidden" bit 52 in fraction */
	b64[1] |= ((f >> 48) & 0xF);
	b64[2] = (f >> 40) & 0xFF;
	b64[3] = (f >> 32) & 0xFF;
	b64[4] = (f >> 24) & 0xFF;
	b64[5] = (f >> 16) & 0xFF;
	b64[6] = (f >> 8) & 0xFF;
	b64[7] = f & 0xFF;

	return 0;
}

/* convert from binary32 to float32 */
int lwm2m_b32_to_f32(u8_t *b32, size_t len, float32_value_t *f32)
{
	s32_t f, k, i, e;
	bool sign = false;

	if (len != 4) {
		return -EINVAL;
	}

	f32->val1 = 0;
	f32->val2 = 0;

	/* calc sign: bit 31 */
	sign = SHIFT_RIGHT(b32[0], 7, 0x1);

	/* calc exponent: bits 30-23 */
	e  = SHIFT_LEFT(b32[0], 1, 0xFF);
	e += SHIFT_RIGHT(b32[1], 7, 0x1);
	/* remove bias */
	e -= 127;

	/* enable "hidden" fraction bit 23 which is always 1 */
	f  = ((s32_t)1 << 22);
	/* calc fraction: bits 22-0 */
	f += ((s32_t)(b32[1] & 0x7F) << 16);
	f += ((s32_t)b32[2] << 8);
	f += b32[3];

	/* handle whole number */
	if (e > -1) {
		/* precision overflow */
		if (e > 23) {
			e = 23;
		}

		f32->val1 = (f >> (23 - e)) * (sign ? -1 : 1);
	}

	/* calculate the rest of the decimal */
	k = LWM2M_FLOAT32_DEC_MAX;

	/* account for -e */
	while (e < -1) {
		k /= 2;
		e++;
	}

	for (i = 22 - e; i >= 0; i--) {
		k /= 2;
		if (f & (1 << i)) {
			f32->val2 += k;

		}
	}

	return 0;
}

/* convert from binary64 to float64 */
int lwm2m_b64_to_f64(u8_t *b64, size_t len, float64_value_t *f64)
{
	s64_t f, k;
	int i, e;
	bool sign = false;

	if (len != 8) {
		return -EINVAL;
	}

	f64->val1 = 0LL;
	f64->val2 = 0LL;

	/* calc sign: bit 63 */
	sign = SHIFT_RIGHT(b64[0], 7, 0x1);

	/* get exponent: bits 62-52 */
	e  = SHIFT_LEFT((u16_t)b64[0], 4, 0x7F0);
	e += SHIFT_RIGHT(b64[1], 4, 0xF);
	/* remove bias */
	e -= 1023;

	/* enable "hidden" fraction bit 53 which is always 1 */
	f  = ((s64_t)1 << 52);
	/* get fraction: bits 51-0 */
	f += ((s64_t)(b64[1] & 0xF) << 48);
	f += ((s64_t)b64[2] << 40);
	f += ((s64_t)b64[3] << 32);
	f += ((s64_t)b64[4] << 24);
	f += ((s64_t)b64[5] << 16);
	f += ((s64_t)b64[6] << 8);
	f += b64[7];

	/* handle whole number */
	if (e > -1) {
		/* precision overflow */
		if (e > 52) {
			e = 52;
		}

		f64->val1 = (f >> (52 - e)) * (sign ? -1 : 1);
	}

	/* calculate the rest of the decimal */
	k = LWM2M_FLOAT64_DEC_MAX;

	/* account for -e */
	while (e < -1) {
		k /= 2;
		e++;
	}

	for (i = 51 - e; i >= 0; i--) {
		k /= 2;
		if (f & ((s64_t)1 << i)) {
			f64->val2 += k;

		}
	}

	return 0;
}