`timescale 1ns / 1ns

module FDBK_LOOP(
M,
SET_I,
SET_Q,
dynamic_setpoint,
dkcm_bus,
t_mod_4,
feedback_enable,
integrate_enable,
feedforward_data,
RESET,
CLK,
fdbk_err_out
);

input[11:0] M;
input[13:0] SET_I;
input[13:0] SET_Q;
input dynamic_setpoint;
input[20:0] dkcm_bus;
input[1:0] t_mod_4;
input feedback_enable;
input integrate_enable;
input[7:0] feedforward_data;
input RESET;
input CLK;
output[11:0] fdbk_err_out;

wire  [11:0] M;
wire  [13:0] SET_I;
wire  [13:0] SET_Q;
wire   dynamic_setpoint;
wire  [1:0] t_mod_4;
wire   feedback_enable;
wire   integrate_enable;
wire  [7:0] feedforward_data;
wire   RESET;
wire   CLK;
wire  [11:0] fdbk_err_out;

// internal signal declarations
reg [9:0] ERROR_sat_latched;
wire [11:0] PROP_ERR;
wire [11:0] scaled_PROP_ERR;
wire [12:0] fdbk_err_wide;

  // component instantiations
  ERROR3 u4(
    .SATURATED(ERROR_sat_latched),
    .dkcm_bus(dkcm_bus),
    .t_mod_4(t_mod_4),
    .FDBK_EN(feedback_enable),
    .RESET(RESET),
    .CLK(CLK),
    .ERR3(PROP_ERR),
    .ERR4(scaled_PROP_ERR));

  reg [13:0] AVG_M, ACCUM;
  always @(posedge CLK) begin
    if((t_mod_4 == 2'b 00)) begin
      AVG_M <= ACCUM;
      ACCUM <= {M[11] ,M[11] ,M};
    end
    else begin
      ACCUM <= ACCUM + {M[11] ,M[11] ,M};
    end
  end

  reg [13:0] set_wave;
  wire [13:0] iqset = t_mod_4[0] ? SET_I : SET_Q;
  wire [14:0] target = {AVG_M[13],AVG_M} + {set_wave[13],set_wave};
  wire [15:0] iERROR = {M[11] ,M[11],M,2'b 00} - {target[14],target};
  // arithmetic saturation from 16 bits down to 10 bits
//  wire [9:0] ERROR_sat = ((iERROR[15:9]  == 7'b 0000000) || (iERROR[15:9]  == 7'b 1111111)) ?
//     iERROR[9:0] : ( (iERROR[15]  == 1'b 1) ? 10'b 1000000000 : 10'b 0111111111 );

  function ERROR_sat;
  input [15:0]iError;
  begin
  ERROR_sat = ((iERROR[15:9]  == 7'b 0000000) || (iERROR[15:9]  == 7'b 1111111)) ?
      iERROR[9:0] : ( (iERROR[15]  == 1'b 1) ? 10'b 1000000000 : 10'b 0111111111 );
  end
  endfunction    

  always @(posedge CLK) begin
    set_wave <= t_mod_4[1] ? iqset :  -iqset;
    ERROR_sat_latched <= ERROR_sat(iERROR);
  end


  // Integrator works on interleaved I and Q.
  reg [15:0] integrate_out;
  reg [12:0] integrate_input;
  reg [16:0] integrate_sum;
  // arithmetic saturation from 17 bits down to 16 bits
  wire [15:0] integrate_sat = ((integrate_sum[16:15]  == 2'b 00) || (integrate_sum[16:15]  == 2'b 11)) ?
       integrate_sum[15:0] : ( (integrate_sum[16]  == 1'b 1) ? 16'b 1000000000000000 : 16'b 0111111111111111 );
  always @(posedge CLK) begin
    integrate_out <= integrate_enable ? integrate_sat : {16{1'b0}};
    // to avoid wrap-around, arguments are resized 1 bit larger (sign extended) 
    // before addition or subtraction is performed      
    integrate_sum <= {integrate_out[15] ,integrate_out} + {integrate_input[12], integrate_input[12] ,integrate_input[12] ,integrate_input[12] ,integrate_input};
    integrate_input <= {feedforward_data[7] ,feedforward_data[7] ,feedforward_data,3'b 000} + {scaled_PROP_ERR[11],scaled_PROP_ERR};
  end

  // Combine low-latency linear feedback with the integration results
  // This doesn't saturate or deal with the carry yet.
  assign fdbk_err_wide = {integrate_out[15] ,integrate_out[15:4] } + {PROP_ERR[11],PROP_ERR};
  wire [11:0] fdbk_err = ((fdbk_err_wide[12:11]  == 2'b 00) || (fdbk_err_wide[12:11]  == 2'b 11)) ?
	fdbk_err_wide[11:0] : ( (fdbk_err_wide[12]  == 1'b 1) ? 12'b 100000000000 : 12'b 011111111111 );
  assign fdbk_err_out = fdbk_err;

endmodule