---------------------------------------------------------------------------
-- NDT Solutions Ltd. Proprietary Information
-- Company Confidential
-- Copyright 2008
--
-- Filename: RamWriteCtrl.vhd
-- Language: VHDL '93
-- Project:  NDTS2100 DSP FPGA
--
-- Design Description:
-- RAM data selection and write control
---------------------------------------------------------------------------

library IEEE;
use IEEE.Std_Logic_1164.all;
use IEEE.Std_Logic_Arith.all;

use work.PCK_CRC32_D16.all;

entity RamWriteCtrl is
  port (nReset            : in  std_ulogic;
        SysClk            : in  std_ulogic;
        LClk              : in  std_ulogic;
        -- Sample data
        SampleIn          : in  std_logic_vector(15 downto 0);
        SampleValid       : in  std_ulogic;
        TestMode          : in  std_ulogic;
        AScanStart        : in  std_ulogic;
        StartCapture      : in  std_ulogic;
        SequenceActive    : in  std_ulogic;
        AScanDone         : out std_ulogic;
        -- Encoder and Gata Data
        EncoderCount0     : in  std_logic_vector(31 downto 0);
        EncoderCount1     : in  std_logic_vector(31 downto 0);
        EncoderCount2     : in  std_logic_vector(31 downto 0);
        EncoderCount3     : in  std_logic_vector(31 downto 0);
        DualEncoderCount  : in  std_logic_vector(31 downto 0);
        PulseCount0       : in  std_logic_vector(31 downto 0);
        PulseCount1       : in  std_logic_vector(31 downto 0);
        PulseCount2       : in  std_logic_vector(31 downto 0);
        PulseCount3       : in  std_logic_vector(31 downto 0);
        GateData          : in  std_logic_vector(31 downto 0);
        GateSelect        : out std_logic_vector(2 downto 0);
        GateDistance      : out std_ulogic;
        -- RAM write controls out
        WriteData         : out std_logic_vector(31 downto 0);
        WriteAEn          : out std_ulogic;
        WriteCEn          : out std_ulogic;
        ResetAScanAddr    : out std_ulogic;
        ResetCScanAddr    : out std_ulogic;
        BankSel           : out std_ulogic;
        -- Register signals
        GateEnable        : in  std_logic_vector(7 downto 0);
        AScanRange        : in  std_logic_vector(23 downto 0);
        SequencesPerBank  : in  std_logic_vector(15 downto 0);
        SequenceSteps     : in  std_logic_vector(4 downto 0);
        StartSequence     : in  std_ulogic;
        SequencesInRam    : out std_logic_vector(15 downto 0);
        SequenceStep      : out std_logic_vector(4 downto 0);
        StorageMode       : in  std_logic_vector(1 downto 0);
        SetDataReady      : out std_ulogic;
        SetNewTrigger     : out std_ulogic;
        SetEarlyTrigError : out std_ulogic;
        SetDataFullError  : out std_ulogic;
        SomeDataReady     : out std_ulogic;
        DataReady         : in  std_ulogic;
        EarlyDataReq      : in  std_ulogic;
        ResetSampling     : in  std_ulogic);
end entity RamWriteCtrl;


