`timescale 1ns / 1ns

// Super-simple cavity model.
// Includes delay, phase shift, and the cavity pole.
module cavity(clk40, dac_output, adc_input);
	input clk40;
	input [11:0] dac_output;
	output [11:0] adc_input;
	reg signed [11:0] d1, d2, d3, d4, d5, d6, d7, d8;

	// implement delay as a ring buffer in memory
	// ix is the pointer;
	// 40-long buffer x 25 ns = 1000 ns delay
	reg [11:0] db[39:0];
	reg [11:0] drive1, drive2;
	reg [5:0] ix;
	integer i;
	initial begin
		for (i=0; i<40; i=i+1) db[i]=0;
		ix=0;
		drive1=0;
		drive2=0;
	end
	always @(posedge clk40) begin
		ix <= (ix==39)?0:(ix+1);
		if (dac_output !== 12'bxxxxxxxxxxxx) db[ix] <= dac_output;
		drive1 <= db[ix];
		drive2 <= drive1;
	end
	// 45 degree phase shift (45/64 ~ 1/sqrt(2))
	wire [17:0] drivel = 45*( {{6{drive1[11]}},drive1} +
	                          {{6{drive2[11]}},drive2} );
	wire [11:0] drive  = drivel[17:6];

	// cavity state, on-resonance
	// time constant is 128 samples, 3.2 us
	reg signed [15:0] field1, field2;
	initial field1=0; initial field2=0;
	wire [15:0] delta = {drive,4'b0000}-field1;
	always @(posedge clk40) begin
		field1 <= -field2;
		field2 <= field1+{{7{delta[15]}},delta[15:7]};
	end

	// measurement includes DC offset
	assign adc_input = field2[15:4]-20;

	reg signed [11:0] dac_output_s;
	always @(negedge clk40) begin
		dac_output_s = dac_output;
		$display("cavity %d %d", dac_output_s, field1);
	end
endmodule

module main();
	integer cc;
	reg clk, load;
	reg signed [11:0] konstant;
	reg  [2:0] addr;
	initial begin
		// $dumpfile("fdbk_loop.vcd");
		// $dumpvars(4,main);
		// 25 MHz clk
		for (cc = 0; cc < 900; cc = cc + 1) begin
			#20;
			clk = 1;
			#20;
			clk = 0;
			load = (cc%40 == 10) && (cc<100);
			if (cc ==  5) begin konstant= 600; addr=1; end  // real
			if (cc == 45) begin konstant=-600; addr=2; end  // imag
			if (cc == 85) begin konstant=1000; addr=3; end  // integral
		end
		$finish;
	end
	wire [20:0] dkcm_bus;
	wire busy;
	dkcm_controller mut(clk, dkcm_bus, konstant, load, addr, busy);

	reg clk40;
	always begin
		clk40=1;  #12;
		clk40=0;  #13;
	end

	reg [1:0] sync_count;  initial sync_count=0;
	always @(posedge clk40) sync_count <= sync_count+1;

	wire [11:0] adc_input, output40;
	reg feedback_on, integrate_on;
	initial feedback_on=0; initial integrate_on=0;
	
	always @(posedge clk40) begin
		feedback_on  <= (cc>120);
		integrate_on <= (cc>180);
	end

	wire [13:0] set_wave;
	reg [13:0] SET_I=1000, SET_Q=500;
	dds dds(clk40, set_wave, SET_I, SET_Q,
		cc>20, 16'd0, );
	fdbk_loop fdbk(
		.M(adc_input), .set_wave(set_wave),
		.dkcm_bus(dkcm_bus), .dkcm_clk(clk),
		.feedback_enable(feedback_on), .integrate_enable(integrate_on),
		.feedforward_data(8'b0),
		.CLK(clk40), .t_mod_4(sync_count),
		.fdbk_err_out(output40));

	cavity cav(clk40, output40, adc_input);

	reg signed [11:0] output40_signed;
	always @(posedge clk40) begin
		if (1'b0 && cc > 100) begin
			output40_signed = output40;
			$display("feedback output %d", output40_signed);
		end
	end

endmodule