/*
 * main.c
 *
 *  Created on: Apr 2, 2019
 *      Author: amras
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include "diag/Trace.h"

#include "stm32f4xx.h"
#include "stm32f4xx_hal.h"

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wmissing-declarations"
#pragma GCC diagnostic ignored "-Wreturn-type"

#define MANTISSA_BITS 23
#define EXPONENT_BITS 8

#define FIRST_BIT (1 << (MANTISSA_BITS - 1))
#define MAX_MANTISSA ((1 << MANTISSA_BITS) - 1)
#define MANTISSA_MASK MAX_MANTISSA

#define MAX_EXPONENT ((1 << EXPONENT_BITS) - 1)
#define EXPONENT_OFFSET (MAX_EXPONENT >> 1)

struct floating {
	uint8_t sign:1;
	uint32_t mantissa:MANTISSA_BITS;
	uint8_t exponent:EXPONENT_BITS;
};

struct floating zero(){
	struct floating f;
	f.sign = 1;
	f.mantissa = 0;
	f.exponent = 0;
	return f;
}

struct floating infinity() {
	struct floating f;
	f.sign = 1;
	f.mantissa = 0;
	f.exponent = MAX_EXPONENT;
	return f;
}

struct floating two() {
	struct floating f;
	f.sign = 1;
	f.mantissa = 0;
	f.exponent = EXPONENT_OFFSET + 1;
	return f;
}

struct floating negate(struct floating f) {
	f.sign = !f.sign;
	return f;
}

struct floating add(struct floating a, struct floating b) {
	struct floating res;
	struct floating bigger, smaller;
	uint8_t exp, exp_diff;
	uint32_t bigger_mantissa, smaller_mantissa, mantissa;

	if (a.exponent == b.exponent) {
		if (a.mantissa >= b.mantissa) {
			bigger = a;
			smaller = b;
		} else {
			bigger = b;
			smaller = a;
		}
	}
	if (a.exponent > b.exponent) {
		bigger = a;
		smaller = b;
	}
	if (a.exponent < b.exponent) {
		bigger = b;
		smaller = a;
	}

	exp = bigger.exponent;
	exp_diff = bigger.exponent - smaller.exponent;
	bigger_mantissa = bigger.mantissa;
	smaller_mantissa = smaller.mantissa >> exp_diff;
	if (exp_diff > 0) {
		smaller_mantissa |= FIRST_BIT >> (exp_diff - 1);
	}

	if (bigger.sign == smaller.sign) {
		// adding
		if (exp_diff == 0 || smaller_mantissa > MAX_MANTISSA - bigger_mantissa) {
			// overflow
			exp++;
			bigger_mantissa = bigger_mantissa >> 1 | FIRST_BIT;
			if (exp_diff == 0)
				smaller_mantissa = smaller_mantissa >> 1 | FIRST_BIT;
			else
				smaller_mantissa = smaller_mantissa >> 1;
		}
		mantissa = bigger_mantissa + smaller_mantissa;
	} else {
		// subtracting
		mantissa = bigger_mantissa - smaller_mantissa;

		if (exp_diff == 0 || smaller_mantissa > bigger_mantissa) {
			while ((mantissa & FIRST_BIT) == 0) {
				mantissa = mantissa & MANTISSA_MASK;
				if (mantissa == 0)
					return zero();
				exp--;
				mantissa = mantissa << 1;
			}
			// shift one more bit because of the implied leading '1'
			exp--;
			mantissa = mantissa << 1;
		}
	}

	mantissa = mantissa & MANTISSA_MASK;

	res.sign = bigger.sign;
	res.mantissa = mantissa;
	res.exponent = exp;

	return res;
}

struct floating sub(struct floating a, struct floating b) {
	return add(a, negate(b));
}

struct floating frac(struct floating f) {
	if (f.mantissa == 0)
		return zero();
	while ((f.mantissa & FIRST_BIT) == 0) {
		f.mantissa <<= 1;
		f.exponent--;
	}
	f.mantissa <<= 1;
	f.exponent--;

	f.mantissa &= MANTISSA_MASK;

	return f;
}

struct floating multiply(struct floating a, struct floating b){
	struct floating pa, pb, fa, fb, out;


	if (a.exponent == 0 || b.exponent == 0)
		return zero();

	if (a.mantissa == 0 && b.mantissa == 0){
		out.sign = a.sign == b.sign ? 1 : 0;
		out.exponent = a.exponent - EXPONENT_OFFSET + b.exponent;
		out.mantissa = 0;
		return out;
	}

	pa = a;
	pb = b;
	pa.mantissa = 0;
	pb.mantissa = 0;
	fa = frac(a);
	fb = frac(b);

	return add(multiply(pa, pb),
	    add(multiply(pb, fa),
		add(multiply(pa, fb),
		multiply(fa, fb))));
}

struct floating reciprocal(struct floating f) {
	struct floating guess;
	uint8_t i;

	guess.sign = f.sign;
	guess.mantissa = 0;
	guess.exponent = -(f.exponent - EXPONENT_OFFSET) + EXPONENT_OFFSET;

	for (i=0; i<32; ++i)
		guess = multiply(guess, sub(two(), multiply(f, guess)));

	return guess;
}

struct floating divide(struct floating a, struct floating b){
	return multiply(a, reciprocal(b));
}

int ipow(int base, int exponent) {
	int i, res;
	for (i=0, res=1; i<exponent; ++i, res=res*base);
	return res;
}

void show(char* str, struct floating f, int digits){
	int e;
	uint32_t whole, decimal;
	uint64_t a, b, c, d;

	e = f.exponent - ((1 << (EXPONENT_BITS - 1)) - 1);
	if (e >= 0) {
		// exponent is positive
		// multiply by 2^exponent
		whole = (f.mantissa >> (MANTISSA_BITS - e))
				// and add 2^exponent because of the implied '1' in mant.
				+ (1 << e);
		decimal = (f.mantissa << e) & MANTISSA_MASK;
	}
	else {
		whole = 0;
		decimal = (f.mantissa >> (0 - e)) + (1 << (MANTISSA_BITS + e));
	}

	a = (unsigned long) ipow(10, digits); // 100000
	b = (unsigned long) decimal; // 4194304
	c = a * b; // 2818572288 WHY
	//d = (unsigned long) decimal * (unsigned long) ipow(10, digits);
	d = c / (1 << MANTISSA_BITS);

	decimal = d;
	str[0] = f.sign > 0 ? '+' : '-';
	sprintf(str+1, "%d.%d", (int)whole, (int)decimal);
}

int main(int argc, char* argv[])
{
	struct floating f, g, h;
	char buffer[128];

	trace_puts("Running");

	// f = 3.0f;
	f.sign = 1;
	f.mantissa = FIRST_BIT;
	f.exponent = 128;

	// g = 5.0f;
	g.sign = 0;
	g.mantissa = (FIRST_BIT >> 1) + (FIRST_BIT >> 2);
	g.exponent = 129;

	// h = some random value
	h.sign = 1;
	h.mantissa = FIRST_BIT + (FIRST_BIT >> 3) + (FIRST_BIT >> 5) + (FIRST_BIT >> 14) + (FIRST_BIT >> 20);
	h.exponent = 139;

	show(buffer, f, 5);
	trace_puts("F = ");
	trace_puts(buffer);

	show(buffer, g, 5);
	trace_puts("G = ");
	trace_puts(buffer);

	show(buffer, h, 5);
	trace_puts("H = ");
	trace_puts(buffer);

	show(buffer, add(f, g), 5);
	trace_puts("F+G = ");
	trace_puts(buffer);

	show(buffer, sub(f, g), 5);
	trace_puts("F-G = ");
	trace_puts(buffer);

	show(buffer, sub(h, h), 5);
	trace_puts("H-H = ");
	trace_puts(buffer);

	show(buffer, add(h, g), 5);
	trace_puts("H+G = ");
	trace_puts(buffer);

	show(buffer, multiply(f, g), 5);
	trace_puts("F*G = ");
	trace_puts(buffer);

	show(buffer, multiply(g, g), 5);
	trace_puts("G*G = ");
	trace_puts(buffer);

	show(buffer, multiply(f, h), 5);
	trace_puts("F*H = ");
	trace_puts(buffer);

	show(buffer, divide(g, g), 5);
	trace_puts("G/G = ");
	trace_puts(buffer);


	show(buffer, divide(h, f), 5);
	trace_puts("H/F = ");
	trace_puts(buffer);

	show(buffer, divide(g, f), 5);
	trace_puts("G/F = ");
	trace_puts(buffer);

	return 0;
}