architecture Rtl of RamWriteCtrl is
  component SyncPulseUp
    port (InClock  : in  std_ulogic;
          OutClock : in  std_ulogic;
          nReset   : in  std_ulogic;
          DIn      : in  std_ulogic;
          DOut     : out std_ulogic);
  end component;

  component SyncPulseDown
    port (InClock  : in  std_ulogic;
          OutClock : in  std_ulogic;
          nReset   : in  std_ulogic;
          DIn      : in  std_ulogic;
          DOut     : out std_ulogic);
  end component;

  signal PipeData1      : std_logic_vector(15 downto 0);
  signal PipeData2      : std_logic_vector(15 downto 0);
  signal PipeData3      : std_logic_vector(15 downto 0);
  signal SampleDelayed  : std_logic_vector(15 downto 0);
  signal Sample         : std_logic_vector(15 downto 0);
  signal TestSample     : std_logic_vector(15 downto 0);

  -- Control logic States
  type tCtrlStates is (sIdle, sStoreAScan1, sStoreAScan2, sStoreAEncData1, sStoreAEncData2, sStoreACrcData1,
                       sStoreACrcData2, sStoreACrcData3, sStoreACrcData4, sStoreBCrcData1, sStoreBCrcData2,
                       sStoreBCrcData3, sStoreAEncDelay, sStoreCEncData1, sStoreCEncData2, sStoreHeader1,
                       sStoreHeader2, sStoreCDelay, sStoreData1, sStoreData2, sStoreData3, sStoreData4,
                       sStoreData5, sEndStore, sStoreCCrcData1, sStoreCCrcData2, sStoreCCrcData3, sStoreDone);
  signal CtrlState: tCtrlStates;

  signal AScanCount         : unsigned(23 downto 0);
  signal AScanCountZero     : std_ulogic;
  signal AScanEn            : std_ulogic;
  signal SeqClkEn           : std_ulogic;
  signal StepInc            : std_ulogic;
  signal SeqReset           : std_ulogic;
  signal SequenceActiveD    : std_ulogic;
  signal StepCount          : unsigned(4 downto 0);
  signal LastStep           : std_ulogic;
  signal LastSeq            : std_ulogic;
  signal SequencesCount     : unsigned(15 downto 0);
  signal SequencesInRamSys  : unsigned(15 downto 0);
  signal BankDone           : std_ulogic;
  signal BankDoneD          : std_ulogic;
  signal iBankSel           : std_ulogic;

  type tCaptureStates is (sEmpty, sFilling1, sFull1, sFilling2, sFull2);
  signal CaptureState: tCaptureStates;
  signal CaptureDone      : std_ulogic;
  signal StartExtend      : std_ulogic;
  signal DataRead         : std_ulogic;
  signal DataFullError    : std_ulogic;
  signal DataFullReported : std_ulogic;
  signal SetDataReadySys  : std_ulogic;
  signal SomeDataReadySys : std_ulogic;
  signal SomeDataReadySync: std_ulogic;
  signal ResetSamplingSys : std_ulogic;
  signal nResetSampling   : std_ulogic;
  signal EarlyDataReqSync : std_logic_vector(2 downto 0);
  signal EarlyDataReqSys  : std_ulogic;
  signal EarlyDataReqD    : std_ulogic;
  signal EarlyTrigErr     : std_ulogic;
  signal SequencesInRamEn : std_ulogic;

  signal StoredCount      : std_logic_vector(7 downto 0);
  signal CurrentGate      : std_logic_vector(2 downto 0);
  signal GateEnables      : std_logic_vector(7 downto 0);
  signal NoMoreGates      : std_ulogic;

  signal StoreCScanEn     : std_ulogic;

  signal ACrc             : std_logic_vector(31 downto 0);
  signal BCrc             : std_logic_vector(31 downto 0);
  signal CCrc             : std_logic_vector(31 downto 0);
  signal Crc              : std_logic_vector(31 downto 0);
  signal CrcH             : std_logic_vector(15 downto 0);
  signal CrcData          : std_logic_vector(15 downto 0);
  signal ACrcRst          : std_ulogic;
  signal BCrcRst          : std_ulogic;
  signal ACrcEn           : std_ulogic;
  signal BCrcEn           : std_ulogic;
  signal ACrcHEn          : std_ulogic;
  signal BCrcHEn          : std_ulogic;
  signal ACrcEnD          : std_ulogic;
  signal BCrcEnD          : std_ulogic;
  signal BCSwap           : std_ulogic;

  signal EncoderData      : std_logic_vector(31 downto 0);
  signal EncoderSel       : std_logic_vector(3 downto 0);
  signal iWriteData       : std_logic_vector(31 downto 0);
  signal WrDataSel        : std_logic_vector(2 downto 0);
