/*
 * esq.v
 * Copyright (C) 2014  Krzysztof Mazur <krzysiek@podlesie.net>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 */

module esq(clk, reset, sync_in, rnd, in, out);

parameter IN_BITS = 9;
parameter EXTRA_BITS = 2;
parameter OUT_BITS = 2;
parameter INX_BITS = IN_BITS + EXTRA_BITS;
parameter SHIFT_BITS = INX_BITS - OUT_BITS;
parameter SHIFT_OFFSET = 1 << (SHIFT_BITS - 1);

parameter INX_OFFSET = (1 << (INX_BITS - 1)) - (1 << (IN_BITS - 1));

input clk;
input reset;
input sync_in;
input [2 * SHIFT_BITS - 1:0] rnd;

input [IN_BITS - 1:0] in;
output [OUT_BITS - 1:0] out;

wire [INX_BITS - 1:0] inx = INX_OFFSET + in;

/* quantization error signal */
reg signed [INX_BITS:0] e = 0;

wire [SHIFT_BITS:0] rnd_a = { 1'b0, rnd[SHIFT_BITS - 1:0] };
wire [SHIFT_BITS:0] rnd_b = { 1'b0, rnd[2 * SHIFT_BITS - 1:SHIFT_BITS] };

wire signed [SHIFT_BITS:0] dither = rnd_a - rnd_b;

wire signed [INX_BITS:0] c = inx - e;
wire signed [INX_BITS:0] u = c + dither + SHIFT_OFFSET;
wire signed [OUT_BITS:0] u_shifted = u[INX_BITS:SHIFT_BITS];
reg signed [OUT_BITS:0] u_saturated;
always @(u_shifted) begin
	if (u_shifted[OUT_BITS:OUT_BITS - 1] == 2'b11)
		u_saturated <= 0;
	else if (u_shifted[OUT_BITS:OUT_BITS - 1] == 2'b10)
		u_saturated <= (1 << OUT_BITS) - 1;
	else
		u_saturated <= u_shifted;
end

reg signed [INX_BITS:0] u_saturated_ext;
always @(u_saturated) begin
	u_saturated_ext = 0;
	u_saturated_ext[INX_BITS:INX_BITS - OUT_BITS] = u_saturated;
end

always @(posedge clk) begin
	if (0) begin
		$display("inx: %h, %h, e: %h",
			in, inx, e);
		$display("	c: %h",
			c);
		$display("	inx: %h, %h, e: %h, dither: %h, u: %h, u_shifted: %h",
			in, inx, e, dither, u, u_shifted);
		$display("	u_sat: %h  u_saturated_ext %h,  c %h",
				u_saturated, u_saturated_ext, c);
	end

	if (reset)
		e <= 0;
	else if (sync_in)
		e <= u_saturated_ext - c;
	else
		e <= e;
end

reg [OUT_BITS - 1:0] out = 0;
always @(posedge clk) begin
	if (reset)
		out <= 0;
	else if (sync_in)
		out <= u_saturated[OUT_BITS - 1:0];
	else
		out <= out;
end

endmodule