---------------------------------------------------------------------------
-- NDT Solutions Ltd. Proprietary Information
-- Company Confidential
-- Copyright 2004
--
-- Filename: EncoderMonitor.vhd
-- Language: VHDL '93
-- Project:  Digitiser Card Peripheral FPGA
--
-- Design Description:
-- Module to monitor the shaft encoders - with dual encoder monitoring
---------------------------------------------------------------------------

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


entity EncoderMonitor is
  Port (-- Local bus signals
        pnReset          : in  std_ulogic;
        pLCLK            : in  std_ulogic;
        pEncoderReset    : in  std_logic_vector(5 downto 0);
        pEncoderSelect   : in  std_logic_vector(2 downto 0);
        pClicksPerBScan  : in  std_logic_vector(15 downto 0);
        pFractClicks     : in  std_logic_vector(7 downto 0);
        pBScanActive     : in  std_ulogic;
        pEncoder0In      : in  std_logic_vector(1 downto 0);
        pEncoder1In      : in  std_logic_vector(1 downto 0);
        pEncoder2In      : in  std_logic_vector(1 downto 0);
        pEncoder3In      : in  std_logic_vector(1 downto 0);
        pEncoder4In      : in  std_logic_vector(1 downto 0);
        pEncoder5In      : in  std_logic_vector(1 downto 0);
        pSyncIn          : in  std_ulogic;
        pAckIn           : in  std_ulogic;
        pSyncAckEn       : in  std_ulogic;
        pEncoder0Pos     : out std_logic_vector(31 downto 0);
        pEncoder1Pos     : out std_logic_vector(31 downto 0);
        pEncoder2Pos     : out std_logic_vector(31 downto 0);
        pEncoder3Pos     : out std_logic_vector(31 downto 0);
        pEncoder4Pos     : out std_logic_vector(31 downto 0);
        pEncoder5Pos     : out std_logic_vector(31 downto 0);
        pSerialOut       : out std_ulogic;
        pSerialOutEn     : out std_ulogic;
        pBScanTriggerOut : out std_ulogic;
        pBScanTriggerIn  : in  std_ulogic);
end entity EncoderMonitor;


architecture Rtl of EncoderMonitor is

  component PISO6x32
    port (pnReset      : in  std_ulogic;
          pClock       : in  std_ulogic;
          pStart       : in  std_ulogic;
          pDataIn0     : in  std_logic_vector(31 downto 0);
          pDataIn1     : in  std_logic_vector(31 downto 0);
          pDataIn2     : in  std_logic_vector(31 downto 0);
          pDataIn3     : in  std_logic_vector(31 downto 0);
          pDataIn4     : in  std_logic_vector(31 downto 0);
          pDataIn5     : in  std_logic_vector(31 downto 0);
          pSerialOut   : out std_ulogic;
          pSerialOutEn : out std_ulogic);
  end component;

  signal ClkEnCount:    integer range 0 to 15;
  signal ClkEn1:        std_ulogic;
  signal ClkEn2:        std_ulogic;
  signal ClkEn3:        std_ulogic;

  type tEncInputs is array (0 to 5) of std_logic_vector(1 downto 0);
  signal EncInputs:  tEncInputs;
  signal LastInputs: tEncInputs;

  type tEncCounts is array (0 to 5) of std_logic_vector(31 downto 0);
  signal EncCounts: tEncCounts;

  signal EncInc:    std_logic_vector(5 downto 0);
  signal EncDec:    std_logic_vector(5 downto 0);

  signal DualInit:   std_ulogic;
  signal DualSelect: std_ulogic;
  signal LastInc:   std_logic_vector(1 downto 0);
  signal LastDec:   std_logic_vector(1 downto 0);
  signal DualInc:   std_ulogic;
  signal DualDec:   std_ulogic;
  signal PosInc:    std_logic_vector(5 downto 0);
  signal PosDec:    std_logic_vector(5 downto 0);

  signal SelEncInc:    std_ulogic;
  signal SelEncDec:    std_ulogic;
  signal SelEncReset:  std_ulogic;
  signal EncoderReset: std_logic_vector(5 downto 0);

  signal TrigCount: std_logic_vector(15 downto 0);
  signal IncHysterisis: std_ulogic;
  signal DecHysterisis: std_ulogic;
  signal FractTickCount: std_logic_vector(7 downto 0);

  signal BScanActive:   std_ulogic;