--  signal PulseTest        : std_logic_vector(7 downto 0);

  constant DSelNull       : std_logic_vector(2 downto 0) := "000";
  constant DSelAScanHdr   : std_logic_vector(2 downto 0) := "001";
  constant DSelCScanHdr   : std_logic_vector(2 downto 0) := "010";
  constant DSelAScan      : std_logic_vector(2 downto 0) := "011";
  constant DSelEncoder    : std_logic_vector(2 downto 0) := "100";
  constant DSelGate       : std_logic_vector(2 downto 0) := "101";
  constant DSelACrc       : std_logic_vector(2 downto 0) := "110";
  constant DSelSeqCrc     : std_logic_vector(2 downto 0) := "111";

begin

  -- Delay sample data to match processed data pipeline delay
  DelaySample: process(SysClk)
  begin
    if SysClk'event and SysClk = '1' then
      if SampleValid = '1' then
        PipeData1 <= SampleIn;
        PipeData2 <= PipeData1;
        PipeData3 <= PipeData2;
        if TestMode = '1' then
          SampleDelayed <= TestSample;
        else
          SampleDelayed <= PipeData3;
        end if;
        Sample <= SampleDelayed;
      end if;
      if (AScanStart = '1' or StartExtend = '1') and SampleValid = '1' then
        TestSample <= x"0000";
      elsif SampleValid = '1' then
        TestSample <= unsigned(TestSample) + '1';
      end if;
    end if;
  end process DelaySample;


  SampReset: SyncPulseUp
    port map (InClock  => LClk,
              OutClock => SysClk,
              nReset   => nReset,
              DIn      => ResetSampling,
              DOut     => ResetSamplingSys);

  nResetSampling <= '0' when nReset = '0' or ResetSamplingSys = '1' else '1';

  TrigPulse: SyncPulseDown
    port map (InClock  => SysClk,
              OutClock => LClk,
              nReset   => nResetSampling,
              DIn      => StartCapture,
              DOut     => SetNewTrigger);

  -- A-Scan Counters
  AScanCounter: process(nResetSampling, SysClk)
  begin
    if nResetSampling = '0' then
      AScanCount <= (others => '0');
      AScanCountZero <= '0';
      AScanEn <= '0';
      EarlyTrigErr <= '0';
    elsif SysClk'event and SysClk = '1' then
      EarlyTrigErr <= '0';

      if AScanStart = '1' then
        AScanCount <= unsigned(AScanRange);
        AScanCountZero <= '0';
        AScanEn <= '1';
        if AScanEn = '1' then
          EarlyTrigErr <= '1';
        end if;
      elsif SampleValid = '1' then
        if AScanCountZero = '1' then
          AScanEn <= '0';
        elsif AScanEn = '1' then
          AScanCount <= AScanCount - 1;
          if AScanCount(23 downto 1) = 0 then
            AScanCountZero <= '1';
          else
            AScanCountZero <= '0';
          end if;
        end if;
      end if;
    end if;
  end process AScanCounter;

  AScanDone <= AScanCountZero and AScanCount(0);

  ETrig: SyncPulseDown
    port map (InClock  => SysClk,
              OutClock => LClk,
              nReset   => nResetSampling,
              DIn      => EarlyTrigErr,
              DOut     => SetEarlyTrigError);

  SeqClkEn <= '1'; -- tie for 100MHz operation

  -- Sequence Counters
  SeqCounter: process(nResetSampling, SysClk)
  begin
    if nResetSampling = '0' then
--      SeqClkEn  <= '0';
      StepInc   <= '0';
      SeqReset <= '0';
      StepCount <= (others => '0');
      SequencesCount <= (others => '0');
      SequencesInRamSys <= (others => '0');
      BankDone <= '0';
      BankDoneD <= '0';
      LastStep  <= '0';
      LastSeq <= '0';
    elsif SysClk'event and SysClk = '1' then
