`timescale 1ns / 1ns

`define DAC902

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

input[11:0] M;
input[13:0] SET_I;
input[13:0] SET_Q;
input[20:0] dkcm_bus;
input dkcm_clk;
input[1:0] t_mod_4;
input feedback_enable;
input integrate_enable;
input[7:0] feedforward_data;
input CLK;
output[11:0] fdbk_err_out;
reg [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),
    .dkcm_clk(dkcm_clk),
    .t_mod_4(t_mod_4),
    .FDBK_EN(feedback_enable),
    .CLK(CLK),
    .ERR3(PROP_ERR),
    .ERR4(scaled_PROP_ERR));

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


  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



  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'b00} - {target[14], target};
  // arithmetic saturation from 16 bits down to 10 bits
  // wire [9:0] ERROR_sat = ((iERROR[15:9]  == 7'b0000000) || (iERROR[15:9]  == 7'b1111111)) ?
  //    iERROR[9:0] : ( (iERROR[15]  == 1'b1) ? 10'b1000000000 : 10'b0111111111 );

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


  // feedforward handling is officially untested
  reg [15:0] integrate_out;
`ifdef DAC902
  wire [12:0] ff_ready = {feedforward_data[7], feedforward_data[7], feedforward_data, 3'b000} ^ {13{t_mod_4[1]}};
  wire [16:0] int_wrap = {~integrate_out[15], ~integrate_out};
`else
  wire [12:0] ff_ready = {feedforward_data[7], feedforward_data[7], feedforward_data, 3'b000};
  wire [16:0] int_wrap = {integrate_out[15], integrate_out};
`endif

  // Integrator works on interleaved I and Q.
  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'b00) || (integrate_sum[16:15]  == 2'b11)) ?
       integrate_sum[15:0] : ( (integrate_sum[16]  == 1'b1) ? 16'b1000000000000000 : 16'b0111111111111111 );
  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 <= int_wrap + {{4{integrate_input[12]}}, integrate_input};
    integrate_input <= ff_ready + {scaled_PROP_ERR[11], scaled_PROP_ERR};
  end

  // Combine low-latency linear feedback with the integration results
  // Saturation and sign extension are claimed valid
  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'b00) || (fdbk_err_wide[12:11]  == 2'b11)) ?
	fdbk_err_wide[11:0] : ( (fdbk_err_wide[12]  == 1'b1) ? 12'b100000000000 : 12'b011111111111 );
  always @(posedge CLK) fdbk_err_out <= fdbk_err;

endmodule