module Hq2x_optimized
#(
    parameter LENGTH = 858, 
    parameter HALF_DEPTH = 0
) 
(
    input             clk,
    input             ce_x4,
    input  [DWIDTH:0] inputpixel,
    input             mono,
    input             disable_hq2x,
    input             reset_frame,
    input             reset_line,
    input       [1:0] read_y,
    input             hblank,
    output [DWIDTH:0] outpixel /*verilator public*/
);

localparam AWIDTH = $clog2(LENGTH)-1;
localparam DWIDTH = HALF_DEPTH ? 11 : 23;
localparam DWIDTH1 = DWIDTH+1;
localparam EXACT_BUFFER = 1;

`HQ_TABLE_TYPE [5:0] hqTable[256] = '{
    19, 19, 26, 11, 19, 19, 26, 11, 23, 15, 47, 35, 23, 15, 55, 39,
    19, 19, 26, 58, 19, 19, 26, 58, 23, 15, 35, 35, 23, 15, 7,  35,
    19, 19, 26, 11, 19, 19, 26, 11, 23, 15, 55, 39, 23, 15, 51, 43,
    19, 19, 26, 58, 19, 19, 26, 58, 23, 15, 51, 35, 23, 15, 7,  43,
    19, 19, 26, 11, 19, 19, 26, 11, 23, 61, 35, 35, 23, 61, 51, 35,
    19, 19, 26, 11, 19, 19, 26, 11, 23, 15, 51, 35, 23, 15, 51, 35,
    19, 19, 26, 11, 19, 19, 26, 11, 23, 61, 7,  35, 23, 61, 7,  43,
    19, 19, 26, 11, 19, 19, 26, 58, 23, 15, 51, 35, 23, 61, 7,  43,
    19, 19, 26, 11, 19, 19, 26, 11, 23, 15, 47, 35, 23, 15, 55, 39,
    19, 19, 26, 11, 19, 19, 26, 11, 23, 15, 51, 35, 23, 15, 51, 35,
    19, 19, 26, 11, 19, 19, 26, 11, 23, 15, 55, 39, 23, 15, 51, 43,
    19, 19, 26, 11, 19, 19, 26, 11, 23, 15, 51, 39, 23, 15, 7,  43,
    19, 19, 26, 11, 19, 19, 26, 11, 23, 15, 51, 35, 23, 15, 51, 39,
    19, 19, 26, 11, 19, 19, 26, 11, 23, 15, 51, 35, 23, 15, 7,  35,
    19, 19, 26, 11, 19, 19, 26, 11, 23, 15, 51, 35, 23, 15, 7,  43,
    19, 19, 26, 11, 19, 19, 26, 11, 23, 15, 7,  35, 23, 15, 7,  43
};

reg [23:0] Prev0, Prev1, Prev2, Curr0, Curr1, Curr2, Next0, Next1, Next2;
reg [23:0] A, B, D, F, G, H;
reg  [7:0] pattern, nextpatt;
reg  [1:0] cyc;

reg  curbuf;
reg  prevbuf = 0;
wire iobuf = !curbuf;

wire diff0, diff1;
DiffCheck diffcheck0(
    clk, 
    Curr1, 
    (cyc == 0) ? Prev0 : 
        (cyc == 1) ? Curr0 : 
            (cyc == 2) ? Prev2 : Next1, 
    diff0
);
DiffCheck diffcheck1(
    clk, 
    Curr1, 
    (cyc == 0) ? Prev1 : 
        (cyc == 1) ? Next0 : 
            (cyc == 2) ? Curr2 : Next2, 
    diff1
);

wire [7:0] new_pattern = {diff1, diff0, pattern[7:2]};

wire [23:0] X = (cyc == 0) ? A : (cyc == 1) ? Prev1 : (cyc == 2) ? Next1 : G;
wire [23:0] blend_result_pre;
Blend blender(clk, hqTable[nextpatt], disable_hq2x, Curr0_d, X_d, B_d, D_d, F_d, H_d, blend_result_pre);
reg [23:0] Curr0_d, X_d, B_d, D_d, F_d, H_d;

delayline #(
    .WIDTH(6*24),
    .CYCLES(4)
) blender_queue (
    .clock(clk),
    .in({ Curr0, X, B, D, F, H }),
    .out({ Curr0_d, X_d, B_d, D_d, F_d, H_d })
);

wire [DWIDTH:0] Curr20tmp;
wire     [23:0] Curr20 = HALF_DEPTH ? h2rgb(Curr20tmp) : Curr20tmp;
wire [DWIDTH:0] Curr21tmp;
wire     [23:0] Curr21 = HALF_DEPTH ? h2rgb(Curr21tmp) : Curr21tmp;

reg  [AWIDTH:0] wrin_addr2;
reg  [DWIDTH:0] wrpix;
reg             wrin_en;

function [23:0] h2rgb;
    input [11:0] v;
begin
    h2rgb = mono ? {v[7:0], v[7:0], v[7:0]} : {v[11:8],v[11:8],v[7:4],v[7:4],v[3:0],v[3:0]};
end
endfunction

function [11:0] rgb2h;
    input [23:0] v;
begin
    rgb2h = mono ? {4'b0000, v[23:20], v[19:16]} : {v[23:20], v[15:12], v[7:4]};
end
endfunction

hq2x_in #(.LENGTH(LENGTH), .DWIDTH(DWIDTH)) hq2x_in
(
    .clk(clk),

    .rdaddr(offs),
    .rdbuf0(prevbuf),
    .rdbuf1(curbuf),
    .q0(Curr20tmp),
    .q1(Curr21tmp),

    .wraddr(wrin_addr2),
    .wrbuf(iobuf),
    .data(wrpix),
    .wren(wrin_en)
);

reg     [AWIDTH+1:0] read_x /*verilator public*/;
reg     [AWIDTH+1:0] wrout_addr, wrout_addr_delayed;
reg                  wrout_en, wrout_en_delayed;
reg  [DWIDTH1*4-1:0] wrdata, wrdata_pre;
wire [DWIDTH1*4-1:0] outpixel_x4;
reg  [DWIDTH1*2-1:0] outpixel_x2;

delayline #(
    .WIDTH(AWIDTH+1+1+1),
    .CYCLES(22)
) wrdl (
    .clock(clk),
    .in({ wrout_en, wrout_addr }),
    .out({ wrout_en_delayed, wrout_addr_delayed })
);

assign outpixel = read_x[0] ? outpixel_x2[DWIDTH1*2-1:DWIDTH1] : outpixel_x2[DWIDTH:0];

hq2x_buf #(.NUMWORDS(EXACT_BUFFER ? LENGTH : LENGTH*2), .AWIDTH(AWIDTH+1), .DWIDTH(DWIDTH1*4-1)) hq2x_out
(
    .clock(clk),

    .rdaddress(EXACT_BUFFER ? read_x[AWIDTH+1:1] : {read_x[AWIDTH+1:1],read_y[1]}),
    .q(outpixel_x4),

    .data(wrdata),
    .wraddress(wrout_addr_delayed),
    .wren(wrout_en_delayed)
);

wire [DWIDTH:0] blend_result = HALF_DEPTH ? rgb2h(blend_result_pre) : blend_result_pre[DWIDTH:0];

reg [AWIDTH:0] offs /*verilator public*/;
always @(posedge clk) begin
    reg old_reset_line;
    reg old_reset_frame;

    wrout_en <= 0;
    wrin_en  <= 0;

    if(ce_x4) begin

        pattern <= new_pattern;
        if(read_x[0]) outpixel_x2 <= read_y[0] ? outpixel_x4[DWIDTH1*4-1:DWIDTH1*2] : outpixel_x4[DWIDTH1*2-1:0];

        if(~&offs) begin
            if (cyc == 1) begin
                Prev2 <= Curr20;
                Curr2 <= Curr21;
                Next2 <= HALF_DEPTH ? h2rgb(inputpixel) : inputpixel;
                wrpix <= inputpixel;
                wrin_addr2 <= offs;
                wrin_en <= 1;
            end

            case({cyc[1],^cyc})
                0: wrdata[DWIDTH1+DWIDTH:DWIDTH1]     <= blend_result;
                1: wrdata[DWIDTH1*3+DWIDTH:DWIDTH1*3] <= blend_result;
                2: wrdata[DWIDTH:0]                   <= blend_result;
                3: wrdata[DWIDTH1*2+DWIDTH:DWIDTH1*2] <= blend_result;
            endcase

            if(cyc==3) begin
                offs <= offs + 1'd1;
                wrout_addr <= EXACT_BUFFER ? offs : {offs, curbuf};
                wrout_en <= 1;
            end
        end

        if(cyc==0) begin
            nextpatt <= {new_pattern[7:6], new_pattern[3], new_pattern[5], new_pattern[2], new_pattern[4], new_pattern[1:0]};
        end else begin
            nextpatt <= {nextpatt[5], nextpatt[3], nextpatt[0], nextpatt[6], nextpatt[1], nextpatt[7], nextpatt[4], nextpatt[2]};
        end

        if(cyc==3) begin
            {A, G} <= {Prev0, Next0};
            {B, F, H, D} <= {Prev1, Curr2, Next1, Curr0};
            {Prev0, Prev1} <= {Prev1, Prev2};
            {Curr0, Curr1} <= {Curr1, Curr2};
            {Next0, Next1} <= {Next1, Next2};
        end else begin
            {B, F, H, D} <= {F, H, D, B};
        end

        cyc <= cyc + 1'b1;
        if(old_reset_line && ~reset_line) begin
            old_reset_frame <= reset_frame;
            offs <= 0;
            cyc <= 0;
            curbuf <= ~curbuf;
            prevbuf <= curbuf;
            {Prev0, Prev1, Prev2, Curr0, Curr1, Curr2, Next0, Next1, Next2} <= '0;
            if(old_reset_frame & ~reset_frame) begin
                curbuf <= 0;
                prevbuf <= 0;
            end
        end
        
        if(~hblank & ~&read_x) read_x <= read_x + 1'd1;
        if(hblank) read_x <= 0;

        old_reset_line  <= reset_line;
    end
end

endmodule