module stepper_motor_control
        #(
            parameter   CORE_ID          = 32'h5a5a_3333,
            
            parameter   WB_ADR_WIDTH     = 6,
            parameter   WB_DAT_SIZE      = 3,
            parameter   WB_DAT_WIDTH     = (8 << WB_DAT_SIZE),
            parameter   WB_SEL_WIDTH     = (1 << WB_DAT_SIZE),
            
            parameter   MICROSTEP_WIDTH  = 12,
            parameter   Q_WIDTH          = 24,
            parameter   X_WIDTH          = 24 + Q_WIDTH,
            parameter   X_SHIFT          = Q_WIDTH - 8,
            parameter   V_WIDTH          = Q_WIDTH,
            parameter   A_WIDTH          = Q_WIDTH,
            parameter   X_DIFF_WIDTH     = 32,
            
            parameter   INIT_CTL_ENABLE  = 1'b0,
            parameter   INIT_CTL_TARGET  = 1'b0,
            parameter   INIT_CTL_PWM     = 2'b11,
            parameter   INIT_TARGET_X    = 0,
            parameter   INIT_TARGET_V    = 0,
            parameter   INIT_TARGET_A    = 0,
            parameter   INIT_MAX_V       = 1000,
            parameter   INIT_MAX_A       = 100,
            parameter   INIT_MAX_A_NEAR  = 120
        )
        (
            input   wire                                reset,
            input   wire                                clk,
            
            input   wire            [WB_ADR_WIDTH-1:0]  s_wb_adr_i,
            input   wire            [WB_DAT_WIDTH-1:0]  s_wb_dat_i,
            output  wire            [WB_DAT_WIDTH-1:0]  s_wb_dat_o,
            input   wire                                s_wb_we_i,
            input   wire            [WB_SEL_WIDTH-1:0]  s_wb_sel_i,
            input   wire                                s_wb_stb_i,
            output  wire                                s_wb_ack_o,
            
            input   wire    signed  [X_DIFF_WIDTH-1:0]  in_x_diff,
            input   wire                                in_valid,
            
            output  wire                                motor_en,
            output  wire                                motor_a,
            output  wire                                motor_b,
            
            
            output  wire                                monitor_update,
            output  wire    signed  [X_WIDTH-1:0]       monitor_cur_x,
            output  wire    signed  [V_WIDTH:0]         monitor_cur_v,
            output  wire    signed  [A_WIDTH:0]         monitor_cur_a,
            output  wire    signed  [X_WIDTH-1:0]       monitor_target_x,
            output  wire    signed  [V_WIDTH:0]         monitor_target_v,
            output  wire    signed  [A_WIDTH:0]         monitor_target_a
        );
    
    
    // -----------------------------------------
    //  latch input
    // -----------------------------------------
    
    reg     signed  [X_DIFF_WIDTH-1:0]   reg_in_x_diff;
    always @(posedge clk) begin
        if ( in_valid ) begin
            reg_in_x_diff <= in_x_diff;
        end
    end
    
    
    
    // -----------------------------------------
    //  registers
    // -----------------------------------------
    
    localparam  ADR_CORE_ID     = 8'h00;
    localparam  ADR_CTL_ENABLE  = 8'h01;
    localparam  ADR_CTL_TARGET  = 8'h02;
    localparam  ADR_CTL_PWM     = 8'h03;
    localparam  ADR_TARGET_X    = 8'h04;
    localparam  ADR_TARGET_V    = 8'h06;
    localparam  ADR_TARGET_A    = 8'h07;
    localparam  ADR_MAX_V       = 8'h09;
    localparam  ADR_MAX_A       = 8'h0a;
    localparam  ADR_MAX_A_NEAR  = 8'h0f;
    localparam  ADR_CUR_X       = 8'h10;
    localparam  ADR_CUR_V       = 8'h12;
    localparam  ADR_CUR_A       = 8'h13;
    localparam  ADR_TIME        = 8'h20;
    localparam  ADR_IN_X_DIFF   = 8'h21;
    
    
    wire                                update;
    
    reg             [0:0]               reg_ctl_enable;
    reg             [3:0]               reg_ctl_target;
    reg             [1:0]               reg_ctl_pwm;
    reg     signed  [X_WIDTH-1:0]       reg_target_x;
    reg     signed  [V_WIDTH:0]         reg_target_v;
    reg     signed  [A_WIDTH:0]         reg_target_a;
    reg             [V_WIDTH-1:0]       reg_max_v;
    reg             [A_WIDTH-1:0]       reg_max_a;
    reg             [A_WIDTH-1:0]       reg_max_a_near;
    reg     signed  [X_WIDTH-1:0]       reg_cur_x;
    reg     signed  [V_WIDTH:0]         reg_cur_v;
    reg     signed  [A_WIDTH:0]         reg_cur_a;
    reg             [31:0]              reg_time;
    
    wire    signed  [V_WIDTH:0]         max_v = {1'b0, reg_max_v};
    wire    signed  [A_WIDTH:0]         max_a = {1'b0, reg_max_a};
    wire    signed  [V_WIDTH:0]         min_v = -max_v;
    wire    signed  [A_WIDTH:0]         min_a = -max_a;
    
    reg                                 reg_start;
    reg     signed  [A_WIDTH:0]         reg_a_tmp;
    reg     signed  [A_WIDTH:0]         reg_a;
    reg     signed  [V_WIDTH:0]         reg_v_tmp;
    reg     signed  [V_WIDTH:0]         reg_v;
    reg                                 reg_update;
    
    wire    signed  [A_WIDTH:0]         calc_a;
    wire                                calc_valid;
    
    function [WB_DAT_WIDTH-1:0] reg_mask(
                                        input [WB_DAT_WIDTH-1:0] org,
                                        input [WB_DAT_WIDTH-1:0] wdat,
                                        input [WB_SEL_WIDTH-1:0] msk
                                    );
    integer i;
    begin
        for ( i = 0; i < WB_DAT_WIDTH; i = i+1 ) begin
            reg_mask[i] = msk[i/8] ? wdat[i] : org[i];
        end
    end
    endfunction
    
    always @(posedge clk) begin
        if ( reset ) begin
            reg_ctl_enable  <= INIT_CTL_ENABLE;
            reg_ctl_target  <= INIT_CTL_TARGET;
            reg_ctl_pwm     <= INIT_CTL_PWM;
            reg_target_x    <= INIT_TARGET_X;
            reg_target_v    <= INIT_TARGET_V;
            reg_target_a    <= INIT_TARGET_A;
            reg_max_v       <= INIT_MAX_V;
            reg_max_a       <= INIT_MAX_A;
            reg_max_a_near  <= INIT_MAX_A_NEAR;
            reg_cur_x       <= 0;
            reg_cur_v       <= 0;
            reg_cur_a       <= 0;
            reg_time        <= 0;
            
            reg_start       <= 1'b0;
            reg_a_tmp       <= 0;
            reg_a           <= 0;
            reg_v_tmp       <= 0;
            reg_v           <= 0;
            
            reg_update      <= 1'b0;
        end
        else begin
            // start
            reg_start <= update;
            
            if ( reg_ctl_target[3] && in_valid ) begin
                reg_target_x <= reg_target_x + in_x_diff;
            end
            
            // acceleration
            reg_a_tmp <= 0;
            if ( reg_ctl_target[2] ) begin
                reg_a_tmp <= reg_target_a;
            end
            else if ( reg_ctl_target[1] ) begin
                reg_a_tmp <= reg_target_v - reg_cur_v;
            end
            if ( reg_ctl_target[0] ) begin
                reg_a <= calc_a;
            end
            else begin
                reg_a <= reg_a_tmp;
                if ( reg_a_tmp > +max_a ) begin reg_a <= +max_a; end
                if ( reg_a_tmp < -max_a ) begin reg_a <= -max_a; end
            end
            
            // speed
            reg_v_tmp <= reg_cur_v + reg_a;
            reg_v <= reg_v_tmp;
            if ( reg_v_tmp > +max_v ) begin reg_v <= +max_v; end
            if ( reg_v_tmp < -max_v ) begin reg_v <= -max_v; end
            
            // update
            reg_update <= 1'b0;
            if ( update && reg_ctl_enable ) begin
                reg_update <= 1'b1;
                reg_cur_x  <= reg_cur_x + reg_v;
                reg_cur_v  <= reg_v;
                reg_cur_a  <= reg_a;
                reg_time   <= reg_time + 1;
            end
            
            
            // write
            if ( s_wb_stb_i && s_wb_we_i ) begin
                case ( s_wb_adr_i )
                ADR_CTL_ENABLE:  reg_ctl_enable  <= reg_mask(reg_ctl_enable,             s_wb_dat_i, s_wb_sel_i);
                ADR_CTL_TARGET:  reg_ctl_target  <= reg_mask(reg_ctl_target,             s_wb_dat_i, s_wb_sel_i);
                ADR_CTL_PWM:     reg_ctl_pwm     <= reg_mask(reg_ctl_pwm,                s_wb_dat_i, s_wb_sel_i);
                ADR_TARGET_X:    reg_target_x    <= reg_mask((reg_target_x >>> X_SHIFT), s_wb_dat_i, s_wb_sel_i) <<< X_SHIFT;
                ADR_TARGET_V:    reg_target_v    <= reg_mask(reg_target_v,               s_wb_dat_i, s_wb_sel_i);
                ADR_TARGET_A:    reg_target_a    <= reg_mask(reg_target_a,               s_wb_dat_i, s_wb_sel_i);
                ADR_MAX_V:       reg_max_v       <= reg_mask(reg_max_v,                  s_wb_dat_i, s_wb_sel_i);
                ADR_MAX_A:       reg_max_a       <= reg_mask(reg_max_a,                  s_wb_dat_i, s_wb_sel_i);
                ADR_MAX_A_NEAR:  reg_max_a_near  <= reg_mask(reg_max_a_near,             s_wb_dat_i, s_wb_sel_i);
                endcase
            end
        end
    end
    
    assign s_wb_dat_o = (s_wb_adr_i == ADR_CORE_ID)     ? CORE_ID                     :
                        (s_wb_adr_i == ADR_CTL_ENABLE)  ? reg_ctl_enable              :
                        (s_wb_adr_i == ADR_CTL_TARGET)  ? reg_ctl_target              :
                        (s_wb_adr_i == ADR_CTL_PWM)     ? reg_ctl_pwm                 :
                        (s_wb_adr_i == ADR_TARGET_X)    ? (reg_target_x >>> X_SHIFT)  :
                        (s_wb_adr_i == ADR_TARGET_V)    ? reg_target_v                :
                        (s_wb_adr_i == ADR_TARGET_A)    ? reg_target_a                :
                        (s_wb_adr_i == ADR_MAX_V)       ? reg_max_v                   :
                        (s_wb_adr_i == ADR_MAX_A)       ? reg_max_a                   :
                        (s_wb_adr_i == ADR_MAX_A_NEAR)  ? reg_max_a_near              :
                        (s_wb_adr_i == ADR_CUR_X)       ? (reg_cur_x >>> X_SHIFT)     :
                        (s_wb_adr_i == ADR_CUR_V)       ? reg_cur_v                   :
                        (s_wb_adr_i == ADR_CUR_A)       ? reg_cur_a                   :
                        (s_wb_adr_i == ADR_TIME)        ? reg_time                    :
                        (s_wb_adr_i == ADR_IN_X_DIFF)   ? reg_in_x_diff               :
                        0;
    assign s_wb_ack_o = s_wb_stb_i;
    
    
    // calc
    stepper_motor_control_calc
            #(
                .X_WIDTH            (X_WIDTH),
                .V_WIDTH            (V_WIDTH),
                .A_WIDTH            (A_WIDTH)
            )
        i_stepper_motor_control_calc
            (
                .reset              (reset),
                .clk                (clk),
                .cke                (1'b1),
                
                .start              (reg_start),
                
                .target_x           (reg_target_x),
                .cur_x              (reg_cur_x),
                .cur_v              (reg_cur_v),
                .max_a              (reg_max_a),
                .max_a_near         (reg_max_a_near),
                
                .out_a              (calc_a),
                .out_valid          (calc_valid)
            );
    
    // drive
    bipolar_stepper_motor_drive
            #(
                .Q_WIDTH            (Q_WIDTH),
                .MICROSTEP_WIDTH    (MICROSTEP_WIDTH)
            )
        i_bipolar_stepper_motor_drive
            (
                .reset              (reset),
                .clk                (clk),
                
                .microstep_en       (reg_ctl_pwm[0]),
                .nanostep_en        (reg_ctl_pwm[1]),
                .asyc_update_en     (1'b0),
                
                .phase              (reg_cur_x[Q_WIDTH+1:0]),
                .update             (update),
                
                .out_a              (motor_a),
                .out_b              (motor_b)
            );
    
    assign motor_en = reg_ctl_enable;
    
    
    // monitor
    assign monitor_update   = reg_update;
    assign monitor_cur_x    = reg_cur_x;
    assign monitor_cur_v    = reg_cur_v;
    assign monitor_cur_a    = reg_cur_a;
    assign monitor_target_x = reg_target_x;
    assign monitor_target_v = reg_target_v;
    assign monitor_target_a = reg_target_a;
    
endmodule