module voice_led_top(
    input  wire clk_25mhz,
    input  wire mic_sd,
    output reg  mic_bclk = 1'b0,
    output reg  mic_ws   = 1'b0,
    output wire led_run,
    output wire led_stop,
    output wire led_activity
);

    reg [3:0] bclk_div = 4'd0;
    reg       bclk_prev = 1'b0;
    reg [5:0] bit_count = 6'd0;
    reg [5:0] slot_bit_index = 6'd0;
    reg [31:0] shift_reg = 32'd0;
    reg [23:0] sample_left = 24'd0;
    reg        sample_strobe = 1'b0;

    reg [31:0] envelope = 32'd0;
    reg        activity = 1'b0;
    reg [15:0] burst_count = 16'd0;
    reg [15:0] holdoff_count = 16'd0;
    reg        run_state = 1'b0;

    wire bclk_rise;
    wire signed [23:0] signed_sample;
    wire [23:0] abs_sample;
    wire [31:0] envelope_next;

    localparam [31:0] ENV_THRESHOLD      = 32'd200000;
    localparam [15:0] BURST_MIN_SAMPLES  = 16'd1200;
    localparam [15:0] BURST_MAX_SAMPLES  = 16'd9000;
    localparam [15:0] HOLDOFF_SAMPLES    = 16'd18000;

    assign bclk_rise = (bclk_prev == 1'b0) && (mic_bclk == 1'b1);
    assign signed_sample = sample_left;
    assign abs_sample = signed_sample[23] ? (~signed_sample + 24'd1) : signed_sample;
    assign envelope_next = envelope - (envelope >> 4) + {8'd0, abs_sample};

    always @(posedge clk_25mhz) begin
        bclk_prev <= mic_bclk;

        if (bclk_div == 4'd7) begin
            bclk_div <= 4'd0;
            mic_bclk <= ~mic_bclk;
        end else begin
            bclk_div <= bclk_div + 4'd1;
        end
    end

    always @(posedge clk_25mhz) begin
        sample_strobe <= 1'b0;

        if (bclk_rise) begin
            if (bit_count == 6'd63) begin
                bit_count <= 6'd0;
            end else begin
                bit_count <= bit_count + 6'd1;
            end

            if (bit_count == 6'd31) begin
                mic_ws <= 1'b1;
            end else if (bit_count == 6'd63) begin
                mic_ws <= 1'b0;
            end

            if (bit_count == 6'd31 || bit_count == 6'd63) begin
                slot_bit_index <= 6'd0;
            end else begin
                slot_bit_index <= slot_bit_index + 6'd1;
            end

            shift_reg <= {shift_reg[30:0], mic_sd};

            if (mic_ws == 1'b0 && slot_bit_index == 6'd23) begin
                sample_left <= {shift_reg[22:0], mic_sd};
                sample_strobe <= 1'b1;
            end
        end
    end

    always @(posedge clk_25mhz) begin
        if (sample_strobe) begin
            envelope <= envelope_next;
            activity <= (envelope_next > ENV_THRESHOLD);

            if (holdoff_count != 16'd0) begin
                holdoff_count <= holdoff_count - 16'd1;
                burst_count <= 16'd0;
            end else begin
                if (envelope_next > ENV_THRESHOLD) begin
                    if (burst_count != 16'hFFFF) begin
                        burst_count <= burst_count + 16'd1;
                    end
                end else begin
                    if (burst_count >= BURST_MIN_SAMPLES &&
                        burst_count <= BURST_MAX_SAMPLES) begin
                        run_state <= ~run_state;
                        holdoff_count <= HOLDOFF_SAMPLES;
                    end
                    burst_count <= 16'd0;
                end
            end
        end
    end

    assign led_run = run_state;
    assign led_stop = ~run_state;
    assign led_activity = activity;

endmodule

`timescale 1ns/1ps

module tb_voice_led_top;

    reg clk_25mhz = 1'b0;
    reg mic_sd = 1'b0;
    wire mic_bclk;
    wire mic_ws;
    wire led_run;
    wire led_stop;
    wire led_activity;

    integer i;
    integer k;
    reg [31:0] slot_word;

    voice_led_top dut (
        .clk_25mhz(clk_25mhz),
        .mic_sd(mic_sd),
        .mic_bclk(mic_bclk),
        .mic_ws(mic_ws),
        .led_run(led_run),
        .led_stop(led_stop),
        .led_activity(led_activity)
    );

    always #20 clk_25mhz = ~clk_25mhz;

    task send_i2s_left_sample;
        input [23:0] s;
        begin
            while (mic_ws !== 1'b0) begin
                @(posedge mic_bclk);
            end

            slot_word = {s, 8'h00};

            for (i = 31; i >= 0; i = i - 1) begin
                @(negedge mic_bclk);
                mic_sd = slot_word[i];
            end

            while (mic_ws !== 1'b1) begin
                @(posedge mic_bclk);
            end

            for (i = 31; i >= 0; i = i - 1) begin
                @(negedge mic_bclk);
                mic_sd = 1'b0;
            end
        end
    endtask

    task send_silence;
        input integer n;
        begin
            for (k = 0; k < n; k = k + 1) begin
                send_i2s_left_sample(24'd0);
            end
        end
    endtask

    task send_burst;
        input integer n;
        begin
            for (k = 0; k < n; k = k + 1) begin
                if (k[0]) begin
                    send_i2s_left_sample(24'h180000);
                end else begin
                    send_i2s_left_sample(24'hE80000);
                end
            end
        end
    endtask

    initial begin
        $display("Starting simulation");

        send_silence(3000);
        $display("Initial state: led_run=%0d led_stop=%0d led_activity=%0d",
                 led_run, led_stop, led_activity);

        send_burst(2000);
        send_silence(3000);
        $display("After burst 1: led_run=%0d led_stop=%0d led_activity=%0d",
                 led_run, led_stop, led_activity);

        send_burst(1500);
        send_silence(4000);
        $display("After burst 2 during holdoff: led_run=%0d led_stop=%0d led_activity=%0d",
                 led_run, led_stop, led_activity);

        send_silence(20000);
        send_burst(2000);
        send_silence(3000);
        $display("After burst 3: led_run=%0d led_stop=%0d led_activity=%0d",
                 led_run, led_stop, led_activity);

        $finish;
    end

endmodule

module gps_uart_rx #(
    parameter integer CLK_HZ = 25000000,
    parameter integer BAUD   = 9600
) (
    input  wire clk,
    input  wire rst,
    input  wire rx,
    output reg  data_valid,
    output reg [7:0] data_byte
);

    localparam integer CLKS_PER_BIT = CLK_HZ / BAUD;
    localparam integer HALF_BIT     = CLKS_PER_BIT / 2;

    reg [15:0] clk_count = 0;
    reg [3:0]  bit_index = 0;
    reg [7:0]  rx_shift  = 8'h00;
    reg [2:0]  state     = 0;
    reg        rx_meta   = 1'b1;
    reg        rx_sync   = 1'b1;

    localparam S_IDLE  = 3'd0;
    localparam S_START = 3'd1;
    localparam S_DATA  = 3'd2;
    localparam S_STOP  = 3'd3;

    always @(posedge clk) begin
        rx_meta <= rx;
        rx_sync <= rx_meta;
    end

    always @(posedge clk) begin
        if (rst) begin
            state      <= S_IDLE;
            clk_count  <= 0;
            bit_index  <= 0;
            rx_shift   <= 8'h00;
            data_byte  <= 8'h00;
            data_valid <= 1'b0;
        end else begin
            data_valid <= 1'b0;

            case (state)
                S_IDLE: begin
                    clk_count <= 0;
                    bit_index <= 0;
                    if (rx_sync == 1'b0) begin
                        state <= S_START;
                    end
                end

                S_START: begin
                    if (clk_count == HALF_BIT) begin
                        if (rx_sync == 1'b0) begin
                            clk_count <= 0;
                            state <= S_DATA;
                        end else begin
                            state <= S_IDLE;
                        end
                    end else begin
                        clk_count <= clk_count + 16'd1;
                    end
                end

                S_DATA: begin
                    if (clk_count == CLKS_PER_BIT - 1) begin
                        clk_count <= 0;
                        rx_shift[bit_index] <= rx_sync;
                        if (bit_index == 4'd7) begin
                            bit_index <= 0;
                            state <= S_STOP;
                        end else begin
                            bit_index <= bit_index + 4'd1;
                        end
                    end else begin
                        clk_count <= clk_count + 16'd1;
                    end
                end

                S_STOP: begin
                    if (clk_count == CLKS_PER_BIT - 1) begin
                        clk_count <= 0;
                        if (rx_sync == 1'b1) begin
                            data_byte <= rx_shift;
                            data_valid <= 1'b1;
                        end
                        state <= S_IDLE;
                    end else begin
                        clk_count <= clk_count + 16'd1;
                    end
                end

                default: begin
                    state <= S_IDLE;
                end
            endcase
        end
    end
endmodule

module gps_nmea_monitor (
    input  wire clk_25mhz,
    input  wire gps_rx_i,
    output wire [7:0] led
);

    wire rx_valid;
    wire [7:0] rx_byte;

    reg rst = 1'b0;

    gps_uart_rx #(
        .CLK_HZ(25000000),
        .BAUD(9600)
    ) u_rx (
        .clk(clk_25mhz),
        .rst(rst),
        .rx(gps_rx_i),
        .data_valid(rx_valid),
        .data_byte(rx_byte)
    );

    reg [23:0] hb_counter = 24'd0;
    reg hb_led = 1'b0;

    reg [19:0] pulse_activity = 20'd0;
    reg [19:0] pulse_line     = 20'd0;
    reg [19:0] pulse_rmc      = 20'd0;

    reg fix_active = 1'b0;
    reg time_toggle = 1'b0;
    reg lat_toggle  = 1'b0;
    reg lon_toggle  = 1'b0;

    reg [7:0] line_pos = 8'd0;
    reg [7:0] field_pos = 8'd0;

    reg in_line = 1'b0;
    reg candidate_rmc = 1'b0;
    reg rmc_seen_this_line = 1'b0;

    reg [7:0] id_buf [0:4];
    reg [7:0] field_buf [0:15];
    reg [4:0] field_len = 5'd0;

    integer i;

    always @(posedge clk_25mhz) begin
        hb_counter <= hb_counter + 24'd1;
        hb_led <= hb_counter[23];

        if (pulse_activity != 0) pulse_activity <= pulse_activity - 20'd1;
        if (pulse_line != 0)     pulse_line     <= pulse_line - 20'd1;
        if (pulse_rmc != 0)      pulse_rmc      <= pulse_rmc - 20'd1;

        if (rx_valid) begin
            pulse_activity <= 20'd500000;

            if (rx_byte == "$") begin
                in_line <= 1'b1;
                line_pos <= 8'd0;
                field_pos <= 8'd0;
                field_len <= 5'd0;
                candidate_rmc <= 1'b0;
                rmc_seen_this_line <= 1'b0;
                fix_active <= fix_active;
            end else if (in_line) begin
                if (rx_byte == 8'h0D) begin
                    in_line <= 1'b1;
                end else if (rx_byte == 8'h0A) begin
                    pulse_line <= 20'd500000;
                    if (rmc_seen_this_line) begin
                        pulse_rmc <= 20'd500000;
                    end
                    in_line <= 1'b0;
                end else if (rx_byte == ",") begin
                    if (field_pos == 8'd0) begin
                        if ((id_buf[0] == "G") &&
                            (id_buf[1] == "P" || id_buf[1] == "N") &&
                            (id_buf[2] == "R") &&
                            (id_buf[3] == "M") &&
                            (id_buf[4] == "C")) begin
                            candidate_rmc <= 1'b1;
                            rmc_seen_this_line <= 1'b1;
                        end
                    end else if (candidate_rmc) begin
                        if (field_pos == 8'd1 && field_len != 0) begin
                            time_toggle <= ~time_toggle;
                        end
                        if (field_pos == 8'd2 && field_len != 0) begin
                            if (field_buf[0] == "A")
                                fix_active <= 1'b1;
                            else
                                fix_active <= 1'b0;
                        end
                        if (field_pos == 8'd3 && field_len != 0) begin
                            lat_toggle <= ~lat_toggle;
                        end
                        if (field_pos == 8'd5 && field_len != 0) begin
                            lon_toggle <= ~lon_toggle;
                        end
                    end

                    field_pos <= field_pos + 8'd1;
                    field_len <= 5'd0;
                end else if (rx_byte == "*") begin
                    if (candidate_rmc) begin
                        if (field_pos == 8'd1 && field_len != 0) begin
                            time_toggle <= ~time_toggle;
                        end
                        if (field_pos == 8'd2 && field_len != 0) begin
                            if (field_buf[0] == "A")
                                fix_active <= 1'b1;
                            else
                                fix_active <= 1'b0;
                        end
                        if (field_pos == 8'd3 && field_len != 0) begin
                            lat_toggle <= ~lat_toggle;
                        end
                        if (field_pos == 8'd5 && field_len != 0) begin
                            lon_toggle <= ~lon_toggle;
                        end
                    end
                end else begin
                    if (field_pos == 8'd0) begin
                        if (line_pos < 8'd5) begin
                            id_buf[line_pos] <= rx_byte;
                        end
                        line_pos <= line_pos + 8'd1;
                    end else begin
                        if (field_len < 5'd16) begin
                            field_buf[field_len] <= rx_byte;
                            field_len <= field_len + 5'd1;
                        end
                    end
                end
            end
        end
    end

    assign led[0] = hb_led;
    assign led[1] = (pulse_activity != 0);
    assign led[2] = (pulse_line != 0);
    assign led[3] = (pulse_rmc != 0);
    assign led[4] = fix_active;
    assign led[5] = time_toggle;
    assign led[6] = lat_toggle;
    assign led[7] = lon_toggle;

endmodule

`timescale 1ns/1ps

module tb_gps_nmea_monitor;

    reg clk = 1'b0;
    reg gps_rx_i = 1'b1;
    wire [7:0] led;

    gps_nmea_monitor dut (
        .clk_25mhz(clk),
        .gps_rx_i(gps_rx_i),
        .led(led)
    );

    always #20 clk = ~clk; // 25 MHz

    localparam integer BIT_NS = 104166; // approx 9600 baud

    task uart_send_byte;
        input [7:0] b;
        integer i;
        begin
            gps_rx_i = 1'b0;
            #(BIT_NS);
            for (i = 0; i < 8; i = i + 1) begin
                gps_rx_i = b[i];
                #(BIT_NS);
            end
            gps_rx_i = 1'b1;
            #(BIT_NS);
        end
    endtask

    task uart_send_string;
        input [8*96-1:0] s;
        integer i;
        reg [7:0] ch;
        begin
            for (i = 95; i >= 0; i = i - 1) begin
                ch = s[i*8 +: 8];
                if (ch != 8'h00)
                    uart_send_byte(ch);
            end
        end
    endtask

    initial begin
        #(1000000);

        uart_send_string({
            "$GPRMC,123519,V,4807.038,N,01131.000,E,0.0,0.0,230394,003.1,W*53",
            8'h0D, 8'h0A
        });

        #(2000000);

        uart_send_string({
            "$GPRMC,123520,A,4807.038,N,01131.000,E,0.1,0.0,230394,003.1,W*52",
            8'h0D, 8'h0A
        });

        #(5000000);

        $display("LED state = %b", led);
        if (led[4] !== 1'b1) begin
            $display("ERROR: fix_active LED did not assert");
            $fatal;
        end

        $display("PASS: RMC monitor parsed active fix.");
        $finish;
    end

endmodule

module uart_rx #(
    parameter integer CLK_HZ = 25000000,
    parameter integer BAUD   = 115200
)(
    input  wire clk,
    input  wire rst,
    input  wire rx,
    output reg  [7:0] data,
    output reg  valid,
    output reg  framing_error
);
    localparam integer CLKS_PER_BIT  = CLK_HZ / BAUD;
    localparam integer HALF_BIT_CLKS = CLKS_PER_BIT / 2;

    reg rx_sync_0;
    reg rx_sync_1;
    reg [15:0] clk_count;
    reg [3:0] bit_index;
    reg [7:0] rx_shift;
    reg [1:0] state;

    localparam [1:0] S_IDLE  = 2'd0;
    localparam [1:0] S_START = 2'd1;
    localparam [1:0] S_DATA  = 2'd2;
    localparam [1:0] S_STOP  = 2'd3;

    always @(posedge clk) begin
        if (rst) begin
            rx_sync_0 <= 1'b1;
            rx_sync_1 <= 1'b1;
        end else begin
            rx_sync_0 <= rx;
            rx_sync_1 <= rx_sync_0;
        end
    end

    always @(posedge clk) begin
        if (rst) begin
            data <= 8'h00;
            valid <= 1'b0;
            framing_error <= 1'b0;
            clk_count <= 16'd0;
            bit_index <= 4'd0;
            rx_shift <= 8'h00;
            state <= S_IDLE;
        end else begin
            valid <= 1'b0;

            case (state)
                S_IDLE: begin
                    framing_error <= 1'b0;
                    clk_count <= 16'd0;
                    bit_index <= 4'd0;
                    if (rx_sync_1 == 1'b0) begin
                        state <= S_START;
                    end
                end

                S_START: begin
                    if (clk_count == HALF_BIT_CLKS - 1) begin
                        clk_count <= 16'd0;
                        if (rx_sync_1 == 1'b0) begin
                            state <= S_DATA;
                        end else begin
                            state <= S_IDLE;
                        end
                    end else begin
                        clk_count <= clk_count + 16'd1;
                    end
                end

                S_DATA: begin
                    if (clk_count == CLKS_PER_BIT - 1) begin
                        clk_count <= 16'd0;
                        rx_shift[bit_index] <= rx_sync_1;
                        if (bit_index == 4'd7) begin
                            bit_index <= 4'd0;
                            state <= S_STOP;
                        end else begin
                            bit_index <= bit_index + 4'd1;
                        end
                    end else begin
                        clk_count <= clk_count + 16'd1;
                    end
                end

                S_STOP: begin
                    if (clk_count == CLKS_PER_BIT - 1) begin
                        clk_count <= 16'd0;
                        data <= rx_shift;
                        valid <= 1'b1;
                        framing_error <= (rx_sync_1 != 1'b1);
                        state <= S_IDLE;
                    end else begin
                        clk_count <= clk_count + 16'd1;
                    end
                end

                default: begin
                    state <= S_IDLE;
                end
            endcase
        end
    end
endmodule

module uart_tx #(
    parameter integer CLK_HZ = 25000000,
    parameter integer BAUD   = 115200
)(
    input  wire clk,
    input  wire rst,
    input  wire [7:0] data,
    input  wire start,
    output reg  tx,
    output reg  busy
);
    localparam integer CLKS_PER_BIT = CLK_HZ / BAUD;

    reg [15:0] clk_count;
    reg [3:0] bit_index;
    reg [9:0] shifter;

    always @(posedge clk) begin
        if (rst) begin
            tx <= 1'b1;
            busy <= 1'b0;
            clk_count <= 16'd0;
            bit_index <= 4'd0;
            shifter <= 10'b1111111111;
        end else begin
            if (!busy) begin
                tx <= 1'b1;
                clk_count <= 16'd0;
                bit_index <= 4'd0;
                if (start) begin
                    shifter <= {1'b1, data, 1'b0};
                    busy <= 1'b1;
                    tx <= 1'b0;
                end
            end else begin
                if (clk_count == CLKS_PER_BIT - 1) begin
                    clk_count <= 16'd0;
                    bit_index <= bit_index + 4'd1;
                    shifter <= {1'b1, shifter[9:1]};
                    tx <= shifter[1];
                    if (bit_index == 4'd9) begin
                        busy <= 1'b0;
                        tx <= 1'b1;
                    end
                end else begin
                    clk_count <= clk_count + 16'd1;
                end
            end
        end
    end
endmodule

module uart_monitor_top(
    input  wire clk_25mhz,
    input  wire btn_rst,
    input  wire mon_rx,
    output wire host_tx,
    output reg  led0
);
    wire rst;
    wire [7:0] rx_data;
    wire rx_valid;
    wire rx_ferr;

    reg [7:0] tx_data;
    reg tx_start;
    wire tx_busy;

    reg [7:0] msg_mem [0:17];
    reg [4:0] msg_len;
    reg [4:0] msg_idx;
    reg sending;
    reg [23:0] led_count;
    integer i;

    assign rst = btn_rst;

    uart_rx #(
        .CLK_HZ(25000000),
        .BAUD(115200)
    ) u_rx (
        .clk(clk_25mhz),
        .rst(rst),
        .rx(mon_rx),
        .data(rx_data),
        .valid(rx_valid),
        .framing_error(rx_ferr)
    );

    uart_tx #(
        .CLK_HZ(25000000),
        .BAUD(115200)
    ) u_tx (
        .clk(clk_25mhz),
        .rst(rst),
        .data(tx_data),
        .start(tx_start),
        .tx(host_tx),
        .busy(tx_busy)
    );

    function [7:0] hexchar;
        input [3:0] nib;
        begin
            if (nib < 4'd10) begin
                hexchar = 8'h30 + {4'b0000, nib};
            end else begin
                hexchar = 8'h41 + ({4'b0000, nib} - 8'd10);
            end
        end
    endfunction

    always @(posedge clk_25mhz) begin
        if (rst) begin
            tx_data <= 8'h00;
            tx_start <= 1'b0;
            msg_len <= 5'd0;
            msg_idx <= 5'd0;
            sending <= 1'b0;
            led0 <= 1'b0;
            led_count <= 24'd0;
            for (i = 0; i < 18; i = i + 1) begin
                msg_mem[i] <= 8'h20;
            end
        end else begin
            tx_start <= 1'b0;

            if (led_count != 24'd0) begin
                led_count <= led_count - 24'd1;
                led0 <= 1'b1;
            end else begin
                led0 <= 1'b0;
            end

            if (rx_valid && !sending) begin
                led_count <= 24'd5000000;

                msg_mem[0] <= "R";
                msg_mem[1] <= "X";
                msg_mem[2] <= " ";
                msg_mem[3] <= hexchar(rx_data[7:4]);
                msg_mem[4] <= hexchar(rx_data[3:0]);
                msg_mem[5] <= " ";

                if (!rx_ferr) begin
                    msg_mem[6] <= "O";
                    msg_mem[7] <= "K";
                    msg_mem[8] <= 8'h0A;
                    msg_len <= 5'd9;
                end else begin
                    msg_mem[6]  <= "F";
                    msg_mem[7]  <= "R";
                    msg_mem[8]  <= "A";
                    msg_mem[9]  <= "M";
                    msg_mem[10] <= "I";
                    msg_mem[11] <= "N";
                    msg_mem[12] <= "G";
                    msg_mem[13] <= "_";
                    msg_mem[14] <= "E";
                    msg_mem[15] <= "R";
                    msg_mem[16] <= "R";
                    msg_mem[17] <= 8'h0A;
                    msg_len <= 5'd18;
                end

                msg_idx <= 5'd0;
                sending <= 1'b1;
            end

            if (sending && !tx_busy) begin
                if (msg_idx < msg_len) begin
                    tx_data <= msg_mem[msg_idx];
                    tx_start <= 1'b1;
                    msg_idx <= msg_idx + 5'd1;
                end else begin
                    sending <= 1'b0;
                end
            end
        end
    end
endmodule

`timescale 1ns/1ps

module tb_uart_monitor_top;
    reg clk;
    reg btn_rst;
    reg mon_rx;
    wire host_tx;
    wire led0;

    localparam integer CLK_HALF_NS = 20;
    localparam integer BIT_NS = 8680;

    integer fd;
    integer i;
    reg [9:0] frame;

    uart_monitor_top dut (
        .clk_25mhz(clk),
        .btn_rst(btn_rst),
        .mon_rx(mon_rx),
        .host_tx(host_tx),
        .led0(led0)
    );

    always #CLK_HALF_NS clk = ~clk;

    task uart_send_byte;
        input [7:0] b;
        integer j;
        begin
            mon_rx = 1'b0;
            #(BIT_NS);
            for (j = 0; j < 8; j = j + 1) begin
                mon_rx = b[j];
                #(BIT_NS);
            end
            mon_rx = 1'b1;
            #(BIT_NS);
        end
    endtask

    initial begin
        clk = 1'b0;
        btn_rst = 1'b1;
        mon_rx = 1'b1;
        fd = $fopen("sim_host_tx_bits.txt", "w");

        #500;
        btn_rst = 1'b0;

        #(BIT_NS * 3);
        uart_send_byte(8'h48);
        #(BIT_NS * 2);
        uart_send_byte(8'h45);
        #(BIT_NS * 2);
        uart_send_byte(8'h4C);

        #(BIT_NS * 250);
        $fclose(fd);
        $finish;
    end

    initial begin
        forever begin
            @(negedge host_tx);
            #(BIT_NS/2);
            frame[0] = host_tx;
            for (i = 1; i < 10; i = i + 1) begin
                #(BIT_NS);
                frame[i] = host_tx;
            end
            $fwrite(fd, "frame bits: %b\n", frame);
        end
    end
endmodule

`timescale 1ns/1ps

module servo_tester #(
    parameter integer CLK_HZ = 25000000,
    parameter integer FRAME_HZ = 50,
    parameter integer PULSE_MIN_US = 1000,
    parameter integer PULSE_CENTER_US = 1500,
    parameter integer PULSE_MAX_US = 2000,
    parameter integer SWEEP_STEP_US = 10,
    parameter integer SWEEP_UPDATE_MS = 20
) (
    input  wire clk,
    input  wire btn_center,
    input  wire btn_min,
    input  wire btn_max,
    input  wire btn_sweep,
    output reg  servo_pwm
);

    localparam integer US_TICKS            = CLK_HZ / 1000000;
    localparam integer FRAME_TICKS         = CLK_HZ / FRAME_HZ;
    localparam integer PULSE_MIN_TICKS     = PULSE_MIN_US * US_TICKS;
    localparam integer PULSE_CENTER_TICKS  = PULSE_CENTER_US * US_TICKS;
    localparam integer PULSE_MAX_TICKS     = PULSE_MAX_US * US_TICKS;
    localparam integer SWEEP_STEP_TICKS    = SWEEP_STEP_US * US_TICKS;
    localparam integer SWEEP_UPDATE_TICKS  = (CLK_HZ / 1000) * SWEEP_UPDATE_MS;

    reg [31:0] frame_counter = 32'd0;
    reg [31:0] pulse_ticks   = PULSE_CENTER_TICKS;
    reg [31:0] sweep_counter = 32'd0;
    reg [31:0] sweep_ticks   = PULSE_CENTER_TICKS;
    reg        sweep_dir_up  = 1'b1;

    always @(posedge clk) begin
        if (btn_min) begin
            pulse_ticks   <= PULSE_MIN_TICKS;
            sweep_counter <= 32'd0;
            sweep_ticks   <= PULSE_CENTER_TICKS;
            sweep_dir_up  <= 1'b1;
        end else if (btn_center) begin
            pulse_ticks   <= PULSE_CENTER_TICKS;
            sweep_counter <= 32'd0;
            sweep_ticks   <= PULSE_CENTER_TICKS;
            sweep_dir_up  <= 1'b1;
        end else if (btn_max) begin
            pulse_ticks   <= PULSE_MAX_TICKS;
            sweep_counter <= 32'd0;
            sweep_ticks   <= PULSE_CENTER_TICKS;
            sweep_dir_up  <= 1'b1;
        end else if (btn_sweep) begin
            pulse_ticks <= sweep_ticks;

            if (sweep_counter >= (SWEEP_UPDATE_TICKS - 1)) begin
                sweep_counter <= 32'd0;

                if (sweep_dir_up) begin
                    if (sweep_ticks >= (PULSE_MAX_TICKS - SWEEP_STEP_TICKS)) begin
                        sweep_ticks  <= PULSE_MAX_TICKS;
                        sweep_dir_up <= 1'b0;
                    end else begin
                        sweep_ticks <= sweep_ticks + SWEEP_STEP_TICKS;
                    end
                end else begin
                    if (sweep_ticks <= (PULSE_MIN_TICKS + SWEEP_STEP_TICKS)) begin
                        sweep_ticks  <= PULSE_MIN_TICKS;
                        sweep_dir_up <= 1'b1;
                    end else begin
                        sweep_ticks <= sweep_ticks - SWEEP_STEP_TICKS;
                    end
                end
            end else begin
                sweep_counter <= sweep_counter + 32'd1;
            end
        end else begin
            pulse_ticks   <= PULSE_CENTER_TICKS;
            sweep_counter <= 32'd0;
            sweep_ticks   <= PULSE_CENTER_TICKS;
            sweep_dir_up  <= 1'b1;
        end

        if (frame_counter >= (FRAME_TICKS - 1)) begin
            frame_counter <= 32'd0;
        end else begin
            frame_counter <= frame_counter + 32'd1;
        end

        if (frame_counter < pulse_ticks) begin
            servo_pwm <= 1'b1;
        end else begin
            servo_pwm <= 1'b0;
        end
    end

endmodule

`timescale 1ns/1ps

module servo_tester_tb;

    reg clk = 1'b0;
    reg btn_center = 1'b0;
    reg btn_min = 1'b0;
    reg btn_max = 1'b0;
    reg btn_sweep = 1'b0;
    wire servo_pwm;

    integer high_count;
    integer i;

    servo_tester #(
        .CLK_HZ(1000000),
        .FRAME_HZ(50),
        .PULSE_MIN_US(1000),
        .PULSE_CENTER_US(1500),
        .PULSE_MAX_US(2000),
        .SWEEP_STEP_US(100),
        .SWEEP_UPDATE_MS(20)
    ) dut (
        .clk(clk),
        .btn_center(btn_center),
        .btn_min(btn_min),
        .btn_max(btn_max),
        .btn_sweep(btn_sweep),
        .servo_pwm(servo_pwm)
    );

    always #500 clk = ~clk;

    task automatic measure_one_frame;
        begin
            while (servo_pwm !== 1'b1) begin
                @(posedge clk);
            end

            high_count = 0;
            while (servo_pwm === 1'b1) begin
                @(posedge clk);
                high_count = high_count + 1;
            end

            $display("Measured high ticks: %0d", high_count);
        end
    endtask

    initial begin
        $display("Starting servo_tester_tb");

        btn_center = 1'b1;
        repeat (3) begin
            measure_one_frame();
        end
        btn_center = 1'b0;

        btn_min = 1'b1;
        repeat (3) begin
            measure_one_frame();
        end
        btn_min = 1'b0;

        btn_max = 1'b1;
        repeat (3) begin
            measure_one_frame();
        end
        btn_max = 1'b0;

        btn_sweep = 1'b1;
        for (i = 0; i < 8; i = i + 1) begin
            measure_one_frame();
        end
        btn_sweep = 1'b0;

        $display("Testbench complete");
        $finish;
    end

endmodule