module floating_add #(parameter N=16, M=4)(input_1, input_2, sum, diff);
	input logic [N-1:0] input_1, input_2;
	output logic [N-1:0] sum;
	output logic [M:0] diff;

	logic flag_a;
	logic flag_b;
	logic [M:0] abs;
	logic [N-3-M:0] res;
	
	// sign_x = x[N-1]
	// exponent_x = x[N-2:N-2-M]
	// mantissa_x = x[N-3-M:0]
	
	always_comb
		begin
			if (input_1[N-2:N-2-M] > input_2[N-2:N-2-M]) // If input 1 has the bigger exponent 
				begin
					// Flags input a as larger and calculates the absolute difference
					flag_a = 1;
					flag_b = 0;
					abs = input_1[N-2:N-2-M] - input_2[N-2:N-2-M];
					// ASsigning overall sign of the output
					sum[N-1] = input_1[N-1];
					// Sets output to have the same exponent
					sum[N-2:N-2-M] = input_1[N-2:N-2-M];
				end
			else if (input_2[N-2:N-2-M] > input_1[N-2:N-2-M]) // If input 2 has the bigger exponent
				begin
					// Similarly flags input b as larger and calculates the absolute difference
					flag_a = 0;
					flag_b = 1;
					abs = input_2[N-2:N-2-M] - input_1[N-2:N-2-M];
					// ASsigning overall sign of the output
					sum[N-1] = input_2[N-1];
					// Sets ouput to have the same exponent
					sum[N-2:N-2-M] = input_2[N-2:N-2-M];
				end
			else 
				begin
					// THe condition that both inputs have the same exponent
					flag_a = 1;
					flag_b = 1;
					abs = 0;
					// ASsigning overall sign of the output based on size of the mantissa
					if (input_1[N-3-M:0] >= input_2[N-3-M:0]) sum[N-1] = input_1[N-1];
					else sum[N-1] = input_2[N-1];
					sum[N-2:N-2-M] = input_1[N-2:N-2-M];
				end
			diff = abs;
		end

		//Second pipeline stage 1
		pipe pipe1(.clk(clk), .reset(reset), .D(Q0), .Q(Q1));

	always_comb
		begin
			// Condition for overflow is that it sets the output to the larger input
			if (abs > 9) // Because size of mantissa is 10 bits and shifting by 10 would give 0
				begin
					if (flag_a & ~flag_b) sum = input_1; // input 1 is larger and is translated to output
					else if (~flag_a & flag_b) sum = input_2; // input 2 is larger and is translated to output
					else // exponents are the same
						begin
							if (input_1[N-3-M:0] >= input_2[N-3-M:0]) sum = input_1;// input 1 has the bigger mantissa
							else sum = input_2; // input 2 has the bigger mantissa
						end
				end
			else
				begin
					// Shifts the smaller input's mantissa to the right based on abs
					if (flag_a & ~flag_b)// If input 1 has the larger exponent
						begin
							// If the signs of both inputs are the same you add, otherwise you subtract
							if (input_1[N-1] == input_2[N-1])
								begin
									res = input_1[N-3-M:0] + (input_2[N-3-M:0] >> abs-1); // Sum the mantissa
									sum[N-3-M:0] = res;
								end
							else
								begin
									res = input_1[N-3-M:0] - (input_2[N-3-M:0] >> abs-1); // Subtract the mantissas
									sum[N-3-M:0] = res;
								end
						end
					else if (~flag_a & flag_b)
						begin
							// If the signs of both inputs are the same you add, otherwise you subtract
							if (input_1[N-1] == input_2[N-1])
								begin
									res = (input_1[N-3-M:0] >> abs-1) + input_2[N-3-M:0]; // Sum the mantissa
									sum[N-3-M:0] = res;
								end
							else
								begin
									res = input_2[N-3-M:0] - (input_1[N-3-M:0] >> abs-1); // Subtract the mantissas
									sum[N-3-M:0] = res;
								end
						end
					else
						begin 
							if (input_1[N-1] == input_2[N-1]) // If exponents and signs equal
								begin
									res = input_1[N-3-M:0] + input_2[N-3-M:0]; // Sum the mantissa
									sum[N-3-M:0] = res;
								end
							else // In this case it will be a subtraction
								begin
									if (input_1[N-3-M:0] > input_2[N-3-M:0]) // Which has the larger mantissa 
										begin
											res = input_1[N-3-M:0] - input_2[N-3-M:0]; // Subtract the mantissa
											sum[N-3-M:0] = res;
										end
									else if (input_1[N-3-M:0] < input_2[N-3-M:0])
										begin
											res = input_2[N-3-M:0] - input_1[N-3-M:0]; // Subtract the mantissa
											sum[N-3-M:0] = res;
										end
									else res = 0; // Both the exponent and the mantissa are equal so subtraction leads to 0
									sum[N-3-M:0] = res;
								end
						end
				end
		end