--      SeqClkEn <= not SeqClkEn;
      SequenceActiveD <= SequenceActive;
      if CaptureDone = '1' then
        StepInc <= '1';
      elsif SeqClkEn = '1' then
        StepInc <= '0';
      end if;

      if StartSequence = '0' and SequenceActive = '0' then
        SeqReset <= '1';
      else
        SeqReset <= '0';
      end if;

      BankDone <= '0';
      if SeqReset = '1' then
        StepCount <= (others => '0');
        SequencesCount <= (others => '0');
      elsif SeqClkEn = '1' then
        if StepInc = '1' then
          if LastStep = '1' then
            StepCount <= (others => '0');
            if LastSeq = '1' or EarlyDataReqSys = '1' then
              SequencesInRamSys <= SequencesCount + 1;
              SequencesCount <= (others => '0');
              BankDone <= '1';
            else
              SequencesCount <= SequencesCount + 1;
            end if;
          else
            StepCount <= StepCount + 1;
          end if;
        end if;
      end if;
      if SequenceActiveD = '0' and EarlyDataReqSys = '1' then
        StepCount <= (others => '0');
        SequencesInRamSys <= SequencesCount + 1;
        SequencesCount <= (others => '0');
        BankDone <= '1';
      end if;

      if StepCount = unsigned(SequenceSteps) then
        LastStep <= '1';
      else
        LastStep <= '0';
      end if;
      if SequencesCount = unsigned(SequencesPerBank) - 1 then
        LastSeq <= '1';
      else
        LastSeq <= '0';
      end if; 
      BankDoneD <= BankDone;
    end if;
  end process SeqCounter;

  SequenceStep <= std_logic_vector(StepCount);

  SeqNoCapt: SyncPulseDown
    port map (InClock  => SysClk,
              OutClock => LClk,
              nReset   => nResetSampling,
              DIn      => BankDoneD,
              DOut     => SequencesInRamEn);

  SeqNoFF: process(nResetSampling, LClk)
  begin
    if nResetSampling = '0' then
      SequencesInRam <= (others => '0');
    elsif LClk'event and LClk = '1' then
      if SequencesInRamEn = '1' then
        SequencesInRam <= std_logic_vector(SequencesInRamSys);
      end if;
    end if;
  end process SeqNoFF;

  DRead: SyncPulseUp
    port map (InClock  => LClk,
              OutClock => SysClk,
              nReset   => nResetSampling,
              DIn      => DataReady,
              DOut     => DataRead);

  EarlyDataReqFF: process(nResetSampling, LClk)
  begin
    if nResetSampling = '0' then
      EarlyDataReqD <= '0';
    elsif LClk'event and LClk = '0' then -- negative edge to guarantee overlap
      EarlyDataReqD <= EarlyDataReq;
    end if;
  end process EarlyDataReqFF;

  GenEarlyDataReq: process(nResetSampling, SysClk)
  begin
    if nResetSampling = '0' then
      EarlyDataReqSync  <= "000";
      EarlyDataReqSys   <= '0';
    elsif SysClk'event and SysClk = '1' then
      EarlyDataReqSync <= EarlyDataReqSync(1 downto 0) & (EarlyDataReq or EarlyDataReqD);

      if EarlyDataReqSync(2 downto 1) = "01" and CaptureState = sFilling1 then
        EarlyDataReqSys <= '1';
      elsif SequenceActiveD = '0' and SeqClkEn = '1' then
        EarlyDataReqSys <= '0';
      end if;

    end if;
  end process GenEarlyDataReq;

  -- Capture state machine
  CaptureSM: process(nResetSampling, SysClk)
    variable vTemp: std_logic_vector(1 downto 0);
  begin
    if nResetSampling = '0' then
      CaptureState     <= sEmpty;
      DataFullError    <= '0';
      DataFullReported <= '0';
      iBankSel         <= '0';
      SetDataReadySys  <= '0';
      SomeDataReadySys <= '0';
    elsif SysClk'event and SysClk = '1' then
      DataFullError    <= '0';
      SetDataReadySys  <= '0';
      SomeDataReadySys <= '1';

      case CaptureState is
        when sEmpty =>
          -- Both banks are empty or have been downloaded
          SomeDataReadySys <= '0';
          if StartCapture = '1' then
            -- First bank has begun filling
            CaptureState <= sFilling1;
          end if;

        when sFilling1 =>
          -- 1 Bank is filling, one is empty
          if BankDone = '1' then
            -- First bank has completed filling
            CaptureState <= sFull1;
            iBankSel <= not iBankSel;
            SetDataReadySys <= '1';  -- Signal there is data ready to the software
          end if;

        when sFull1 =>
          -- 1 Bank is full, one is empty
          vTemp := DataRead & StartCapture;
          case vTemp is
            when "11" =>
              -- Data in first bank has been read and the next A-Scan has started
              CaptureState <= sFilling1;
            when "10" =>
              -- Data in the first bank has been read
              CaptureState <= sEmpty;
            when "01" =>
              -- The next B-Scan has started
              CaptureState <= sFilling2;
            when others =>
              null;
          end case;

        when sFilling2 =>
          -- 1 Bank of SRAM is full, one is filling
          vTemp := DataRead & BankDone;
          case vTemp is
            when "11" =>
              -- Data in first bank has been read as the second bank has completed filling
              CaptureState <= sFull1;
              iBankSel <= not iBankSel;
              SetDataReadySys <= '1';  -- Signal there is data ready to the software
            when "10" =>
              -- Data in the first bank has been read
              CaptureState <= sFilling1;
            when "01" =>
              -- The second bank has completed filling
              CaptureState <= sFull2;
            when others =>
              null;
          end case;

        when sFull2 =>
          -- Both banks of the SRAM are full
          if DataRead = '1' then
            -- Data in the first bank has been read
            CaptureState <= sFull1;
            iBankSel <= not iBankSel;
            SetDataReadySys <= '1';  -- Signal there is data ready to the software
          end if;

          if StartCapture = '1'  and DataFullReported = '0' then
            -- We have seen a new trigger edge while both banks are full
            -- flag up an error!!!
            DataFullError <= '1';
            DataFullReported <= '1';
          end if;
      end case;

      if DataRead = '1' then
        DataFullReported <= '0';
      end if;
    end if;
  end process CaptureSM;

  BankSel <= iBankSel;

  SetDReady: SyncPulseDown
    port map (InClock  => SysClk,
              OutClock => LClk,
              nReset   => nResetSampling,
              DIn      => SetDataReadySys,
              DOut     => SetDataReady);

  SetDFullErr: SyncPulseDown
    port map (InClock  => SysClk,
              OutClock => LClk,
              nReset   => nResetSampling,
              DIn      => DataFullError,
              DOut     => SetDataFullError);

  SyncFF: process(nResetSampling, LClk)
  begin
    if nResetSampling = '0' then
      SomeDataReadySync <= '0';
      SomeDataReady <= '0';
    elsif LClk'event and LClk = '1' then
      SomeDataReadySync <= SomeDataReadySys;
      SomeDataReady <= SomeDataReadySync;
    end if;
  end process SyncFF;

  -- Generate a signal when C-Scan data is to be stored
  StoreCScanEn <= '1' when StorageMode /= "00" else '0';

  -- Main write state machine
  WriteCtrl: process(nResetSampling, SysClk)
  begin
    if nReset = '0' then
      WrDataSel       <= DSelNull;
      WriteAEn        <= '0';
      WriteCEn        <= '0';

      CtrlState       <= sIdle;
      StartExtend     <= '0';
      CaptureDone     <= '0';
      CurrentGate     <= "000";
      GateDistance    <= '0';
      GateEnables     <= "00000000";
      EncoderSel      <= "0000";

      ACrcRst         <= '0';
      ACrcEn          <= '0';
      BCrcEn          <= '0';
      BCSwap          <= '0';

    elsif SysClk'event and SysClk = '1' then
      WrDataSel       <= DSelNull;
      WriteAEn        <= '0';
      WriteCEn        <= '0';

      ACrcRst         <= '0';
      ACrcEn          <= '0';
      BCrcEn          <= '0';
      BCSwap          <= '0';

      if AScanStart = '1' and SampleValid = '0' then
        StartExtend     <= '1';
      elsif SampleValid = '1' then
        StartExtend     <= '0';
      end if;
        
      case CtrlState is
        when sIdle =>
          -- We are waiting to store A-Scan data
          CaptureDone <= '0';
          GateEnables <= GateEnable;
          WrDataSel   <= DSelAScanHdr;
          EncoderSel  <= "0000";

          if (AScanStart = '1' or StartExtend = '1') and SampleValid = '1' then
            WriteAEn  <= '1';
            WrDataSel <= DSelAScan;
            ACrcRst   <= '1';
            BCrcEn    <= '1';

            CtrlState <= sStoreAScan2;
          end if;

        when sStoreAScan1 =>
          if AScanCountZero = '1' then
            -- Correct amount of data written, A-Scan storing operation complete
            CtrlState <= sStoreACrcData1;
          elsif SampleValid = '1' then
            CtrlState <= sStoreAScan2;
            WriteAEn  <= '1';

            ACrcEn    <= '1';
            BCrcEn    <= '1';
          end if;

        when sStoreAScan2 =>
          if SampleValid = '1' then
            WrDataSel <= DSelAScan;
            CtrlState <= sStoreAScan1;
          end if;

        when sStoreACrcData1 =>
          WrDataSel <= DSelACrc;
          CtrlState <= sStoreACrcData2;

        when sStoreACrcData2 =>
          WriteAEn  <= '1';
          BCrcEn    <= '1';
          if LastStep = '1' then
            CtrlState <= sStoreACrcData3;
          else
            CtrlState <= sStoreBCrcData3;
          end if;

        when sStoreACrcData3 =>
          CtrlState <= sStoreACrcData4;

        when sStoreACrcData4 =>
          CtrlState <= sStoreBCrcData1;

        when sStoreBCrcData1 =>
          CtrlState <= sStoreBCrcData2;
          WrDataSel <= DSelSeqCrc;

        when sStoreBCrcData2 =>
          CtrlState <= sStoreBCrcData3;
          WriteAEn  <= '1';

        when sStoreBCrcData3 =>
          if LastStep = '1' then
            -- This is the last A-Scan of the element array
            --Store encoder data
            CtrlState <= sStoreAEncDelay;
            WrDataSel <= DSelEncoder;
          else
            if StoreCScanEn = '1' then
              -- C-Scan data storage is enabled
              CtrlState <= sStoreHeader1;
              WrDataSel <= DSelCScanHdr;
              BCSwap    <= '1';
            else
              -- C-Scan data storage is disabled
              -- Go back to the idle state
              CtrlState   <= sIdle;
              CaptureDone <= '1';
            end if;
          end if;

        when sStoreAEncDelay =>
          CtrlState   <= sStoreAEncData2;
          WriteAEn    <= '1';
          EncoderSel  <= unsigned(EncoderSel) + 1;

        when sStoreAEncData1 =>
          if EncoderSel = "0000" then
            -- Encoder values have been stored in the A-Scan data area
            if StoreCScanEn = '1' then
              -- C-Scan data storage is enabled
              CtrlState <= sStoreHeader1;
              -- Select C-Scan header for SRAM data
              wrDataSel <= DSelCScanHdr;
              BCSwap    <= '1';
            else
              -- C-Scan data storage is disabled
              -- GO back to the idle state
              CtrlState   <= sIdle;
              CaptureDone <= '1';
            end if;
          else
            WriteAEn    <= '1';
            EncoderSel  <= unsigned(EncoderSel) + 1;
            CtrlState <= sStoreAEncData2;
          end if;

        when sStoreAEncData2 =>
          CtrlState   <= sStoreAEncData1;
          WrDataSel   <= DSelEncoder;

        when sStoreHeader1 =>
          WriteCEn  <= '1';
          BCrcEn    <= '1';
          CtrlState <= sStoreHeader2;

        when sStoreHeader2 =>
          CtrlState <= sStoreData1;

        when sStoreData1 =>
          CtrlState <= sStoreData2;
          WrDataSel <= DSelGate;

        when sStoreData2 =>
          -- Check for enabled gate
          if GateEnables(0) = '1' then
            -- The current gate is enabled, write its results to SRAM
            -- Begin store operation
            WriteCEn  <= '1';
            CtrlState <= sStoreData3;
            BCrcEn    <= '1';
          else
            -- The current gate is not enabled, check the next one
            -- If there is one
            if NoMoreGates = '0' then
              CurrentGate <= unsigned(CurrentGate) + 1;
              GateEnables <= '0' & GateEnables(7 downto 1);

              CtrlState  <= sStoreData1;

            else
              -- All data for enabled gates has been written to SRAM
              -- Store C-Scan CRC
              if LastStep = '1' then
                CtrlState <= sStoreCCrcData1;
              else
                -- Store C-Scan data
                CtrlState <= sStoreDone;
              end if;
            end if;
          end if;


        when sStoreData3 =>
          GateDistance  <= '1';
          CtrlState     <= sStoreData4;

        when sStoreData4 =>
          CtrlState <= sStoreData5;
          WrDataSel <= DSelGate;

        when sStoreData5 =>
          WriteCEn  <= '1';
          BCrcEn    <= '1';
          CtrlState <= sEndStore;

        when sEndStore =>
          GateDistance  <= '0';
          CurrentGate   <= unsigned(CurrentGate) + 1;
          GateEnables   <= '0' & GateEnables(7 downto 1);
          CtrlState     <= sStoreData1;

        when sStoreCCrcData1 =>
          CtrlState <= sStoreCCrcData2;
          WrDataSel <= DSelSeqCrc;

        when sStoreCCrcData2 =>
          CtrlState <= sStoreCCrcData3;
          WriteCEn  <= '1';

        when sStoreCCrcData3 =>
          if LastStep = '1' then
            CtrlState   <= sStoreCDelay;
            -- Select Encoder data for SRAM
            WrDataSel   <= DSelEncoder;
            EncoderSel  <= "0000";
          else
            CtrlState    <= sStoreDone;
          end if;

        when sStoreCDelay =>
          CtrlState   <= sStoreCEncData2;
          WriteCEn    <= '1';
          EncoderSel  <= unsigned(EncoderSel) + 1;

        when sStoreCEncData1 =>
          if EncoderSel = "0000" then
            CtrlState <= sStoreDone;
          else
            CtrlState <= sStoreCEncData2;
            WriteCEn    <= '1';
            EncoderSel  <= unsigned(EncoderSel) + 1;
          end if;

        when sStoreCEncData2 =>
          CtrlState <= sStoreCEncData1;
          WrDataSel <= DSelEncoder;

        when sStoreDone =>
          CaptureDone <= '1';
          CtrlState   <= sIdle;
          BCSwap      <= '1';
          CurrentGate <= "000";

        when others =>
          null;
      end case;

    end if;
  end process WriteCtrl;

  GenAddrResets: process(nResetSampling, SysClk)
  begin
    if nResetSampling = '0' then
      ResetAScanAddr <= '0';
      ResetCScanAddr <= '1';
    elsif SysClk'event and SysClk = '1' then
      if BankDone = '1' then
        ResetAScanAddr <= '1';
        ResetCScanAddr <= '1';
      else
        ResetAScanAddr <= '0';
        ResetCScanAddr <= '0';
      end if;

      if StorageMode = "01" then -- one AScan
        if CaptureDone = '1' then
          ResetAScanAddr <= '1';
        end if;
      elsif StorageMode = "11" then -- one BScan
        if CaptureDone = '1' and LastStep = '1' then
          ResetAScanAddr <= '1';
        end if;
      end if;

    end if;
  end process GenAddrResets;
  

  GateSelect <= CurrentGate;

  GenNoMoreGates: process(nReset, SysClk)
  begin
    if nReset = '0' then
      NoMoreGates <= '0';
    elsif SysClk'event and SysClk = '1' then
      if GateEnables(7 downto 1) = "0000000" then
        NoMoreGates <= '1';
      else
        NoMoreGates <= '0';
      end if;
    end if;
  end process GenNoMoreGates;

  -- Encoder Data
  MuxEncoderData: process(SysClk)
  begin
    if SysClk'event and SysClk = '1' then
      case (EncoderSel) is
        when "0000" =>
          EncoderData <= EncoderCount0;
