`timescale 1 ps / 1 ps
module Sobel (
    input clk,
    input rst,
    output [DATA_WIDTH-1:0] data_fifo_out,
    output data_valid_fifo_out,
    input wire [FIFO_DEPTH_LOG2:0] usedw_fifo_out,    
    input wire [DATA_WIDTH-1:0] data_fifo_in,
    output read_fifo_in,
    input wire [FIFO_DEPTH_LOG2:0] usedw_fifo_in,
    input start,
    output endf);

    parameter DATA_WIDTH=32;
    parameter FIFO_DEPTH = 256;
    parameter FIFO_DEPTH_LOG2 = 8;

    Sobel_ram Sobel_ram_instance(
        .clock(clk),
        .data(cachepixels_w),
        .wraddress(add_cachepixels_w),
        .wren(wren_cachepixels),
        .rdaddress(add_cachepixels_r),
        .q(cachepixels_r)
    );
    
    reg [1:0] run;
    always @ (posedge clk or posedge rst) begin
        if (rst == 1) begin
            run <= 0;
        end else begin
            if (start == 1) begin
                run <= 1;
            end else begin
                if (pixel_counter == 0) begin
                    run <= 2;
                end
                if (endf == 1) begin
                    run <= 0;
                end
            end
        end
    end

    reg [18:0] pixel_counter;
    always @ (posedge clk) begin
        if (start == 1) begin
            pixel_counter <= 384000;
        end else begin
            if (data_valid_fifo_out) begin
                pixel_counter <= pixel_counter - 1;
            end
        end
    end


//**********************************************************************Cache Control*******************************************************************************
    wire [7:0] cachepixels_w;
    assign cachepixels_w = data_fifo_in[7:0];

    reg [9:0] free_col;
    reg [1:0] free_line;    
    always @(posedge clk) begin
        if (start == 1) begin
            free_col <= 0;
            free_line <= 0;
        end else begin
            if (read_fifo_in == 1) begin
                if (free_col == 799) begin
                    free_line <= free_line + 1;
                    free_col <= 0;
                end else begin
                    free_col <= free_col + 1;
                end
            end
        end
    end

    wire [11:0] add_cachepixels_w;
    assign add_cachepixels_w = (free_line * 800) + free_col;
    
    wire wren_cachepixels;
    assign wren_cachepixels = (read_fifo_in == 1);

    reg [2:0] free_cache_lines;
    always @(posedge clk) begin
        if (start == 1) begin
            free_cache_lines <= 4;
        end else begin
            if (dec_free_cache_lines == 1) begin
                free_cache_lines <= free_cache_lines - 1;
            end else begin
                if (inc_free_cache_lines == 1) begin
                    free_cache_lines <= free_cache_lines + 1;
                end
            end
        end
    end
    
    wire dec_free_cache_lines;        
    assign dec_free_cache_lines = (free_col == 799) & (read_fifo_in == 1);
    wire inc_free_cache_lines;
    assign inc_free_cache_lines = (sobel_col == 799) & (go_sobel == 1);

//------------------------------------------------------------------------------------------------------------------------------------------------------------------    


//*********************************************************************Sobel Implementation*************************************************************************

    reg [9:0] sobel_col;
    reg [1:0] sobel_line;
    reg [8:0] real_line;
    always @(posedge clk) begin
        if (start == 1) begin
            sobel_col <= 0;
            sobel_line <= 0;
            real_line <= 0;
        end else begin
            if (stage == 2) begin
                if (sobel_col == 799) begin
                    real_line <= real_line + 1;
                    sobel_line <= sobel_line + 1;
                    sobel_col <= 0;
                end else begin
                    sobel_col <= sobel_col + 1;
                end
            end
        end
    end

    wire go_sobel;    
    assign go_sobel = (free_cache_lines < 2) & (stage == 0) & (run == 1);

    wire [11:0] add_cachepixels_r;
    assign add_cachepixels_r = ((sobel_line + g_line) * 800) + sobel_col + g_col;
    
    reg [3:0] stage;
    wire mio;
    assign mio = (g_col == 1) & (g_line == 1) & (stage == 1);
    always @(posedge clk) begin
        if ((start == 1) | (stage == 5)) begin
            stage <= 0;
        end else begin
            if (go_sobel == 1) begin
                stage <= 1;
            end else begin
                if (mio == 1) begin
                    stage <= 2;
                end else begin
                    if (stage > 1) begin
                        stage <= stage + 1;
                    end
                end
            end
        end
    end

    reg [2:0] g_col;
    reg [2:0] g_line;    
    always @(posedge clk) begin
        if (start == 1) begin
            g_col <= -1;
            g_line <= -1;
        end else begin
            if (stage == 1) begin
                if (g_col == 1) begin
                    g_line <= g_line + 1;
                    g_col <= -1;
                end else begin
                    g_col <= g_col + 1;
                end
            end
            if (stage == 2) begin
                g_col <= -1;
                g_line <= -1;
            end
        end
    end    
    
    wire out_bound;
    assign out_bound = (sobel_col == 0) | (sobel_col == 800) | (real_line == 0) | (real_line == 479);

    reg [10:0] gx;
    reg [9:0] gx2;    
    reg [10:0] gy;
    reg [9:0] gy2;
    reg [7:0] sobel_r;
    reg [7:0] g[-1:1][-1:1];
    wire [7:0] cachepixels_r;
    always @(posedge clk) begin
        if (stage == 1) begin
            if (out_bound == 0) begin
                g[g_col][g_line] <= cachepixels_r;
            end else begin
                g[g_col][g_line] <= 0;
            end
        end
        if (stage == 2) begin            
            gx <= g[-1][-1] + g[-1][0] + g[-1][1];
            gx2 <= g[1][-1] + g[1][0] + g[1][1];
            gy <= g[-1][-1] + g[0][-1] + g[1][-1];
            gy2 <= g[-1][1] + g[0][1] + g[1][1];
        end
        if (stage == 3) begin
            gx <= (gx > gx2)? gx - gx2 : gx2 - gx; // I need the absolute value
            gy <= (gy > gy2)? gy - gy2 : gy2 - gy; // 
        end
        if (stage == 4) begin
            sobel_r <= ((gx + gy) > 255)? 255 : gx + gy; // Aprocimacion del gradiente The Pocket Handbook of Image Processing Algorithms In C Myler p.218
        end        
    end

//------------------------------------------------------------------------------------------------------------------------------------------------------------------

    assign read_fifo_in = (usedw_fifo_out < FIFO_DEPTH) & (usedw_fifo_in > 0) & (run == 1) & (free_cache_lines > 0);

    assign data_fifo_out = {8'd0, {3{sobel_r}}};
    assign data_valid_fifo_out = (run == 1) & (stage == 5);
    
    assign endf = (run == 2);
    
endmodule