begin


  -- Generate a low frequency clock enable
  GenClockEn: process(pnReset, pLCLK)
    variable vTemp: std_ulogic;
  begin
    if pnReset = '0' then
      ClkEnCount <= 0;
      ClkEn1     <= '0';
      ClkEn2     <= '0';
      ClkEn3     <= '0';
    elsif rising_edge(pLCLK) then
      ClkEnCount <= (ClkEnCount + 1) mod 16;
      if ClkEnCount = 15 then
        ClkEn1 <= '1';
      else
        ClkEn1 <= '0';
      end if;

      ClkEn3 <= ClkEn2;
      ClkEn2 <= ClkEn1;
    end if;
  end process GenClockEn;


  -- Clock pBScanActive into pLCLK domain
  GenBScanActive: process(pnReset, pLCLK)
    variable vBScanActive: std_ulogic;
  begin
    if pnReset = '0' then
      vBScanActive := '0';
      BScanActive  <= '0';
    elsif rising_edge(pLCLK) then
      BScanActive  <= vBScanActive;
      vBScanActive := pBScanActive;
    end if;
  end process GenBScanActive;


  EncInputs <= (pEncoder0In, pEncoder1In, pEncoder2In,
                pEncoder3In, pEncoder4In, pEncoder5In);


  -- Monitor encoders and check for changes
  CheckChanges: process(pnReset, pLCLK)
    variable vTemp: std_logic_vector(3 downto 0);
    variable vSyncSample : std_logic_vector(1 downto 0);
    variable vAckSample : std_logic_vector(1 downto 0);
  begin
    if pnReset = '0' then

      for Count in 0 to 5 loop
        LastInputs(Count) <= "00";
        EncInc(Count)     <= '0';
        EncDec(Count)     <= '0';
      end loop;
      vSyncSample := "00";
      vAckSample := "00";
    elsif rising_edge(pLCLK) then
      if ClkEn1 = '1' then
        for Count in 0 to 5 loop
          vTemp := EncInputs(Count) & LastInputs(Count);

          EncInc(Count) <= '0';
          EncDec(Count) <= '0';

          -- Decide whether the encoder count needs incrementing or
          -- decrementing depending on the last and current values
          case vTemp is
            when "0001" => EncInc(Count) <= '1';
            when "0010" => EncDec(Count) <= '1';
            when "0100" => EncDec(Count) <= '1';
            when "0111" => EncInc(Count) <= '1';
            when "1011" => EncDec(Count) <= '1';
            when "1000" => EncInc(Count) <= '1';
            when "1110" => EncInc(Count) <= '1';
            when "1101" => EncDec(Count) <= '1';
            when others => null;
          end case;

          LastInputs(Count) <= EncInputs(Count);
        end loop;
        if pSyncAckEn = '1' then
          -- override Encoder 4/5 counters with Sync/Ack rising edges
          if vSyncSample = "01"  then
            EncInc(4) <= '1';
          else
            EncInc(4) <= '0';
          end if;
          if vAckSample = "01"  then
            EncInc(5) <= '1';
          else
            EncInc(5) <= '0';
          end if;
        end if;
        vSyncSample := vSyncSample(0) & pSyncIn;
        vAckSample := vAckSample(0) & pAckIn;
      end if;
    end if;
  end process CheckChanges;


  -- Generate Encoder position counts
  GenPosCounts: process(pnReset, pLCLK)
  begin
    if pnReset = '0' then

      for Count in 0 to 5 loop
        EncoderReset(Count) <= '0';
        EncCounts(Count)    <= X"00000000";
      end loop;

    elsif rising_edge(pLCLK) then

      for Count in 0 to 5 loop
        -- Latch pEncoderReset until ClkEn3 has seen it
        if pEncoderReset(Count) = '1' then
          EncoderReset(Count) <= '1';
        elsif ClkEn3 = '1' then
          EncoderReset(Count) <= '0';
        end if;

        if ClkEn3 = '1' then
          if EncoderReset(Count) = '1' then
            EncCounts(Count) <= X"00000000";

          elsif PosInc(Count) = '1' then
            if EncCounts(Count) /= X"80000000" then
              --  Increment count
              EncCounts(Count) <= (signed(EncCounts(Count)) + 1) and X"FFFFFFFF";
            end if;

          elsif PosDec(Count) = '1' then
            if EncCounts(Count) /= X"80000000" then
              -- Decrement count
              EncCounts(Count) <= (signed(EncCounts(Count)) - 1) and X"FFFFFFFF";
            end if;
          end if;
        end if;

      end loop;
    end if;
  end process GenPosCounts;


  -- Connect counts to outputs
  pEncoder0Pos <= EncCounts(0);
  pEncoder1Pos <= EncCounts(1);
  pEncoder2Pos <= EncCounts(2);
  pEncoder3Pos <= EncCounts(3);
  pEncoder4Pos <= EncCounts(4);
  pEncoder5Pos <= EncCounts(5);


  GenSelEnc: process(pnReset, pLCLK)
  begin
    if pnReset = '0' then
      SelEncInc   <= '0';
      SelEncDec   <= '0';
      SelEncReset <= '0';
    elsif rising_edge(pLCLK) then
      if ClkEn3 = '1' then
        -- Cancel SelEncReset
        SelEncReset <= '0';
      end if;

      case pEncoderSelect is
        when "000"|"001" | "010" | "011" | "100" | "101" =>
          SelEncInc   <= EncInc(conv_integer(unsigned(pEncoderSelect)));
          SelEncDec   <= EncDec(conv_integer(unsigned(pEncoderSelect)));
          if pEncoderReset(conv_integer(unsigned(pEncoderSelect))) = '1' then
            SelEncReset <= '1';
          end if;
        when "110" => -- 2 encoder mode
          SelEncInc   <= '0';
          SelEncDec   <= '0';
          if pEncoderReset(0) = '1' or pEncoderReset(1) = '1' then
            SelEncReset <= '1';
          end if;
        when others =>
          SelEncInc   <= '0';
          SelEncDec   <= '0';
      end case;
    end if;
  end process GenSelEnc;

  GenDual: process(pnReset, pLCLK)
    variable vDualInc: std_ulogic;
    variable vDualDec: std_ulogic;
  begin
    if pnReset = '0' then
      DualInit <= '1';
      DualSelect <= '0';
      LastInc <= "00";
      LastDec <= "00";
      DualInc <= '0';
      DualDec <= '0';
      PosInc <= (others => '0');
      PosDec <= (others => '0');
    elsif rising_edge(pLCLK) then
      if ClkEn2 = '1' then
        if pEncoderSelect = "110" then
          vDualInc := '0';
          vDualDec := '0';
          -- save last Inc/Dec
          if EncInc(0) = '1' or EncInc(1) = '1' then
            LastInc <= EncInc(1 downto 0);
            LastDec <= "00";
          elsif EncDec(0) = '1' or EncDec(1) = '1' then
            LastDec <= EncDec(1 downto 0);
            LastInc <= "00";
          end if;

          if SelEncReset = '1' then
            DualInit <= '1';
          elsif DualInit = '1' then
            -- select initial priority on which encoder moves first
            if EncInc(0) = '1' or EncDec(0) = '1' then
              DualSelect <= '0';
              DualInit <= '0';
              vDualInc := EncInc(0);
              vDualDec := EncDec(0);
            elsif EncInc(1) = '1' or EncDec(1) = '1' then
              DualSelect <= '1';
              DualInit <= '0';
              vDualInc := EncInc(1);
              vDualDec := EncDec(1);
            end if;
          else
            if DualSelect = '0' then
              if EncInc(0) = '1' then
                vDualInc := EncInc(0);
              elsif EncDec(0) = '1' then
                vDualDec := EncDec(0);
              elsif EncInc(1) = '1' then
                if LastInc(0) /= '1' then
                  vDualInc := EncInc(1);
                  DualSelect <= '1';
                end if;
              elsif EncDec(1) = '1' then
                if LastDec(0) /= '1' then
                  vDualDec := EncDec(1);
                  DualSelect <= '1';
                end if;
              end if;
            else
              if EncInc(1) = '1' then
                vDualInc := EncInc(1);
              elsif EncDec(1) = '1' then
                vDualDec := EncDec(1);
              elsif EncInc(0) = '1' then
                if LastInc(1) /= '1' then
                  vDualInc := EncInc(0);
                  DualSelect <= '0';
                end if;
              elsif EncDec(0) = '1' then
                if LastDec(1) /= '1' then
                  vDualDec := EncDec(0);
                  DualSelect <= '0';
                end if;
              end if;
            end if;
          end if;
        else
          DualInit <= '1';
          DualSelect <= '0';
          LastInc <= "00";
          LastDec <= "00";
          DualInc <= '0';
          DualDec <= '0';
        end if;
      end if;

      DualInc <= vDualInc;
      DualDec <= vDualDec;
      for Count in 0 to 5 loop
        if pEncoderSelect = "110" and Count < 2 then
          PosInc(Count) <= vDualInc;
          PosDec(Count) <= vDualDec;
        else
          PosInc(Count) <= EncInc(Count);
          PosDec(Count) <= EncDec(Count);
        end if;
      end loop;
    end if;
  end process GenDual;

  GenBScanTrig: process(pnReset, pLCLK)
    variable vFractTickCount: std_logic_vector(8 downto 0);
    variable vClicksPerBScan: std_logic_vector(15 downto 0);
  begin
    if pnReset = '0' then
      TrigCount      <= X"0000";
      IncHysterisis  <= '0';
      DecHysterisis  <= '1';
      FractTickCount <= x"00";
      pBScanTriggerOut <= '0';
    elsif rising_edge(pLCLK) then

      if ClkEn3 = '1' then
        pBScanTriggerOut  <= '0';

        if SelEncReset = '1' then
          TrigCount <= X"0000";
          IncHysterisis  <= '0';
          DecHysterisis  <= '1';
          FractTickCount <= x"00";
        else
          if SelEncInc = '1' or DualInc = '1' then
            vFractTickCount := unsigned('0' & FractTickCount) + unsigned(pFractClicks);
            if vFractTickCount(8) = '1' then
              vClicksPerBScan := unsigned(pClicksPerBScan) + 1;
            else
              vClicksPerBScan := pClicksPerBScan;
            end if;
  
            if TrigCount = vClicksPerBScan then
              TrigCount      <= X"0000";
              FractTickCount <= vFractTickCount(7 downto 0);
              IncHysterisis  <= '0';
              DecHysterisis  <= '1';
              if IncHysterisis = '0' and BScanActive = '0' then
                pBScanTriggerOut <= '1';
              end if;
            else
              TrigCount <= (unsigned(TrigCount) + 1) and X"FFFF";
              IncHysterisis <= '0';
              DecHysterisis <= '0';
            end if;

          elsif SelEncDec = '1' or DualDec = '1' then
            vFractTickCount := unsigned('0' & FractTickCount) - unsigned(pFractClicks);
            if vFractTickCount(8) = '1' then
              vClicksPerBScan := unsigned(pClicksPerBScan) + 1;
            else
              vClicksPerBScan := pClicksPerBScan;
            end if;
  
            if TrigCount = X"0000" then
              TrigCount      <= vClicksPerBScan;
              FractTickCount <= vFractTickCount(7 downto 0);
              IncHysterisis  <= '1';
              DecHysterisis  <= '0';
              if DecHysterisis = '0' and BScanActive = '0' then
                pBScanTriggerOut <= '1';
              end if;

            else
              TrigCount <= (unsigned(TrigCount) - 1) and X"FFFF";
              IncHysterisis <= '0';
              DecHysterisis <= '0';
            end if;
          end if;

        end if;
      end if;

    end if;
  end process GenBScanTrig;

  ShiftReg: PISO6x32
    port map (pnReset      => pnReset,
              pClock       => pLCLK,
              pStart       => pBScanTriggerIn,
              pDataIn0     => EncCounts(0),
              pDataIn1     => EncCounts(1),
              pDataIn2     => EncCounts(2),
              pDataIn3     => EncCounts(3),
              pDataIn4     => EncCounts(4),
              pDataIn5     => EncCounts(5),
              pSerialOut   => pSerialOut,
              pSerialOutEn => pSerialOutEn);

end Rtl;
