--
-- pllspu.vhd
--
-- PLL and Signal Processing Unit for CERN TMS PUPE FPGA design
--
-- Based on pll3.vhd provided by G. Kasprowicz (CERN)
--
-- Version 1.0 4/1/06 A.McCormick (Alpha Data)
--
-- Version 1.1 13/3/06 A.McCormick (Alpha Data)
-- Added 2nd loop filter


library ieee;
use ieee.std_logic_1164.all;
use ieee.std_logic_signed.all;
use ieee.std_logic_arith.all;
use ieee.std_logic_misc.all;

entity pllspu is
  
  port (
    clk               : in  std_logic;
    rst               : in  std_logic;
    ten_mhz_clk       : in  std_logic;
    -- Timing Inputs (already edge detected)
    cycle_start       : in  std_logic;
    cycle_start_ten_mhz : in  std_logic;
    cycle_stop        : in  std_logic;
    cal_start         : in  std_logic;
    cal_stop          : in  std_logic;
    injection         : in  std_logic;
    hchange           : in  std_logic;
    fref              : in  std_logic;
    frev_in           : in  std_Logic;  -- (non edge detected)
    -- PLL Registers
    init              : in  std_logic;
    loop_ctrl         : in  std_logic;
    dds_freq_limit_en : in  std_logic;
    blr_disable       : in  std_logic;
    pll_frequency     : in  std_logic_vector(31 downto 0);
    pll_freqdelay     : in  std_logic_vector(31 downto 0);
    pll_phasedelay    : in  std_logic_vector(31 downto 0);
    pll_gain          : in  std_logic_vector(31 downto 0);
    dds_freq_min      : in  std_logic_vector(31 downto 0);
    dds_freq_max      : in  std_logic_vector(31 downto 0);
    use_neighbour_pll : in  std_logic_vector(3 downto 0);
    -- Cross connections to other pupes plls
    pll_mixed_sigma0_out : out std_logic_vector(13 downto 0);
    pll_mixed_sigma1_out : out std_logic_vector(13 downto 0);
    pll_mixed_sigma0a_in : in std_logic_vector(13 downto 0);
    pll_mixed_sigma1a_in : in std_logic_vector(13 downto 0);
    pll_mixed_sigma0b_in : in std_logic_vector(13 downto 0);
    pll_mixed_sigma1b_in : in std_logic_vector(13 downto 0);  
    -- ADC Inputs
    sigma             : in  std_logic_vector(13 downto 0);
    deltax            : in  std_logic_vector(13 downto 0);
    deltay            : in  std_logic_vector(13 downto 0);
    -- Switch Table Read Only Interface
    switch_table_addr : out std_logic_vector(3 downto 0);
    switch_table_data : in  std_logic_vector(31 downto 0);
    -- Phase Table Read Only Interface
    phase_table_addr  : out std_logic_vector(12 downto 0);
    phase_table_data  : in  std_logic_vector(7 downto 0);
    -- Outputs to Data Logging Module
    result_valid      : out std_logic;
    result_0          : out std_logic_vector (23 downto 0);
    result_1          : out std_logic_vector (23 downto 0);
    result_2          : out std_logic_vector (23 downto 0);
    acq               : out std_logic;
    cycle             : out std_logic_vector(31 downto 0);
    ctime             : out std_logic_vector(31 downto 0);
    ms_pulse_out      : out std_Logic;
    phase_table_sync  : out std_Logic;
    hchange_sync      : out std_logic;
    injection_sync    : out std_logic;
    cal_start_sync    : out std_logic;
    -- Output to IO
    phase_table_msb_out : out std_logic;
    -- State Machine PCI Control
    cycle_init_val    : in  std_logic_vector(31 downto 0);
    cycle_val_set     : in  std_logic;
    switch_state_reg  : out std_logic_vector(3 downto 0);
    -- Diagnostics Output
    diagnostics       : out std_logic_vector(255 downto 0)
    );

end pllspu;

