module servo_cntr (
    input clk, reset_p,
    input [4:0] servo_mode,
    input servo_start,
    input [7:0] servo_angle,
    output servo,
    output reg [7:0] step,
    output [15:0] led
    );

    localparam IDLE     = 5'b00001;
    localparam ROUND    = 5'b00010;
    localparam ROUND_D  = 5'b00100;
    localparam ONEWAY_D = 5'b01000;
    localparam ONETIME  = 5'b10000;

    assign led[0] = servo_start;
    assign led[5:1] = servo_mode;

    integer cnt_servo;
    reg [3:0] cnt_delay;
    reg servo_dir = 1;
    always @(posedge clk or posedge reset_p) begin
        if(reset_p) begin
            step <= 40;
            servo_dir <= 1;
            cnt_servo <= 0;
            cnt_delay <= 0;
        end
        else begin
            if (servo_start) begin
                if (cnt_servo >= 3_000_000) begin
                    cnt_servo <= 0;
                    case (servo_mode)
                        IDLE     : begin
                            if (step <= 40) servo_dir <= 1;
                            else step <= step - 1;
                        end
                        ROUND    : begin
                            if (servo_dir) begin
                                if (step >= (40 + servo_angle)) servo_dir <= 0;
                                else step <= step + 1;
                            end
                            else if (servo_dir == 0) begin
                                if (step <= 40) servo_dir <= 1;
                                else step <= step - 1;
                            end
                        end
                        ROUND_D  : begin
                            if (servo_dir) begin
                                if (step >= (40 + servo_angle)) servo_dir <= 0;
                                else if (step % 10 == 0) begin
                                    if (cnt_delay >= 10) begin
                                        cnt_delay <= 0;
                                        step <= step + 1;
                                    end
                                    else cnt_delay <= cnt_delay + 1;
                                end
                                else step <= step + 1;
                            end
                            else if (servo_dir == 0) begin
                                if (step <= 40) servo_dir <= 1;
                                else if (step % 10 == 0) begin
                                    if (cnt_delay >= 10) begin
                                        cnt_delay <= 0;
                                        step <= step - 1;
                                    end
                                    else cnt_delay <= cnt_delay + 1;
                                end
                                else step <= step - 1;
                            end
                        end
                        ONEWAY_D : begin
                            if (servo_dir) begin
                                if (step >= (40 + servo_angle)) servo_dir <= 0;
                                else if (step % 10 == 0) begin
                                    if (cnt_delay >= 10) begin
                                        cnt_delay <= 0;
                                        step <= step + 1;
                                    end
                                    else cnt_delay <= cnt_delay + 1;
                                end
                                else step <= step + 1;
                            end
                            else if (servo_dir == 0) begin
                                if (step <= 40) servo_dir <= 1;
                                else step <= step - 1;
                            end
                        end
                        ONETIME  : begin
                            if (step < (40 + servo_angle)) step <= step + 1;
                            else if (step > (40 + servo_angle)) step <= step - 1;
                        end
                        default  : begin
                            if (step <= 40) servo_dir <= 1;
                            else step <= step - 1;
                        end
                    endcase
                end
                else begin
                    cnt_servo <= cnt_servo + 1;
                end
            end
            else if (!servo_start && servo_mode == ONETIME) begin
                if (cnt_servo >= 3_000_000) begin
                    cnt_servo <= 0;
                    if (step > 40) step <= step - 1;
                end
                else begin
                    cnt_servo <= cnt_servo + 1;
                end
            end
        end
    end

    pwm_Nstep #(.pwm_freq(50), .duty_step_N(1800)) pwm_servo (clk, reset_p, step, servo);
endmodule

module pwm_Nstep (
    input clk, reset_p,
    input [31:0] duty,                          // PWM Duty Rate, CCR Capture Compare Register
    output reg pwm                              // PWM Duty Applied Pulse
    );

    parameter sys_clk_freq  = 100_000_000;      // System Clock Frequency
    parameter pwm_freq      = 10_000;           // PWM Frequency
    parameter duty_step_N   = 256;              // Duty Step, ARR Auto Reload Register
    parameter temp = sys_clk_freq / pwm_freq / duty_step_N / 2; // Half of Cycle

    integer cnt_sysclk;                         // System Clock Count
    reg pwm_freqXn;                             // PWM Frequency
    always @(posedge clk, posedge reset_p) begin
        if (reset_p) begin
            cnt_sysclk = 0;                     // System Clock Count Reset
            pwm_freqXn = 0;                     // PWM Frequency Reset
        end
        else begin
            if (cnt_sysclk >= temp - 1) begin   // when Half of Cycle
                cnt_sysclk = 0;                 // System Clock Count Reset
                pwm_freqXn = ~pwm_freqXn;       // PWM Frequency Generation
            end
            else cnt_sysclk = cnt_sysclk + 1;   // Count System Clock
        end
    end

    // Edge Detection of PWM Frequency
    wire pwm_freqXn_nedge;                      // PWM Frequency Negative Edge
    edge_detector_pos pwm_freqXn_ed (.clk(clk), .reset_p(reset_p),
        .cp(pwm_freqXn), .n_edge(pwm_freqXn_nedge));

    integer cnt_duty;                           // PWM Frequency Count
    always @(posedge clk, posedge reset_p) begin
        if (reset_p) begin
            cnt_duty = 0;                       // PWM Frequency Count Reset
            pwm = 0;                            // Pulse Reset
        end
        else if (pwm_freqXn_nedge) begin
            if (cnt_duty >= duty_step_N) cnt_duty = 0;  // Count to End of Duty
            else cnt_duty = cnt_duty + 1;       // Count PWM Frequency

            if (cnt_duty < duty) pwm = 1;       // Pulse High Until Duty
            else pwm = 0;                       // Pulse Low After Duty
        end
    end
endmodule