`timescale 1ns / 1ps
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 
// 
// Create Date: 2020/12/14 14:44:05
// Design Name: 
// Module Name: phase_adjust
// Project Name: 
// Target Devices: 
// Tool Versions: 
// Description: 
// 
// Dependencies: 
// 
// Revision:
// Revision 0.01 - File Created
// Additional Comments:
// 
//////////////////////////////////////////////////////////////////////////////////


module phase_adjust(
		input clk,
		input rst,
		input [9:0] din_i,
		input [9:0] din_q,
		input nd,
		input framesync,
		output  reg rdy,
		output  [9:0] dout_i,
		output  [9:0] dout_q
    );
	
	
	reg  [7:0] addra = 8'd0;
	reg [19:0] dina = 20'd0;
	reg [7:0] addrb = 8'd0;
	wire [19:0] doutb;
	RAM_phase RAM_phase (
	  .clka(clk), // input clka
	  .wea(1'b1), // input [0 : 0] wea
	  .addra(addra), // input [9 : 0] addra
	  .dina(dina), // input [15 : 0] dina
	  .clkb(clk), // input clkb
	  .addrb(addrb), // input [10 : 0] addrb
	  .doutb(doutb) // output [7 : 0] doutb
	);
	
	
	wire [10:0] squarei;
	wire [10:0] squareq;
	wire [47:0] m_axis_dout_tdata;
	wire m_axis_dout_tvalid;
	CompMultIP CompMultIP1 (
		.aclk(clk),                              // input wire aclk
		.s_axis_a_tvalid(nd),        // input wire s_axis_a_tvalid
		.s_axis_a_tdata({6'd0,din_q,6'd0,din_i}),          // input wire [31 : 0] s_axis_a_tdata
		.s_axis_b_tvalid(nd),        // input wire s_axis_b_tvalid
		.s_axis_b_tdata({6'd0,din_q,6'd0,din_i}),          // input wire [31 : 0] s_axis_b_tdata
		.m_axis_dout_tvalid(m_axis_dout_tvalid),  // output wire m_axis_dout_tvalid
		.m_axis_dout_tdata(m_axis_dout_tdata)    // output wire [47 : 0] m_axis_dout_tdata
	);
	assign squareq = m_axis_dout_tdata[40:30];
	assign squarei = m_axis_dout_tdata[16:6];
		
	wire [11:0] square2i;
	wire [11:0] square2q;
	wire [47:0] m_axis_dout_tdata2;
	wire m_axis_dout_tvalid2;
	CompMultIP2 CompMultIP2 (
		.aclk(clk),                              // input wire aclk
		.s_axis_a_tvalid(m_axis_dout_tvalid),        // input wire s_axis_a_tvalid
		.s_axis_a_tdata({5'd0,squareq,5'd0,squarei}),          // input wire [31 : 0] s_axis_a_tdata
		.s_axis_b_tvalid(m_axis_dout_tvalid),        // input wire s_axis_b_tvalid
		.s_axis_b_tdata({5'd0,squareq,5'd0,squarei}),          // input wire [31 : 0] s_axis_b_tdata
		.m_axis_dout_tvalid(m_axis_dout_tvalid2),  // output wire m_axis_dout_tvalid
		.m_axis_dout_tdata(m_axis_dout_tdata2)    // output wire [47 : 0] m_axis_dout_tdata
	);
	assign square2q = m_axis_dout_tdata2[41:30];
	assign square2i = m_axis_dout_tdata2[17:6];
	

	reg [19:0] sum256i = 20'd0;
	reg [19:0] sum256q = 20'd0;	
	reg [9:0] addradelay = 10'd0;
	reg [3:0] state_phase;
	reg [9:0] cnt_phase;
	reg sum_rdy;
	reg rotate_nd;
	reg [9:0] phase_estm;
	reg [9:0] phase_shengyu;
	reg [9:0] phase_final;
	reg [9:0] phase_pi = 10'b0110010010;
	
	
	wire [9:0] phase_out;
	wire [5:0] phase_out_head;
	wire phaserdy;
	atan2_phase atan2_phase (
		.aclk(clk),                                        // input wire aclk
		.s_axis_cartesian_tvalid(sum_rdy),  // input wire s_axis_cartesian_tvalid
		.s_axis_cartesian_tdata({4'd0,sum256q,4'd0,sum256i}),    // input wire [47 : 0] s_axis_cartesian_tdata
		.m_axis_dout_tvalid(phaserdy),            // output wire m_axis_dout_tvalid
		.m_axis_dout_tdata({phase_out_head,phase_out})              // output wire [15 : 0] m_axis_dout_tdata
	);
	
	wire [5:0] rotate_headq;
	wire [5:0] rotate_headi;
	wire rotate_rdy;
	

	atan2_rotate atan2_rotate (
		.aclk(clk),                                        // input wire aclk
		.s_axis_cartesian_tvalid(rotate_nd),  // input wire s_axis_cartesian_tvalid
		.s_axis_cartesian_tdata({6'd0,doutb[19:10],6'd0,doutb[9:0]}),    // input wire [47 : 0] s_axis_cartesian_tdata
		.s_axis_phase_tvalid(rotate_nd),  // input wire s_axis_cartesian_tvalid
		.s_axis_phase_tdata({6'd0,phase_final[9],phase_final[9],phase_final[9:2]}),    // input wire [47 : 0] s_axis_cartesian_tdata
		.m_axis_dout_tvalid(rotate_rdy),            // output wire m_axis_dout_tvalid
		.m_axis_dout_tdata({rotate_headq,dout_q,rotate_headi,dout_i})              // output wire [15 : 0] m_axis_dout_tdata
	);
	
	always @ (posedge clk) begin
		if(rst) begin
			state_phase <= 4'd0;
			addradelay <= 10'd0;
			sum256i <= 20'd0;
			sum256q <= 20'd0;
			cnt_phase <= 10'd0;
			sum_rdy <= 1'b0;
			phase_estm <= 10'd0;
			phase_shengyu <= 10'd0;
			phase_final <= 10'd0;
			rotate_nd <= 1'b0;
			rdy	<= 1'b0;
		end
		else begin
			rdy <= rotate_rdy;
			case(state_phase)
				4'd0:begin
					if(framesync) begin
						state_phase <= 4'd1;
					end
					else begin
						state_phase <= 4'd0;
					end
				end
				4'd1:begin
					if(nd)begin
						addradelay	<= addradelay + 10'd1;
						dina		<= {din_q,din_i};
						addra 		<= addradelay[7:0];
						if(addradelay==10'd255) begin
							state_phase <= 4'd2;
							addradelay <= 10'd0;
						end
						else begin
							state_phase <= 4'd1;
						end
					end 
					else begin
						state_phase <= 4'd1;
						addradelay	<= addradelay;
						addra		<= addra;
						dina		<= dina;
						state_phase	<= 2'd1;
					end
					
					
					if(m_axis_dout_tvalid2)begin
						cnt_phase <= cnt_phase + 10'd1;
						sum256i <= sum256i + {square2i[11],square2i[11],square2i[11],square2i[11],square2i[11],square2i[11],square2i[11],square2i[11],square2i};
						sum256q <= sum256q + {square2q[11],square2q[11],square2q[11],square2q[11],square2q[11],square2q[11],square2q[11],square2q[11],square2q};
					end
					else begin
						cnt_phase <= cnt_phase;
						sum256i <= sum256i;
						sum256q <= sum256q;
					end
				end
				4'd2:begin
					if(m_axis_dout_tvalid2)begin
						if(cnt_phase==10'd255)begin
							cnt_phase <= 10'd0;
							state_phase <= 4'd3;
							sum_rdy <= 1'b1;
						end
						else begin
							cnt_phase <= cnt_phase + 10'd1;
							sum_rdy <= 1'b0;
							state_phase <= 4'd2;
						end
						sum256i <= sum256i + {square2i[11],square2i[11],square2i[11],square2i[11],square2i[11],square2i[11],square2i[11],square2i[11],square2i};
						sum256q <= sum256q + {square2q[11],square2q[11],square2q[11],square2q[11],square2q[11],square2q[11],square2q[11],square2q[11],square2q};
					end
					else begin
						cnt_phase <= cnt_phase;
						state_phase <= 4'd2;
						sum256i <= sum256i;
						sum256q <= sum256q;
					end	
				end
				4'd3:begin
					sum_rdy <= 1'b0;
					if(phaserdy) begin
						sum256i <= 20'd0;
						sum256q <= 20'd0;
						phase_estm <= phase_out;
						if(phase_out[9]) begin
							phase_shengyu <= phase_pi + phase_out;
						end
						else begin
							phase_shengyu <= phase_pi - phase_out;
						end
						state_phase <= 4'd4;
					end
					else begin
						phase_estm <= phase_estm;
						state_phase <= 4'd3;
					end
				end
				4'd4:begin
					if(phase_estm[9]) begin
						phase_final <= -phase_shengyu;
					end
					else begin
						phase_final <= phase_shengyu;
					end
					if(cnt_phase == 10'd0) begin
						cnt_phase <= cnt_phase + 10'd1;
						addrb <= 10'd0;
						rotate_nd <= 1'b1;
					end
					else if(cnt_phase == 10'd256) begin
						cnt_phase <= 10'd0;
						rotate_nd <= 1'b0;
						state_phase <= 4'd1;
						addrb <= 8'd0;
					end
					else begin
						cnt_phase <= cnt_phase + 10'd1;
						addrb <= addrb + 8'd1;
						rotate_nd <= 1'b1;
					end
				end
				default:begin
					if(framesync) begin
						state_phase <= 4'd1;
					end
					else begin
						state_phase <= 4'd0;
					end
				end
			endcase
		end
	end
	
	
	
	
endmodule