architecture rtl of pllspu is

  --BLR
  signal e0, e1, e2                                  : std_logic_vector(23 downto 0);
  signal b0, b1, b2                                  : std_logic_vector(23 downto 0);
  signal x0, x1, x2, y0, y1, y2                      : std_logic_vector(23 downto 0);
  signal BLR_data_in_0, BLR_data_in_1, BLR_data_in_2 : std_logic_vector (23 downto 0);
  signal BLR_data_out_0                              : std_logic_vector (23 downto 0);
  signal BLR_data_out_1                              : std_logic_vector (23 downto 0);
  signal BLR_data_out_2                              : std_logic_vector (23 downto 0);

  --DDS
  signal ph_table_addr                    : std_logic_vector (12 downto 0);
  signal dds_ph, dds_freq, dds_freq_limit : std_logic_vector (31 downto 0);
  signal f_error_dds                      : std_logic_vector(31 downto 0);

  signal load_dds_freq, dds_freq_OVR, dds_freq_UR, clk62 : std_logic := '0';
  --multiplier
  signal mul_data_in1,mul_data_in2,mult_out1,mult_out2, mult_neg_data_in1,mult_neg_data_in2         : std_logic_vector (13 downto 0);

  --filter
  signal n1, n2, n3, n4, n5, n6, s3, s4, s5, s6, filter_data_out : std_logic_vector(31 downto 0);
  signal 	filter1_in : std_logic_vector (15 downto 0);
  signal nb1, nb2, nb3, nb4, nb5, nb6, sb3, sb4, sb5, sb6 : std_logic_vector(31 downto 0);
  signal 	filter2_in : std_logic_vector (15 downto 0);	
  -- integrators
  type   STATE_TYPE is (idle, integrate, ready, reset_acc);
  signal state                                                                           : STATE_TYPE;
  signal accumulate_0, accumulate_1, accumulate_2, result0_tmp, result1_tmp, result2_tmp : std_logic_vector (23 downto 0) := (others => '0');
  signal acc_reset, result_Write_en                                        : std_logic;

  type shifted_ferror_type is array (natural range <>) of std_logic_vector(31 downto 0);


  signal switch_state   : std_logic_vector(3 downto 0);
  signal switch_reading : std_logic;
  signal rf_select1      : std_logic;
  signal rf_select2      : std_logic;

  signal cycle_count : std_logic_vector(15 downto 0);
  signal ctime_count : std_logic_vector(15 downto 0);
  signal ctimer      : std_logic_vector(16 downto 0);
  signal ms_pulse : std_logic;
  signal ms_pulse_reg : std_logic_vector(7 downto 0);

  signal hchange_switch : std_logic;
  signal hchange_temp : std_Logic;
  signal phase_table_msb_falling : std_Logic;
  signal phase_table_msb,phase_table_msb_reg : std_logic;
  signal phase_table_addr_i  : std_logic_vector(12 downto 0);

  signal LO_pulse1,LO_pulse2,blr_pulse,gate_pulse : std_logic;
  signal f_error,f_err : std_logic_vector(31 downto 0);
  signal Frev_num		:	std_logic_vector(13 downto 0);

  signal pll_init : std_logic;
  signal pll_init_count : std_logic_vector(15 downto 0);
  signal sel_filter : std_Logic;

  signal pll_msb : std_logic;
  signal use_pll_for_lo1 : std_logic;
  signal use_pll_for_lo2 : std_logic;
  
  signal event_delayed_i : std_logic;
 
  signal injection_switch : std_logic;
  signal cal_start_switch : std_logic;

  signal event : std_logic;
  
  signal event_delay_count : std_logic_vector(3 downto 0);
  signal event_delayed_switch : std_logic;

  signal other_plls1a,other_plls2a : std_logic_vector(13 downto 0);
  signal other_plls1b,other_plls2b : std_logic_vector(13 downto 0);

  signal injection_reg : std_logic;
  signal hchange_reg : std_logic;
  signal cal_start_reg : std_logic;
  
