module ultrasonic_cntr (
    input clk, reset_p,
    input ultra_echo,                           // Input Echo Pulse
    output reg ultra_trig,                      // Output Initiate Signal
    output reg [11:0] distance,                 // Distance Calculation
    output [15:0] led                           // for Debugging, State Output
    );
    
    // Change State Using Shift
    localparam S_IDLE   = 5'b0_0001;            // Standby State
    localparam S_TIRG_H = 5'b0_0010;            // Trig Pin High Signal Transmission
    localparam S_TIRG_L = 5'b0_0100;            // Trig Pin Low Signal Transmission
    localparam S_ECHO_H = 5'b0_1000;            // Echo Pin High Signal Reception
    localparam S_ECHO_L = 5'b1_0000;            // Echo Pin Low Signal Reception & Distance Calculation

    // Clock Divide 100, 10ns x 100 = 1us
    wire clk_usec_nedge;                        // Divide Clock 1us
    clock_div_100 us_clk (.clk(clk), .reset_p(reset_p), .nedge_div_100(clk_usec_nedge));

    // us Unit Count
    reg [21:0] cnt_usec;                        // us Count
    reg cnt_usec_e;                             // us Count Enable
    always @(negedge clk, posedge reset_p) begin
        if (reset_p) cnt_usec = 0;              // Count Clear
        else if (clk_usec_nedge && cnt_usec_e) begin    // Count Start when Enable & us Negative Edge
            cnt_usec = cnt_usec + 1;            // Count During Enable
        end
        else if (!cnt_usec_e) cnt_usec = 0;     // Count Clear when Disable
    end

    reg [21:0] div_usec_58;                     // Count for Division by 58
    reg div_usec_58_e;                          // Count Enable
    reg [11:0] cnt_dist;                        // Distance Count
    always @(negedge clk, posedge reset_p) begin
        if (reset_p) begin
            div_usec_58 = 0;                    // Count Reset
            cnt_dist = 0;                       // Distance Count Reset
        end
        else if (clk_usec_nedge && div_usec_58_e) begin    // Count Start when Enable & us Negative Edge
            if (div_usec_58 >= 57) begin        // When 58us
                div_usec_58 = 0;                // Count Reset
                cnt_dist = cnt_dist + 1;        // Distance Increase
            end
            else div_usec_58 = div_usec_58 + 1; // Count During Enable
        end
        else if (!div_usec_58_e) begin
            div_usec_58 = 0;                    // Count Reset when Disable
            cnt_dist = 0;                       // Distance Reset when Disable
        end
    end

    // Edge Detection of Ultrasonic Echo Signal
    wire ultra_nedge, ultra_pedge;
    edge_detector_pos btn_ed (.clk(clk), .reset_p(reset_p),
        .cp(ultra_echo), .p_edge(ultra_pedge), .n_edge(ultra_nedge));

    reg [4:0] state, next_state;                // Current & Next State
    assign led [4:0] = state;                   // State LED Output
    assign led [5] = ultra_trig;                // Trig Signal LED Output
    assign led [6] = ultra_echo;                // Echo Signal LED Output
    // Change State in Negative Edge
    always @(negedge clk, posedge reset_p) begin
        if (reset_p) state = S_IDLE;            // Basic Standby State
        else state = next_state;                // Change State in Negative Edge
    end

    // reg [14:0] echo_time;                       // Echo Pulse Width
    always @(posedge clk, posedge reset_p) begin
        if (reset_p) begin
            next_state = S_IDLE;                // Basic Standby State
        end
        else begin
            case (state)
                S_IDLE   : begin                // Standby State
                    if (cnt_usec < 22'd200_000) begin   // Real 200_000, Test 1_000
                        cnt_usec_e = 1;         // us Count Enable
                    end
                    else begin
                        cnt_usec_e = 0;         // us Count Disable, Clear
                        next_state = S_TIRG_H;  // Change State S_TIRG_H
                    end
                end
                S_TIRG_H : begin                // Trig Pin High Signal Transmission
                    if (cnt_usec < 22'd10) begin    // Hold High Signal for 10us
                        cnt_usec_e = 1;         // us Count Enable
                        ultra_trig = 1;         // Trig Pin High Signal
                    end
                    else begin
                        cnt_usec_e = 0;         // us Count Disable, Clear
                        next_state = S_TIRG_L;  // Change State S_TIRG_L
                    end
                end
                S_TIRG_L : begin                // Trig Pin Low Signal Transmission
                    ultra_trig = 0;             // Trig Pin Low Signal
                    cnt_usec_e = 1;             // Count for Checking Response Time
                    if (cnt_usec > 22'd200_000) begin   // No Response 200ms, Error, Etc..
                        cnt_usec_e = 0;         // us Count Disable, Clear
                        next_state = S_IDLE;    // Change State S_IDLE
                    end
                    if (ultra_pedge) begin      // Check Echo High Signal
                        cnt_usec_e = 0;         // us Count Disable, Clear
                        div_usec_58_e = 1;      // Enable 58us Count
                        next_state = S_ECHO_H;  // Change State S_ECHO_H
                    end
                end
                S_ECHO_H : begin                // Echo Pin High Signal Reception
                    cnt_usec_e = 1;             // us Count Enable
                    if (cnt_usec > 22'd25_000) begin   // No Response 25ms, Error, Etc..
                        cnt_usec_e = 0;         // us Count Disable, Clear
                        div_usec_58_e = 0;      // 
                        next_state = S_IDLE;    // Change State S_IDLE
                    end
                    if (ultra_nedge) begin      // Check Echo Low Signal
                        // echo_time = cnt_usec;   // Echo Pulse Width Record, us
                        distance = cnt_dist;    // Distance Record
                        next_state = S_ECHO_L;  // Change State S_ECHO_L
                    end
                end
                S_ECHO_L : begin                // Echo Pin Low Signal Reception
                    cnt_usec_e = 0;             // us Count Disable, Clear
                    div_usec_58_e = 0;          // 58us Count Disable, Clear
                    // distance = echo_time / 58;  // Distance Calculation
                    next_state = S_IDLE;        // Change State S_IDLE
                end
                default  : next_state = S_IDLE; // Basic Stanby State
            endcase
        end
    end
endmodule