-- altera vhdl_input_version vhdl_2008
-- XXX: QSYS Fix (https://www.intel.com/content/www/us/en/support/programmable/articles/000079458.html)

library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;

use work.rtps_package.all;
use work.ros_package.all;
use work.user_config.all;

-- This Entity converts from RTPS/DDS Time (TIME_TYPE) to ROS Time (ROS_TIME_TYPE).
-- TIME_TYPE is defined as a 64-bit seconds value in 32Q32 fixed point format.
-- ROS_TIME_TYPE is defined as 32-bit seconds and 32-bit nanoseconds
-- This entity uses a multiplier to calculate the nanoseconds from the fraction part of the seconds.
-- Since this entity has an internal pipeline, the time is adjusted by the pipeline delay.
-- This entity has a minimum delay of 2 clock cycles (1 for the multiplier and 1 for the time adjustment).

entity ros_time_converter is
    generic (
        PIPELINE_STAGES : natural := 2
    );
    port (
        -- SYSTEM
        clk         : in std_logic;
        reset       : in std_logic;
        time_in     : in TIME_TYPE;
        time_out    : out ROS_TIME_TYPE
    );
end entity;

architecture arch of ros_time_converter is
    
    type SECOND_DELAY_ARRAY is array (0 to PIPELINE_STAGES-2) of std_logic_vector(31 downto 0);
    
    -- This is the TIME_TYPE(1) value of 1 nanosecond
    -- 0.23283064365 in 0Q54 Fixed Point
    constant NANOSECOND_CONSTANT : std_logic_vector(53 downto 0) := "001110111001101011001001111111111111101110111110110011";
    
    signal res : std_logic_vector(85 downto 0);
    signal time_adjust : TIME_TYPE;
    signal second_delay : SECOND_DELAY_ARRAY;
    
begin
    
    assert (PIPELINE_STAGES >= 2) report "PIPELINE_STAGES has to be at least 2" severity FAILURE;
    
    mult_inst : configuration work.mult_cfg
        generic map (
            PIPELINE_STAGES => PIPELINE_STAGES-1,
            DATAA_WIDTH     => 32,
            DATAB_WIDTH     => 54,
            DATAB_CONST     => TRUE
        )
        port map (
            clk	    => clk,
            reset   => reset,
            dataa	=> std_logic_vector(time_adjust(1)),
            datab	=> NANOSECOND_CONSTANT,
            result  => res
        );
    
    time_out    <= (sec => second_delay(second_delay'length-1), nanosec => res(85 downto 54));
    
    sync_prc : process(clk)
    begin
        if rising_edge(clk) then
            time_adjust <= time_in + gen_duration(CLOCK_PERIOD * PIPELINE_STAGES);
            if (reset = '1') then
                second_delay    <= (others => (others => '0'));
            else
                second_delay(0) <= std_logic_vector(time_adjust(0));
                if (second_delay'length >= 1) then
                    for i in 1 to second_delay'length-1 loop
                        second_delay(i) <= second_delay(i-1);
                    end loop;
                end if;
            end if;
        end if;
    end process;
    
end architecture;