begin  -- rtl

  cycle            <= EXT(cycle_count, 32);
  
  switch_state_reg <= switch_state;

  ctimer0: process (ten_mhz_clk, rst)
  begin  -- process ctimer0
    if rst = '1' then                   -- asynchronous reset 
      ctime_count    <= (others => '0');
      ctimer         <= CONV_STD_LOGIC_VECTOR(9999, 17);
      ms_pulse       <= '0';
    elsif ten_mhz_clk'event and ten_mhz_clk = '1' then  -- rising clock edge
      ms_pulse <= not OR_reduce(ctimer);
      if cycle_start_ten_mhz = '1' then
        ctimer      <= CONV_STD_LOGIC_VECTOR(9999, 17);
        ctime_count <= (others => '0');
      else
        if OR_reduce(ctimer) = '0' then
          ctimer      <= CONV_STD_LOGIC_VECTOR(9999, 17);
          ctime_count <= ctime_count+1;
        else
          ctimer <= ctimer-1;
        end if;
      end if;
    end if;
  end process ctimer0;

  
  switch_fsm0 : process (clk, rst)
  begin  -- process switch_fsm0
    if rst = '1' then                   -- asynchronous reset 
      switch_state   <= (others => '1');
      switch_reading <= '0';
      cycle_count    <= (others => '0');
      ms_pulse_reg <= (others => '0');
      ms_pulse_out <= '0';
      ctime <= (others => '0');
      hchange_temp <= '0';
      hchange_switch <= '0';
      injection_reg <= '0';
      hchange_reg <= '0';
    elsif clk'event and clk = '1' then  -- rising clock edge
      ms_pulse_reg <= ms_pulse_reg (6 downto 0) & ms_pulse;
      -- Update CTIME 56ns after rising edge of MS Pulse
      -- to give plenty of time for ctime_count to settle
      if ms_pulse_reg(7) = '0' and ms_pulse_reg(6) = '1' then
        ms_pulse_out <= '1';
        ctime <= EXT(ctime_count, 32);
      else
        ms_pulse_out <= '0';
      end if;
      if cycle_val_set = '1' then
        cycle_count <= cycle_init_val(15 downto 0);
      elsif cycle_stop = '1' then
        cycle_count <= cycle_count+1;
      end if;
      if init = '1' then
        switch_state   <= (others => '1');
        switch_reading <= '0';
      else
        -- 1 cycle delay between switching states and next state switch table
        -- data being available
        if switch_reading = '1' then
          switch_reading <= '0';
        else
          if switch_state = "1111" or switch_state = "1110" then
            if cycle_start = '1' then
              switch_state   <= "0000";
              switch_reading <= '1';
            end if;
          else
            if cycle_stop = '1' then
              switch_state   <= switch_table_data(11 downto 8);
              switch_reading <= '1';
            end if;
            if cal_stop = '1' then
              switch_state   <= switch_table_data(15 downto 12);
              switch_reading <= '1';
            end if;
            if cal_start_switch = '1' and phase_table_msb_falling = '1' then
              switch_state   <= switch_table_data(19 downto 16);
              switch_reading <= '1';
            end if;
            if injection_switch = '1' and phase_table_msb_falling = '1' then
              switch_state   <= switch_table_data(23 downto 20);
              switch_reading <= '1';
            end if;
            if hchange_switch = '1' and phase_table_msb_falling = '1' then
              switch_state   <= switch_table_data(27 downto 24);
              switch_reading <= '1';
            end if;
            if event_delayed_switch = '1' and phase_table_msb_falling = '1'  then
              switch_state   <= switch_table_data(31 downto 28);
              switch_reading <= '1';
            end if;
          end if;
        end if;
      end if;
      -- Delay HCHANGE Effect till cycle table msb falling edge
      -- Reset on Cycle Start
      if cycle_start = '1' then
        hchange_temp <= '0';
        hchange_switch <= '0';
        injection_switch <= '0';
        cal_start_switch <= '0';
      else
        if hchange = '1' and hchange_reg = '0' then