--          PulseTest <= unsigned(PulseTest) + '1';
        when "0001" =>
          EncoderData <= EncoderCount1;
        when "0010" =>
          EncoderData <= EncoderCount2;
        when "0011" =>
          EncoderData <= EncoderCount3;
        when "0100" =>
          EncoderData <= DualEncoderCount;
        when "0101" =>
          EncoderData <= PulseCount0;
        when "0110" =>
          EncoderData <= PulseCount1;
        when "0111" =>
          EncoderData <= PulseCount2;
        when "1000" =>
          EncoderData <= PulseCount3;
--          EncoderData <= x"000000" & PulseTest;
        when others =>
          EncoderData <= (others => '0');
      end case;    
    end if;
  end process MuxEncoderData;

  -- Main Data Mux
  MuxWriteData: process(nReset, SysClk)
  begin
    if nReset = '0' then
      iWriteData  <= (others => '0');
      StoredCount <= (others => '0');
    elsif SysClk'event and SysClk = '1' then

      case WrDataSel is
        when DSelNull =>
          null;

        when DSelAScanHdr =>
          iWriteData(7 downto 0)  <= "000" & std_logic_vector(StepCount);
          iWriteData(31 downto 8) <= X"41534E";  -- ASCII 'A' 'S' 'N' for A-SCAN data
          StoredCount <= "000" & std_logic_vector(StepCount);

        when DSelCScanHdr =>
          iWriteData(7 downto 0) <= StoredCount;
          iWriteData(31 downto 8) <= X"43534E";  -- ASCII 'C' 'S' 'N' for C-SCAN data

        when DSelAScan =>  -- A-Scan Data
          iWriteData(15 downto 0)  <= SampleDelayed;
          iWriteData(31 downto 16) <= Sample;

        when DSelEncoder =>
          iWriteData <= EncoderData;

        when DSelGate =>
          iWriteData <= GateData;

        when DSelACrc =>  -- A-Scan CRC
          iWriteData <= not ACrc;

        when DSelSeqCrc =>  -- Sequence/C-Scan CRC
          iWriteData <= not BCrc;

        when others => null;
      end case;
    end if;
  end process MuxWriteData;

  WriteData <= iWriteData;

  -- CRC calc
  GenCrc: process(nReset, SysClk)
  begin
    if nReset = '0' then
      ACrc <= (others => '1');
      BCrc <= (others => '1');
      CCrc <= (others => '1');
      CrcH <= (others => '0');
      CrcData <= (others => '0');
      ACrcHEn <= '0';
      BCrcHEn <= '0';
      ACrcEnD <= '0';
      BCrcEnD <= '0';
      BCrcRst <= '0';
    elsif SysClk'event and SysClk = '1' then
      CrcH <= iWriteData(31 downto 16);
      if BCrcEn = '1' then
        CrcData <= iWriteData(7 downto 0) & iWriteData(15 downto 8);
      else
        CrcData <= CrcH(7 downto 0) & CrcH(15 downto 8);
      end if;

      ACrcHEn <= ACrcEn;
      ACrcEnD <= ACrcEn or ACrcHEn;
      if ACrcRst = '1' then
        ACrc <= (others => '1');
      elsif ACrcEnD = '1' then
        ACrc <= nextCRC32_D16(CrcData, ACrc);
      end if;

      BCrcHEn <= BCrcEn;
      BCrcEnD <= BCrcEn or BCrcHEn;
      BCrcRst <= SequenceActive and not SequenceActiveD;
    
      if BCrcRst = '1' then
        BCrc <= (others => '1');
        CCrc <= (others => '1');
      elsif BCSwap = '1' then
        BCrc <= CCrc;
        CCrc <= BCrc;
      elsif BCrcEnD = '1' then
        BCrc <= nextCRC32_D16(CrcData, BCrc);
      end if;
    end if;
  end process GenCrc;


end architecture Rtl;