`timescale 1ns / 1ns
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 
// 
// Create Date: 2019/12/23 17:06:52
// Design Name: 
// Module Name: freqestm
// Project Name: 
// Target Devices: 
// Tool Versions: 
// Description: 
// 
// Dependencies: 
// 
// Revision:
// Revision 0.01 - File Created
// Additional Comments:
// 
//////////////////////////////////////////////////////////////////////////////////


module freqestm (
    input            clk,
    input            rst,
    input            framesync,
    input            mgc_rdy,
    input      [9:0] datain_i,
    input      [9:0] datain_q,
    output reg [9:0] dataout_i,
    output reg [9:0] dataout_q,
    output reg       framesync_delay,
    output reg       data_save,
    output reg       rst_freq
);

    wire [19:0] datain_01;
    wire [19:0] datain_02;
    wire [19:0] datain_03;
    wire        framesync_d2;
    wire        framesync_d3;

    wire [19:0] dataout_16;

    assign datain_01 = {datain_q, datain_i};

    shiftram_freq shiftram_freq_01 (
        .CLK(clk),
        .D  (datain_01),
        .Q  (datain_02)
    );
    shiftram_freq shiftram_freq_02 (
        .CLK(clk),
        .D  (datain_02),
        .Q  (datain_03)
    );
    wire empty;
    wire full;
    reg  rd_en;
    reg  wr_en;

    fifo_freq fifo_freq (
        .clk  (clk),
        .srst (rst),
        .din  (datain_03),
        .wr_en(wr_en),
        .full (full),
        .rd_en(rd_en),
        .empty(empty),
        .dout (dataout_16)

    );
    reg [15:0] cnt_fifo;
    reg [ 2:0] state_fifo;
    always @(posedge clk) begin
        if (rst) begin
            cnt_fifo   <= 16'd0;
            wr_en      <= 1'b0;
            rd_en      <= 1'b0;
            state_fifo <= 3'd0;
        end
        else begin
            case (state_fifo)
                3'd0: begin
                    if (mgc_rdy) begin
                        state_fifo <= 3'd1;
                    end
                    else begin
                        state_fifo <= state_fifo;
                    end
                end
                3'd1: begin
                    if (cnt_fifo == 16'd2175) begin
                        wr_en    <= 1'b1;
                        cnt_fifo <= cnt_fifo + 16'd1;
                    end
                    else if (cnt_fifo == 16'd18065) begin
                        rd_en    <= 1'b1;
                        cnt_fifo <= cnt_fifo;
                    end
                    else begin
                        cnt_fifo <= cnt_fifo + 16'd1;
                        wr_en    <= wr_en;
                        rd_en    <= rd_en;
                    end
                    state_fifo <= state_fifo;
                end
            endcase
        end
    end


    reg  [11:0] cnt_pilot;
    reg  [14:0] cnt_frame;
    reg  [ 7:0] cnt_bag;
    reg  [11:0] cnt_cp;
    reg  [11:0] cnt_process;
    reg  [11:0] addra_pilot01;
    reg  [11:0] addrb_pilot01;
    reg  [11:0] addra_pilot02;
    reg  [11:0] addrb_pilot02;
    reg  [ 4:0] state_freq;

    reg  [19:0] din_pilot01;
    reg  [19:0] din_pilot02;
    wire [19:0] dout_pilot01;
    wire [19:0] dout_pilot02;

    reg         framesync_delay_d1 = 1'b0;
    reg         framesync_delay_d2 = 1'b0;
    reg         framesync_delay_d3 = 1'b0;
    reg         framesync_delay_d4 = 1'b0;
    reg         framesync_delay_d5 = 1'b0;
    reg         framesync_delay_d6 = 1'b0;
    reg         framesync_delay_d7 = 1'b0;
    reg         framesync_delay_d8 = 1'b0;
    reg         framesync_delay_d9 = 1'b0;


    reg         wea_01;
    reg         wea_02;

    ram_pilot ram_pilota (
        .clka (clk),
        .clkb (clk),
        .addra(addra_pilot01),
        .addrb(addrb_pilot01),
        .dina (din_pilot01),
        .doutb(dout_pilot01),
        .wea  (wea_01)
    );

    ram_pilot ram_pilotb (
        .clka (clk),
        .clkb (clk),
        .addra(addra_pilot02),
        .addrb(addrb_pilot02),
        .dina (din_pilot02),
        .doutb(dout_pilot02),
        .wea  (wea_02)
    );

    wire [ 2:0] Multouti01_head;
    wire [ 2:0] Multoutq01_head;
    wire [20:0] Multouti01;
    wire [20:0] Multoutq01;
    reg         acc_rdy;

    com_freq com_freq (
        .aclk              (clk),
        .s_axis_a_tvalid   (1'd1),
        .s_axis_a_tdata    ({6'd0, dout_pilot01[19:10], 6'd0, dout_pilot01[9:0]}),
        .s_axis_b_tvalid   (1'd1),
        .s_axis_b_tdata    ({6'd0, dout_pilot02[19:10], 6'd0, dout_pilot02[9:0]}),
        .m_axis_dout_tvalid(m_axis_dout_tvalid01),
        .m_axis_dout_tdata ({Multoutq01_head, Multoutq01, Multouti01_head, Multouti01})
    );


    reg         dfreq_word_nd_DDS;
    reg  [31:0] acc_com_i;
    reg  [31:0] acc_com_q;
    reg  [ 9:0] phase_reg;
    reg  [13:0] freq_out;
    wire [ 9:0] phase_out;
    wire [ 5:0] phase_out_head;
    wire        phaserdy;
    reg  [ 3:0] cnt_6;
    always @(posedge clk) begin
        if (rst) begin
            //dataout_i <= 12'd0;
            //dataout_q <= 12'd0;
            framesync_delay_d2 <= 1'b0;
            framesync_delay_d3 <= 1'b0;
            framesync_delay_d4 <= 1'b0;
            framesync_delay_d5 <= 1'b0;
            framesync_delay_d6 <= 1'b0;
            framesync_delay_d7 <= 1'b0;
            framesync_delay_d8 <= 1'b0;
            framesync_delay    <= 1'b0;
            state_freq         <= 5'd0;
            framesync_delay_d1 <= 1'b0;
            cnt_bag            <= 8'd0;
            acc_com_i          <= 32'd0;
            acc_com_q          <= 32'd0;
            phase_reg          <= 10'd0;
            freq_out           <= 14'd0;
            cnt_pilot          <= 12'd0;
            cnt_frame          <= 15'd0;
            cnt_process        <= 12'd0;
            addra_pilot01      <= 12'd0;
            addra_pilot02      <= 12'd0;
            addrb_pilot01      <= 12'd0;
            addrb_pilot02      <= 12'd0;
            din_pilot01        <= 20'd0;
            din_pilot02        <= 20'd0;
            cnt_cp             <= 12'd0;
            wea_01             <= 1'b0;
            wea_02             <= 1'b0;
            acc_rdy            <= 1'b0;
            data_save          <= 1'b0;
            dfreq_word_nd_DDS  <= 1'b0;
            cnt_6              <= 4'd0;
        end
        else begin
            framesync_delay_d2 <= data_save;
            framesync_delay_d3 <= framesync_delay_d2;
            framesync_delay_d4 <= framesync_delay_d3;
            framesync_delay_d5 <= framesync_delay_d4;
            framesync_delay_d6 <= framesync_delay_d5;
            framesync_delay_d7 <= framesync_delay_d6;
            framesync_delay_d8 <= framesync_delay_d7;
            framesync_delay    <= framesync_delay_d8;
            case (state_freq)
                5'd0: begin
                    if (framesync) begin
                        state_freq <= 5'd1;
                        wea_01     <= 1'b1;
                        if (datain_q == 10'd512) din_pilot01[19:10] <= 10'd511;
                        else din_pilot01[19:10] <= -datain_q;

                        din_pilot01[9:0] <= datain_i[9:0];
                    end
                    else begin
                        state_freq <= 5'd0;
                    end
                    //dataout_i <= 12'd0;
                    //dataout_q <= 12'd0;
                    framesync_delay_d1 <= 1'b0;
                    cnt_bag            <= 8'd0;
                    acc_com_i          <= 32'd0;
                    acc_com_q          <= 32'd0;
                    phase_reg          <= 10'd0;
                    freq_out           <= 14'd0;
                    cnt_pilot          <= 12'd0;
                    cnt_frame          <= 15'd0;
                    cnt_process        <= 12'd0;
                    addra_pilot01      <= 12'd0;
                    addra_pilot02      <= 12'd0;
                    addrb_pilot01      <= 12'd0;
                    addrb_pilot02      <= 12'd0;
                    din_pilot01        <= 20'd0;
                    din_pilot02        <= 20'd0;
                    cnt_cp             <= 12'd0;
                    wea_01             <= 1'b0;
                    wea_02             <= 1'b0;
                    acc_rdy            <= 1'b0;
                    dfreq_word_nd_DDS  <= 1'b0;
                end

                5'd1: begin
                    if (cnt_pilot == 12'd1023) begin
                        addra_pilot01 <= addra_pilot01;
                        cnt_pilot     <= 12'd0;
                        state_freq    <= 5'd11;
                        cnt_frame     <= cnt_frame + 15'd1;
                        cnt_bag       <= cnt_bag;

                        wea_01        <= 1'b0;
                    end
                    else begin
                        if (datain_q == 10'd512) din_pilot01[19:10] <= 10'd511;
                        else din_pilot01[19:10] <= -datain_q;

                        din_pilot01[9:0] <= datain_i[9:0];
                        addra_pilot01    <= addra_pilot01 + 12'd1;
                        cnt_pilot        <= cnt_pilot + 12'd1;
                        cnt_frame        <= cnt_frame + 15'd1;
                        cnt_bag          <= cnt_bag;
                        wea_01           <= 1'b1;
                    end
                end

                5'd2: begin
                    if (cnt_frame == 15'd13823) begin
                        state_freq  <= 5'd3;
                        cnt_frame   <= 15'd0;
                        wea_02      <= 1'b1;
                        din_pilot02 <= {datain_q, datain_i};
                    end
                    else begin
                        cnt_frame <= cnt_frame + 15'd1;
                    end
                end

                5'd3: begin
                    if (cnt_pilot == 12'd1023) begin
                        addra_pilot02 <= addra_pilot02;
                        cnt_pilot     <= 12'd0;
                        state_freq    <= 5'd13;
                        cnt_frame     <= cnt_frame + 15'd1;
                        cnt_bag       <= cnt_bag + 8'd1;
                        wea_02        <= 1'b0;
                    end
                    else begin
                        din_pilot02   <= {datain_q, datain_i};
                        addra_pilot02 <= addra_pilot02 + 12'd1;
                        cnt_pilot     <= cnt_pilot + 12'd1;
                        cnt_frame     <= cnt_frame + 15'd1;
                        cnt_bag       <= cnt_bag;
                        wea_02        <= 1'b1;
                    end
                end

                5'd4: begin
                    state_freq <= 5'd5;
                    cnt_frame  <= cnt_frame + 15'd1;
                    acc_com_i  <= 32'd0;
                    acc_com_q  <= 32'd0;
                end

                5'd5: begin
                    if (cnt_6 == 4'd6) begin
                        if (Multouti01[20] == 1'b1) acc_com_i <= acc_com_i + {11'b11111111111, Multouti01};
                        else acc_com_i <= acc_com_i + {11'b00000000000, Multouti01};
                        if (Multoutq01[20] == 1'b1) acc_com_q <= acc_com_q + {11'b11111111111, Multoutq01};
                        else acc_com_q <= acc_com_q + {11'b00000000000, Multoutq01};
                        cnt_6 <= cnt_6;
                    end
                    else begin
                        acc_com_q <= acc_com_q;
                        acc_com_i <= acc_com_i;
                        cnt_6     <= cnt_6 + 4'd1;
                    end
                    if (cnt_pilot >= 12'd2047) begin
                        addrb_pilot01 <= 12'd0;
                        addrb_pilot02 <= 12'd0;
                        cnt_frame     <= cnt_frame + 15'd1;
                        if (cnt_pilot == 12'd2053) begin
                            acc_rdy    <= 1'b1;
                            cnt_pilot  <= 12'd0;
                            state_freq <= 5'd6;
                            cnt_6      <= 4'd0;
                        end
                        else begin
                            cnt_pilot <= cnt_pilot + 12'd1;
                        end
                    end
                    else begin
                        cnt_pilot     <= cnt_pilot + 12'd1;
                        addrb_pilot01 <= addrb_pilot01 + 12'd1;
                        addrb_pilot02 <= addrb_pilot02 + 12'd1;
                        acc_rdy       <= 1'b0;
                        cnt_frame     <= cnt_frame + 15'd1;
                    end
                end

                5'd6: begin
                    acc_rdy <= 1'b0;
                    if (phaserdy) begin
                        phase_reg  <= phase_out;
                        state_freq <= 5'd7;
                        cnt_frame  <= cnt_frame + 15'd1;
                        wea_01     <= 1'b1;

                        if (dout_pilot02[19:10] == 10'd512) din_pilot01[19:10] <= 10'd511;
                        else din_pilot01[19:10] <= -dout_pilot02[19:10];
                        din_pilot01[9:0] <= dout_pilot02[9:0];

                        addrb_pilot02    <= addrb_pilot02 + 12'd1;
                    end
                    else begin
                        phase_reg <= phase_reg;
                        cnt_frame <= cnt_frame + 15'd1;
                    end
                end

                5'd7: begin
                    state_freq <= 5'd8;
                    data_save  <= 1'b1;

                    if (phase_reg[9] == 1'b1) freq_out <= {3'b111, phase_reg, 1'b0} + {4'b1111, phase_reg};
                    else freq_out <= {3'b000, phase_reg, 1'b0} + {4'b0000, phase_reg};
                    cnt_frame         <= cnt_frame + 15'd1;
                    dfreq_word_nd_DDS <= 1'b1;
                    if (cnt_bag == 8'd1) framesync_delay_d1 <= 1'b1;
                    else framesync_delay_d1 <= 1'b0;

                    wea_01 <= 1'b1;

                    if (dout_pilot02[19:10] == 10'd512) din_pilot01[19:10] <= 10'd511;
                    else din_pilot01[19:10] <= -dout_pilot02[19:10];
                    din_pilot01[9:0] <= dout_pilot02[9:0];

                    addrb_pilot02    <= addrb_pilot02 + 12'd1;
                end

                5'd8: begin
                    dfreq_word_nd_DDS  <= 1'b0;
                    framesync_delay_d1 <= 1'b0;
                    if (cnt_pilot >= 12'd2045) begin

                        addrb_pilot02 <= 12'd0;
                        if (cnt_pilot == 12'd2047) begin
                            addra_pilot01 <= 12'd0;
                            addrb_pilot02 <= 12'd0;
                            cnt_pilot     <= 12'd0;
                            state_freq    <= 5'd9;
                            cnt_frame     <= cnt_frame + 15'd1;
                            wea_01        <= 1'b0;
                        end
                        else begin
                            if (dout_pilot02[19:10] == 10'd512) din_pilot01[19:10] <= 10'd511;
                            else din_pilot01[19:10] <= -dout_pilot02[19:10];
                            din_pilot01[9:0] <= dout_pilot02[9:0];

                            addra_pilot01    <= addra_pilot01 + 12'd1;

                            cnt_pilot        <= cnt_pilot + 12'd1;
                            cnt_frame        <= cnt_frame + 15'd1;
                            wea_01           <= 1'b1;
                        end
                    end
                    else begin
                        if (dout_pilot02[19:10] == 10'd512) din_pilot01[19:10] <= 10'd511;
                        else din_pilot01[19:10] <= -dout_pilot02[19:10];
                        din_pilot01[9:0] <= dout_pilot02[9:0];

                        addra_pilot01    <= addra_pilot01 + 12'd1;
                        addrb_pilot02    <= addrb_pilot02 + 12'd1;
                        cnt_pilot        <= cnt_pilot + 12'd1;
                        cnt_frame        <= cnt_frame + 15'd1;
                        wea_01           <= 1'b1;
                    end
                end
                5'd9: begin
                    if (cnt_frame == 15'd13823) begin

                        if (cnt_bag == 8'd40) begin
                            state_freq <= 5'd10;
                            cnt_bag    <= cnt_bag + 8'd1;
                        end
                        else begin
                            state_freq  <= 5'd3;
                            wea_02      <= 1'b1;
                            din_pilot02 <= {datain_q, datain_i};

                        end

                        cnt_frame <= 15'd0;
                    end
                    else begin
                        cnt_frame <= cnt_frame + 15'd1;
                    end
                end

                5'd10: begin
                    if (cnt_frame == 15'd13695) begin
                        cnt_frame  <= 15'd0;
                        state_freq <= 5'd0;
                    end
                    else begin
                        cnt_frame <= cnt_frame + 15'd1;
                    end

                end

                5'd11: begin
                    if (cnt_cp == 12'd127) begin
                        state_freq    <= 5'd12;
                        cnt_cp        <= 12'd0;
                        wea_01        <= 1'b1;
                        addra_pilot01 <= addra_pilot01 + 12'd1;
                        cnt_frame     <= cnt_frame + 15'd1;
                        if (datain_q == 10'd512) din_pilot01[19:10] <= 10'd511;
                        else din_pilot01[19:10] <= -datain_q;

                        din_pilot01[9:0] <= datain_i[9:0];
                    end
                    else begin
                        cnt_cp     <= cnt_cp + 12'd1;
                        state_freq <= state_freq;
                        cnt_frame  <= cnt_frame + 15'd1;
                    end

                end


                5'd12: begin
                    if (cnt_pilot == 12'd1023) begin
                        addra_pilot01 <= 12'd0;
                        cnt_pilot     <= 12'd0;
                        state_freq    <= 12'd2;
                        cnt_frame     <= cnt_frame + 15'd1;
                        cnt_bag       <= cnt_bag;
                        wea_01        <= 1'b0;
                    end
                    else begin
                        if (datain_q == 10'd512) din_pilot01[19:10] <= 10'd511;
                        else din_pilot01[19:10] <= -datain_q;

                        din_pilot01[9:0] <= datain_i[9:0];
                        addra_pilot01    <= addra_pilot01 + 12'd1;
                        cnt_pilot        <= cnt_pilot + 12'd1;
                        cnt_frame        <= cnt_frame + 15'd1;
                        cnt_bag          <= cnt_bag;
                        wea_01           <= 1'b1;
                    end
                end

                5'd13: begin
                    if (cnt_cp == 12'd127) begin
                        state_freq    <= 5'd14;
                        cnt_cp        <= 12'd0;
                        cnt_frame     <= cnt_frame + 15'd1;
                        din_pilot02   <= {datain_q, datain_i};
                        addra_pilot02 <= addra_pilot02 + 12'd1;
                        wea_02        <= 1'b1;
                    end
                    else begin
                        cnt_cp     <= cnt_cp + 12'd1;
                        state_freq <= state_freq;
                        cnt_frame  <= cnt_frame + 15'd1;
                    end

                end


                5'd14: begin
                    if (cnt_pilot == 12'd1023) begin
                        addra_pilot02 <= 12'd0;
                        cnt_pilot     <= 12'd0;
                        state_freq    <= 5'd4;
                        cnt_frame     <= cnt_frame + 15'd1;
                        cnt_bag       <= cnt_bag;
                        wea_02        <= 1'b0;
                    end
                    else begin
                        din_pilot02   <= {datain_q, datain_i};
                        addra_pilot02 <= addra_pilot02 + 12'd1;
                        cnt_pilot     <= cnt_pilot + 12'd1;
                        cnt_frame     <= cnt_frame + 15'd1;
                        cnt_bag       <= cnt_bag;
                        wea_02        <= 1'b1;
                    end
                end

            endcase
        end

    end


    atan2_freq atan2_freq (
        .aclk                   (clk),                         // input wire aclk
        .s_axis_cartesian_tvalid(acc_rdy),                     // input wire s_axis_cartesian_tvalid
        .s_axis_cartesian_tdata ({acc_com_q, acc_com_i}),      // 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 [11:0] DDS_trunc_i;
    wire [11:0] DDS_trunc_q;
    wire        DDS_trunc_nd;
    wire        Demod_ovrflw;



    reg  [11:0] DDSin_i;
    reg  [11:0] DDSin_q;

    always @(posedge clk) begin
        if (dataout_16[19] == 1'b1) DDSin_q <= {2'b11, dataout_16[19:10]};
        else DDSin_q <= {2'b00, dataout_16[19:10]};
        if (dataout_16[9] == 1'b1) DDSin_i <= {2'b11, dataout_16[9:0]};
        else DDSin_i <= {2'b00, dataout_16[9:0]};
    end


    DDS_compensate DDS_compensate_mx (
        .clk              (clk),
        .rst              (rst),                //捕获模块复位受到发端影响，测试捕获情冿
        .din_nd           (1'd1),
        .din_i            (DDSin_i),
        .din_q            (DDSin_q),
        .acq_nd           (1'b0),               // 捕获信息 //捕获到了复位丿欿
        .dfreq_word_nd    (dfreq_word_nd_DDS),  // 频偏NCO信息
        .dfreq_word       (freq_out),
        .framesync_success(framesync),          // 帧同步标志，用于切码	
        .demod_trunc_i    (DDS_trunc_i),
        .demod_trunc_q    (DDS_trunc_q),
        .demod_trunc_nd   (DDS_trunc_nd),
        .Demod_ovrflw     (Demod_ovrflw)
    );

    always @(posedge clk) begin
        if (DDS_trunc_i[11:9] == 3'b000 || DDS_trunc_i[11:9] == 3'b111) begin
            dataout_i <= DDS_trunc_i[9:0];
        end
        else begin
            dataout_i <= (DDS_trunc_i[11] == 1'b1) ? 10'b10_0000_0000 : 10'b01_1111_1111;
        end
        if (DDS_trunc_q[11:9] == 3'b000 || DDS_trunc_q[11:9] == 3'b111) begin
            dataout_q <= DDS_trunc_q[9:0];
        end
        else begin
            dataout_q <= (DDS_trunc_q[11] == 1'b1) ? 10'b10_0000_0000 : 10'b01_1111_1111;
        end
    end

    reg [19:0] cnt_rst = 20'd0;
    always @(posedge clk) begin
        if (framesync_delay_d8 && !framesync_delay) begin
            cnt_rst  <= 20'd0;
            rst_freq <= 1'b0;
        end
        else begin
            if (cnt_rst == 20'd580620) begin
                rst_freq <= 1'b1;
                cnt_rst  <= cnt_rst + 20'd1;
            end
            else if (cnt_rst == 20'd580621) begin
                rst_freq <= 1'b0;
                cnt_rst  <= cnt_rst;
            end
            else begin
                cnt_rst  <= cnt_rst + 20'd1;
                rst_freq <= 1'b0;
            end
        end
    end

endmodule