--          hchange_temp <= '1';
--        elsif ms_pulse_reg(7) = '0' and ms_pulse_reg(6) = '1' and hchange_temp = '1' then
--          hchange_temp <= '0';
          hchange_switch <= '1';
        elsif hchange_switch = '1' and phase_table_msb_falling = '1' then
          hchange_switch <= '0';
        end if;
        if injection = '1' and injection_reg = '0' then
          injection_switch <= '1';
        elsif injection_switch = '1' and phase_table_msb_falling = '1' then
          injection_switch <= '0';
        end if;
        if cal_start = '1' and cal_start_reg = '0' then
          cal_start_switch <= '1';
        elsif cal_start_switch = '1' and phase_table_msb_falling = '1' then
          cal_start_switch <= '0';
        end if;
      end if;
      injection_reg <= injection;
      hchange_reg <= hchange;
      cal_start_reg <= cal_start;
      
      use_pll_for_lo2 <= switch_table_data(5);
      use_pll_for_lo1 <= switch_table_data(4);
      sel_filter <= switch_table_data(3);
      rf_select2 <= switch_table_data(2);
      rf_select1 <= switch_table_data(1);
      acq       <= switch_table_data(0);
    end if;
  end process switch_fsm0;

  hchange_sync <= hchange_switch and phase_table_msb_falling;
  injection_sync <= injection_switch and phase_table_msb_falling;
  cal_start_sync <= cal_start_switch and phase_table_msb_falling;
  

  delay_event: process (clk, rst)
  begin  -- process delay_event
    if rst = '1' then                   -- asynchronous reset
      event_delayed_i <= '0';
      event <= '0';
      event_delay_count <= (others => '0');
      event_delayed_switch <= '0';
    elsif clk'event and clk = '1' then  -- rising clock edge
      event <= cycle_start or cycle_stop or cal_stop or cal_start or ((hchange_switch or injection_switch) and phase_table_msb_falling);
      if OR_reduce(event_delay_count) = '0' then
        if event = '1' then
          event_delay_count <= CONV_STD_LOGIC_VECTOR(1,12);
        end if;       
      else
        if phase_table_msb_falling = '1' then
          event_delay_count <= event_delay_count+1;
        end if;       
      end if;
      event_delayed_i <= AND_reduce(event_delay_count);
      if event_delayed_i = '1' then
        event_delayed_switch <= '1';
      elsif event_delayed_switch = '1' and phase_table_msb_falling = '1'  then
        event_delayed_switch <= '0';
      end if;
    end if;
  end process delay_event;
  
  switch_table_addr <= switch_state;


  phase_table_addr_i(12 downto 9) <= switch_state;
  phase_table_addr_i(8 downto 0)  <= dds_ph(31 downto 23) + pll_phasedelay(8 downto 0);
  phase_table_addr <= phase_table_addr_i;

  pll_msb <= dds_ph(31);
  phase_table_msb <= phase_table_addr_i(8);
  process (clk)
  begin  -- process
    if clk'event and clk = '1' then  -- rising clock edge
      phase_table_msb_reg <= phase_table_msb;
      -- falling edge of phase table
      phase_table_msb_falling <= (not phase_table_msb) and phase_table_msb_reg;
      --phase_table_sync <= not phase_table_msb and phase_table_msb_reg;
      phase_table_sync <= (not phase_table_msb) and phase_table_msb_reg;
      phase_table_msb_out <= phase_table_msb_reg;
    end if;
  end process;
  
  --LO_out     <= phase_table_data(0);
  LO_pulse1   <= phase_table_data(0) when use_pll_for_lo1 = '0' else not pll_msb;
  BLR_pulse  <= phase_table_data(1);
  GATE_pulse <= phase_table_data(2);
  LO_pulse2   <= phase_table_data(3) when use_pll_for_lo2 = '0' else not pll_msb;

  -- Loop Gain Control
  -- purpose: Shifts f_error by number of bits specified by in "loop gain"
  loop_gain0 : process (pll_gain, f_error)
    variable shifted_ferror : shifted_ferror_type(0 to 15);
    variable loop_gain_int  : integer;
  begin  -- process loop_gain0
    loop_gain_int     := CONV_INTEGER(('0' & pll_gain(19 downto 16)));
    shifted_ferror(0) := f_error;
    for i in 1 to 15 loop
      shifted_ferror(i) := SXT(f_error(31 downto i), 32);
    end loop;  -- i
    F_ERR <= shifted_ferror(loop_gain_int);
  end process loop_gain0;