endmodule : floating_add



module floating_product #(parameter N=16, M=4)(input_1, input_2, product);
	input logic [N-1:0] input_1, input_2;
	output logic [N-1:0] product;

	// sign_x = x[N-1]
	// exponent_x = x[N-2:N-2-M]
	// mantissa_x = x[N-3-M:0]

	logic [N-2:N-2-M] sum;
	logic [2*(N-3-M):0] mult;

	// We have assigned an {M+1} bit exponent so we must have a 2^{M} offset
	assign sum = input_1[N-2:N-2-M] + input_2[N-2:N-2-M];
	assign product[N-2:N-2-M] = sum - (1'b1 << M) + 2;

	always_comb
		begin
				// Setting the mantissa of the output
				mult = input_1[N-3-M:0] * input_2[N-3-M:0];
				if (mult[N-3-M]) product[N-3-M:0] = mult[2*(N-3-M):2*(N-3-M)-9];
				else product[N-3-M:0] = mult[2*(N-3-M):2*(N-3-M)-9] << 1;
				product[N-1] = input_1[N-1] ^ input_2[N-1];
		end
endmodule : floating_product



module pipe #(parameter N=16)(clk, reset, Q, D);
	input logic clk, reset;
	input logic [N-1:0] D;
	output reg [N-1:0] Q;
	reg [N-1:0] in_pipe;
	
	always @(posedge clk or negedge reset)
		begin
			if(reset) in_pipe = 0;
			else in_pipe = D;
		end
	
	always @(posedge clk or negedge reset)
		begin
			if(reset) Q = 0;
			else Q = in_pipe;
		end
endmodule : pipe



module floating_tb;
	reg reset, clk;
	logic [15:0] input_a, input_b, result_add, result_mult;
	logic [4:0] diff;

	floating_add adder1(.input_1(input_a), .input_2(input_b), .sum(result_add), .diff(diff));

	floating_product multiplier1(.input_1(input_a), .input_2(input_b), .product(result_mult));


	reg [15:0] test_mem [29:0][3:0];

	initial $readmemh("../../scripts/fp16_test.hex", test_mem);


	initial begin
        static int num_err = 0;
        static int num_tests = $size(test_mem) * 2;

        for (int i=0; i < $size(test_mem); i++) begin
            input_a = test_mem[i][0];
            input_b = test_mem[i][1];

            #10;
            if(result_add != test_mem[i][2]) begin
                if(num_err < 20)
                    $display("FAIL ADD: %H + %H = %H, expected %H", input_a, input_b, result_add, test_mem[i][2]);
                num_err = num_err + 1;
            end

			if(result_mult != test_mem[i][3]) begin
                if(num_err < 20)
                    $display("FAIL MULTIPLY: %H + %H = %H, expected %H", input_a, input_b, result_mult, test_mem[i][3]);
                num_err = num_err + 1;
            end

        end
        $display("Passed %d of %d tests", num_tests-num_err, num_tests);
        $finish();
	end
endmodule : floating_tb



module floating32_tb;
	reg reset, clk;
	logic [31:0] input_a, input_b, result_add, result_mult;

	floating_add#(.N(32), .M(8)) add0(
		.input_1(input_a), .input_2(input_b), .sum(result_add), .diff()
	);
	floating_product#(.N(32), .M(8)) mult0(
		.input_1(input_a), .input_2(input_b), .product(result_mult)
	);

	reg [31:0] test_mem [29:0][3:0];

	initial $readmemh("scripts/fp32_test.hex", test_mem);


	initial begin
		static int num_err = 0;
		static int num_tests = $size(test_mem) * 2;

		for (int i=0; i < $size(test_mem); i++) begin
			input_a = test_mem[i][0];
			input_b = test_mem[i][1];

			#10;
			if(result_add != test_mem[i][2]) begin
				if(num_err < 20)
					$display("FAIL ADD: %H + %H = %H, expected %H", input_a, input_b, result_add, test_mem[i][2]);
				num_err = num_err + 1;
			end

			if(result_mult != test_mem[i][3]) begin
				if(num_err < 20)
					$display("FAIL MULTIPLY: %H + %H = %H, expected %H", input_a, input_b, result_mult, test_mem[i][3]);
				num_err = num_err + 1;
			end

		end
		$display("Passed %d of %d tests", num_tests-num_err, num_tests);
		$finish();
	end
endmodule : floating32_tb