`include "heron_states.v"

module heron_top_tb();
`define ASSERT(c, msg) if (!c) begin $display("%c[31mAssertion Failed: %s%c[0m", 27, msg, 27); $finish(2); end
`define ASSERT_STATE(state) `ASSERT((fsm_state == state), "not expected state")

parameter CYCLE = 50;
parameter COMMA = 8;

wire  [31:0] edb;
wire  [31:0] eab;
wire  ready;
wire  ram_wr_en, ram_rd_en;
wire  [3:0] fsm_state;
assign fsm_state = heron.fsm.state;

reg reset;
reg load = 0;
reg [15:0] i;

reg clk = 1'b0;
always #(CYCLE/2) clk = ~clk;

reg [31:0] ram [0:1023];

wire [31:0] ram0;
wire [31:0] ram1;
wire [31:0] ram2;
wire [31:0] ram3;
wire [31:0] ram4;
assign ram0 = ram[0];
assign ram1 = ram[1];
assign ram2 = ram[2];
assign ram3 = ram[3];
assign ram4 = ram[4];

assign edb = ram_rd_en ? ram[eab[9:0]] : 32'bz;

task displayRam;
  reg [15:0] j;
  reg [32:0] after_comma;
  begin
    for (j = 1; j <= ram[0]; j = j + 1) begin
      // display fixed point as decimal
      after_comma = ((ram[j][COMMA]     * 10000) >> 1) +
                    ((ram[j][COMMA - 1] * 10000) >> 2) +
                    ((ram[j][COMMA - 2] * 10000) >> 3) +
                    ((ram[j][COMMA - 3] * 10000) >> 4);
      $display("ram[%d] = %d.%d (0x%h)", j, ram[j] >> COMMA, after_comma, ram[j]);
    end
  end
endtask

always @ (posedge clk) begin
  if (ram_wr_en) begin
    ram[eab[9:0]] <= edb;
  end
end

heron_top heron(
  .clk(clk),
  .load(load),
  .reset(reset),
  .edb(edb),
  .eab(eab),
  .ram_rd_en(ram_rd_en),
  .ram_wr_en(ram_wr_en),
  .ready(ready)
);

initial begin
  $dumpfile("heron_top_tb.vcd");
  $dumpvars;

  ram[0] = 4;
  ram[4] = 16 << COMMA;
  ram[3] = 4 << COMMA;

  ram[2] = 0 << COMMA;
  ram[1] = 1 << COMMA;
end

initial begin
  `ASSERT_STATE(`IDLE)
  #(5*CYCLE)
  load = 1;
  reset = 1'b1;
  #(CYCLE)
  reset = 1'b0;
  #(CYCLE)
  load = 0;
  // load mem[0] -> number of operands
  `ASSERT_STATE(`LD_N_1)
  #(CYCLE)
  // load...
  `ASSERT_STATE(`LD_N_2)
  #(CYCLE)
  // if (mem[0] > 0) then
  `ASSERT_STATE(`I_GT_ZERO)
  #(CYCLE)
  // load mem[1]...
  `ASSERT_STATE(`LD_S_1)
  #(CYCLE)
  // ...load
  `ASSERT_STATE(`LD_S_2)
  #(CYCLE)
  // if (mem[1] > 1) then
  `ASSERT_STATE(`S_GT_ONE)
  #(CYCLE)
  // x_0 = s / 2
  `ASSERT_STATE(`X_1)
  #(CYCLE)
  // s / x_n...
  `ASSERT_STATE(`DIV_2)
  #(CYCLE)
  // ..divide
  `ASSERT_STATE(`DIV_3)
  #(CYCLE)
  // x_1 = x_n + (s / x_n) / 2
  `ASSERT_STATE(`DIV_4)
  #(CYCLE)
  // if (old_x <= x_1) ...
  `ASSERT_STATE(`OLD_X_LTE_X_1)
  #(CYCLE)
  // ... then
  `ASSERT_STATE(`OLD_X_LTE_X_2)
  #(CYCLE)
  // store mem[1] = 2
  `ASSERT_STATE(`STORE_X)
  #(CYCLE)
  // i--
  `ASSERT(ram[3] == 2, "sqrt(4) == 2")
  `ASSERT(ram[3] == 1, "sqrt(4) == 2")
  `ASSERT_STATE(`DEC_I)
  #(CYCLE)
  // if (i > 0) then ...
  `ASSERT_STATE(`I_GT_ZERO)

  for (i = 0; i < 1000; i = i + 1) begin
    if (ready != 0) begin
      $display("%c[32mSUCCESS: heron_top_tb tests bench finished;%c[0m", 27, 27);
      displayRam();
      $finish;
    end
    #(CYCLE) ;
  end
  $display("%c[31mFAILED: heron_top_tb tests timeout!%c[0m", 27, 27);
  displayRam();
  $finish(1);
end
endmodule