--**************************************************************************************************************    
--****************************   Input signal selection   ******************************************************    
--**************************************************************************************************************     
--selection of algorithm input signal
  process(clk)
  begin
    if rising_edge(clk) then
      if rf_select1 = '1' then
        mul_data_in1 <= sigma;
        mult_neg_data_in1 <= (not sigma) +1;
      else
        if Frev_in = '1' then
          mul_data_in1 <= pll_gain(13 downto 0);
          mult_neg_data_in1 <= (not pll_gain(13 downto 0)) +1;
        else
          mult_neg_data_in1 <= pll_gain(13 downto 0);
          mul_data_in1 <= (not pll_gain(13 downto 0)) +1;
        end if;
      end if;
      if rf_select2 = '1' then
        mul_data_in2 <= sigma;
        mult_neg_data_in2 <= (not sigma) +1;
      else
        if Frev_in = '1' then
          mul_data_in2 <= pll_gain(13 downto 0);
          mult_neg_data_in2 <= (not pll_gain(13 downto 0)) +1;
        else
          mult_neg_data_in2 <= pll_gain(13 downto 0);
          mul_data_in2 <= (not pll_gain(13 downto 0)) +1;
        end if;
      end if;
    end if;  
  end process;
--generation of bipolar revolution frequency word
--  mult_neg_data_in1 <= (not mul_data_in1) + 1;
--  mult_neg_data_in2 <= (not mul_data_in2) + 1;
--  Frev_num    <= pll_gain(13 downto 0) when Frev_in = '1'  else (not pll_gain(13 downto 0)) +1;
--**************************************************************************************************************    
--****************************                 DDS            ******************************************************    
--**************************************************************************************************************     
  f_error_dds <= F_ERR;

  -- Generate 62.5 MHz Clock
--  process(clk)
--  begin
--    if rising_edge(clk) then 
--      clk62 <= not clk62;
--    end if;
--  end process;
  clk62 <= '1';

  process(clk,rst)
  begin
    if rst = '1' then
      dds_ph       <= (others => '0');
      dds_freq     <= (others => '0');
      dds_freq_OVR <= '0';
      dds_freq_UR  <= '0';
      pll_init_count <= (others => '0');
      pll_init <= '0';
    elsif rising_edge(clk) then 
      if cycle_start = '1'  then
        pll_init_count <= pll_freqdelay(15 downto 0);
        --pll_init <= not OR_reduce(pll_freqdelay(15 downto 0));
        pll_init <= '1';
      else
        if OR_reduce(pll_init_count) = '1' and ms_pulse_reg(7) = '0' and ms_pulse_reg(6) = '1' then
          pll_init_count <= pll_init_count-1;
          if pll_init_count = CONV_STD_LOGIC_VECTOR(1,16) then
            pll_init <= '1';
          else
            pll_init <= '0';
          end if;
        else
          pll_init <= '0';
        end if;
      end if;
      if pll_init = '1' then
        dds_ph       <= (others => '0');
        dds_freq     <= pll_frequency;
        dds_freq_OVR <= '0';
        dds_freq_UR  <= '0';
      elsif clk62 = '1' then
        dds_ph <= dds_ph + dds_freq;
        if dds_freq > dds_freq_max then
          dds_freq_OVR <= '1';
          dds_freq_UR  <= '0';
        elsif dds_freq < dds_freq_min then
          dds_freq_OVR <= '0';
          dds_freq_UR  <= '1';
        end if;

        if load_dds_freq = '1' then
          dds_freq <= dds_freq_limit;
        elsif loop_ctrl = '0' then
          dds_freq     <= dds_freq + f_error_dds;
          dds_freq_OVR <= '0';
          dds_freq_UR  <= '0';
        end if;
      end if;
    end if;
  end process;

  dds_freq_limit <= dds_freq_max when dds_freq_OVR = '1' else dds_freq_min;
  load_dds_freq  <= dds_freq_limit_en and (dds_freq_OVR or dds_freq_UR);

--**************************************************************************************************************    
--****************************             multiplier         ******************************************************    
--**************************************************************************************************************     
--multiplier function call 

    
  mult_out1         <= mul_data_in1 when LO_pulse1 = '0' else mult_neg_data_in1;
  mult_out2         <= mul_data_in2 when LO_pulse2 = '0' else mult_neg_data_in2;

  pll_mixed_sigma0_out <= mult_out1;
  pll_mixed_sigma1_out <= mult_out2;

  
  process(clk)
  begin
    if rising_edge(clk) then
      filter1_in <= SXT(mult_out1, 16);
      filter2_in <= SXT(mult_out2, 16);
      
      if use_neighbour_pll(0) = '0' then
        other_plls1a <= (others => '0');
      else 
        other_plls1a <= pll_mixed_sigma0a_in;
      end if;

      if use_neighbour_pll(1) = '0' then
        other_plls1b <= (others => '0');
      else 
        other_plls1b <= pll_mixed_sigma0b_in;
      end if;

      if use_neighbour_pll(2) = '0' then
        other_plls2a <= (others => '0');
      else 
        other_plls2a <= pll_mixed_sigma1a_in;
      end if;

      if use_neighbour_pll(3) = '0' then
        other_plls2b <= (others => '0');
      else 
        other_plls2b <= pll_mixed_sigma1b_in;
      end if;
      
    end if;
  end process;

--**************************************************************************************************************    
--****************************               filter           ******************************************************    
--**************************************************************************************************************     

  -- Filter input options, only use one
  combine_3_pupes: if false generate
     n1 <= SXT(filter1_in+ SXT(other_plls1a,16) + SXT(other_plls1b,16),32);
     nb1 <= SXT(filter2_in+ SXT(other_plls2a,16) + SXT(other_plls2b,16),32);
  end generate combine_3_pupes;

  independent_pupes: if false generate
    n1 <= SXT(filter1_in,32);
    nb1 <= SXT(filter2_in,32);
  end generate independent_pupes;

  combine_3_pupes_registered: if true generate
    process (clk)
    begin  -- process
      if rising_edge(clk) then
        n1 <= SXT(filter1_in+ SXT(other_plls1a,16) + SXT(other_plls1b,16),32);
        nb1 <= SXT(filter2_in+ SXT(other_plls2a,16) + SXT(other_plls2b,16),32);
      end if;
    end process;
  end generate combine_3_pupes_registered;
  
  process(clk, init, rst)
  begin
    if rst = '1' then
      n2 <= x"00000000"; 
      n3 <= x"00000000";
      n4 <= x"00000000";
      n5 <= x"00000000";
      n6 <= x"00000000";
      s4 <= x"00000000";
      s6 <= x"00000000";  
    elsif rising_edge(clk) then
      if init = '1' then
        n2 <= x"00000000";
        n3 <= x"00000000";
        n4 <= x"00000000";
        n5 <= x"00000000";
        n6 <= x"00000000";
        s4 <= x"00000000";
        s6 <= x"00000000";
      elsif clk62 = '1' then
        n2 <= n1 + n2 - SXT(n2(31 downto 8), 32);
        n3 <= n2 + n3 - SXT(n3(31 downto 8), 32);
        s4 <= n3 - SXT(n3(31 downto 10), 24);
        n4 <= n3 - s4;
        n5 <= n4 + n5 - SXT(n5(31 downto 8), 32);
        s6 <= n5 - SXT(n5(31 downto 10), 32);
        n6 <= n5 - s6;
      end if;
    end if;
  end process;

  

  process(clk, init, rst)
  begin
    if rst = '1' then
      nb2 <= x"00000000";
      nb3 <= x"00000000";
      nb4 <= x"00000000";
      nb5 <= x"00000000";
      nb6 <= x"00000000";
      sb4 <= x"00000000";
      sb6 <= x"00000000";  
    elsif rising_edge(clk) then
      if init = '1' then
        nb2 <= x"00000000";
        nb3 <= x"00000000";
        nb4 <= x"00000000";
        nb5 <= x"00000000";
        nb6 <= x"00000000";
        sb4 <= x"00000000";
        sb6 <= x"00000000";
      elsif clk62 = '1' then
        nb2 <= nb1 + nb2 - SXT(nb2(31 downto 8), 32);
        nb3 <= nb2 + nb3 - SXT(nb3(31 downto 8), 32);
        sb4 <= nb3 - SXT(nb3(31 downto 10), 32);
        nb4 <= nb3 - sb4;
        nb5 <= nb4 + nb5 - SXT(nb5(31 downto 8), 32);
        sb6 <= nb5 - SXT(nb5(31 downto 10), 32);
        nb6 <= nb5 - sb6;
      end if;
    end if;
  end process;

  filter_data_out <= n6(31 downto 0) when sel_filter = '0' else nb6(31 downto 0);
  f_error         <= filter_data_out;

--**************************************************************************************************************    
--****************************             INTEGRATORS        ******************************************************    
--**************************************************************************************************************     

  process(clk,rst)
  begin
    if rst = '1' then
      accumulate_0 <= x"000000";
      accumulate_1 <= x"000000";
      accumulate_2 <= x"000000";
    elsif rising_edge(clk) then
      if acc_reset = '1' then
        accumulate_0 <= x"000000";
        accumulate_1 <= x"000000";
        accumulate_2 <= x"000000";
      elsif GATE_PULSE = '1' then
        accumulate_0 <= accumulate_0 + BLR_data_out_0;
        accumulate_1 <= accumulate_1 + BLR_data_out_1;
        accumulate_2 <= accumulate_2 + BLR_data_out_2;
      end if;
    end if;
  end process;

  process (clk, init,rst)
  begin
    if init = '1'  or rst = '1' then
      state <= idle;
    elsif rising_edge(clk) then
      case state is
        when idle =>
          if GATE_PULSE = '1' then
            state <= integrate;
          else
            state <= idle;
          end if;
          
        when integrate =>
          if GATE_PULSE = '1' then
            state <= integrate;
          else
            state <= ready;
          end if;
          
        when ready =>
          state <= reset_acc;
          
        when reset_acc =>
          state <= idle;
      end case;
    end if;
  end process;

  acc_reset    <= '1' when state = reset_acc     else '0';
  result_valid <= '1' when state = reset_acc     else '0';

  process(clk)
  begin
    if rising_edge(clk) then
      if state = ready then
        result0_tmp <= accumulate_0;
        result1_tmp <= accumulate_1;
        result2_tmp <= accumulate_2;
      end if;
    end if;
  end process;

  result_0 <= result0_tmp;
  result_1 <= result1_tmp;    
  result_2 <= result2_tmp;

--**************************************************************************************************************    
--****************************             BASELINE RESTORATION       **********************************************    
--**************************************************************************************************************     

  BLR_data_in_0 <= SXT(sigma, 24);
  BLR_data_in_1 <= SXT(deltax, 24);
  BLR_data_in_2 <= SXT(deltay, 24);

--accumulate e
  process(clk, init)
  begin
    if init = '1' then
      e0 <= (others => '0');
      e1 <= (others => '0');
      e2 <= (others => '0');
      b0 <= (others => '0');
      b1 <= (others => '0');
      b2 <= (others => '0');
      y0 <= (others => '0');
      y1 <= (others => '0');
      y2 <= (others => '0');
      x0 <= (others => '0');
      x1 <= (others => '0');
      x2 <= (others => '0');
    elsif rising_edge(clk) then
      x0 <= BLR_data_in_0 - b0;
      x1 <= BLR_data_in_1 - b1;
      x2 <= BLR_data_in_2 - b2;
      e0 <= e0+x0;
      e1 <= e1+x1;
      e2 <= e2+x2;
      y0 <= x0 + SXT(e0(23 downto 7), 24);
      y1 <= x1 + SXT(e1(23 downto 7), 24);
      y2 <= x2 + SXT(e2(23 downto 7), 24);
      if BLR_pulse = '1' then
        b0 <= b0 + SXT(y0(23 downto 2), 24);
        b1 <= b1 + SXT(y1(23 downto 2), 24);
        b2 <= b2 + SXT(y2(23 downto 2), 24);
      end if;
    end if;
  end process;

--  BLR_data_out_0 <= b0;
--  BLR_data_out_1 <= b1;
--  BLR_data_out_2 <= b2;
  BLR_data_out_0 <= y0 when blr_disable = '0' else BLR_data_in_0;
  BLR_data_out_1 <= y1 when blr_disable = '0' else BLR_data_in_1;
  BLR_data_out_2 <= y2 when blr_disable = '0' else BLR_data_in_2;

  

  -- Diagnostics
  -- Record 256 bits of signals
  process (clk)
  begin  -- process
    if clk'event and clk = '1' then  -- rising clock edge
      diagnostics(63 downto 56) <= frev_in & pll_msb & phase_table_msb & LO_pulse1 & event_delayed_switch & injection_switch & hchange_switch & LO_pulse2;
      diagnostics(55 downto 48) <= (hchange_switch and phase_table_msb_falling) & sel_filter & rf_select2 & rf_select1 & phase_table_data(7 downto 6) & GATE_pulse & BLR_pulse;
      diagnostics(47 downto 32) <= switch_state & sigma(13 downto 2);
      diagnostics(31 downto 26) <=  hchange & injection & cal_stop & cal_start & cycle_stop & cycle_start;
      diagnostics(25 downto 0) <=  dds_freq(31 downto 6);

      diagnostics(127 downto 120) <= frev_in & pll_msb & phase_table_msb & LO_pulse1 & sigma(13 downto 11) & LO_pulse2;
      diagnostics(115 downto 102) <= mult_out1;
      diagnostics(101 downto 88) <= mult_out2;
      diagnostics(87 downto 64) <= f_error(23 downto 0);

      diagnostics(191 downto 184) <= frev_in & pll_msb & phase_table_msb & LO_pulse1 & sigma(13 downto 11) & LO_pulse2;
      diagnostics(183) <= GATE_pulse;
      diagnostics(182 downto 160) <= b0(22 downto 0);
      diagnostics(159 downto 128) <= SXT(result0_tmp,32);

      diagnostics(255 downto 248) <= frev_in & pll_msb & phase_table_msb & LO_pulse1 & BLR_data_out_0(23) & BLR_data_out_0(12 downto 11)& LO_pulse2;
      
      diagnostics(247) <= BLR_pulse;
      diagnostics(246) <= GATE_pulse;
      diagnostics(245 downto 232) <= sigma;
      diagnostics(231 downto 218) <= deltax;
      diagnostics(217 downto 204) <= deltay;
      diagnostics(198 downto 193) <=  hchange & injection & cal_stop & cal_start & cycle_stop & cycle_start;
--      diagnostics(239 downto 224) <= y0(15 downto 0);
--      diagnostics(223 downto 208) <= e0(15 downto 0);
--      diagnostics(207 downto 192) <= x0(15 downto 0); 
     
    end if;
  end process;
  
  
  
end rtl;
