--------------------------------------------------------------------------------
-- Company: 
-- Engineer:
--
-- Create Date:    12:16:14 10/02/05
-- Design Name:    
-- Module Name:    cordic_stage_pre - Behavioral
-- Project Name:   
-- Target Device:  
-- Tool versions:  
-- Description:
--
-- Dependencies:
-- 
-- Revision:
-- Revision 0.01 - File Created
-- Additional Comments:
-- 
--------------------------------------------------------------------------------
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
USE ieee.numeric_std.ALL;
use work.fixed_ja.all;
use work.cordic_pkg.all;

---- Uncomment the following library declaration if instantiating
---- any Xilinx primitives in this code.
--library UNISIM;
--use UNISIM.VComponents.all;

entity cordic_stage_pre is
Generic
(
	nbits			: integer := 8;
	nbits_int		: integer := 2;
	nbits_out		: integer := 8;
	nbits_int_out	: integer := 2;
	reg_mode		: reg_mode_t := reg_mode_in
);
Port
(
	clk			: in std_logic;
	rst			: in std_logic;
	ce			: in std_logic;
	xin			: in sfixed_t;
	yin			: in sfixed_t;
	zin			: in sfixed_t;
	xout		: out sfixed_t;
	yout		: out sfixed_t;
	zout		: out sfixed_t;
	cordic_mode	: in cordic_mode_t;
	ready 		: out std_logic;
	valid 		: out std_logic
);
end cordic_stage_pre;

architecture Behavioral of cordic_stage_pre is

type xyz_in_t is record
	x : sfixed_t(sproto(nbits, nbits_int)'high downto sproto(nbits, nbits_int)'low);
	y : sfixed_t(sproto(nbits, nbits_int)'high downto sproto(nbits, nbits_int)'low);
	z : sfixed_t(sproto(nbits, nbits_int)'high downto sproto(nbits, nbits_int)'low);
end record;

type xyz_out_t is record
	x : sfixed_t(sproto(nbits_out, nbits_int_out)'high downto sproto(nbits_out, nbits_int_out)'low);
	y : sfixed_t(sproto(nbits_out, nbits_int_out)'high downto sproto(nbits_out, nbits_int_out)'low);
	z : sfixed_t(sproto(nbits_out, nbits_int_out)'high downto sproto(nbits_out, nbits_int_out)'low);
end record;

constant zero_in_sfix 		: sfixed_t := to_sfixed(0.0, nbits, nbits_int);
constant zero_out_sfix 		: sfixed_t := to_sfixed(0.0, nbits_out, nbits_int_out);
constant pi_out_sfix 		: sfixed_t := to_sfixed(pi, nbits_out, nbits_int_out);
constant pi2_out_sfix 		: sfixed_t := to_sfixed(pi/2.0, nbits_out, nbits_int_out);
constant pi2_in_sfix 		: sfixed_t := to_sfixed(pi/2.0, nbits, nbits_int);

signal xyz_in : xyz_in_t;
signal xyz_out : xyz_out_t;
signal valid_r, ready_r : std_logic;

------------------------------------------------------------
begin

gen_direct: if reg_mode = none generate
 
 	reg_in : process (rst, ce, xin, yin, zin)
 	begin
 		if (rst = '1') then
 			ready_r <= '0';
 			valid_r <= '0';
			xyz_in.x <= zero_in_sfix;
			xyz_in.y <= zero_in_sfix;
			xyz_in.z <= zero_in_sfix;
			
 		else
 			ready_r <= '1';
 			valid_r <= ce;
 			xyz_in.x <= to_sfixed(xin, nbits, nbits_int);
 			xyz_in.y <= to_sfixed(yin, nbits, nbits_int);
 			xyz_in.z <= to_sfixed(zin, nbits, nbits_int);
 		end if;	
 	end process;
 
 	reg_out : process (rst, ce, ready_r, valid_r, xyz_out)
 	begin
 		if (rst = '1') then
 			ready <= '0';
 			valid <= '0';
 			xout <= zero_out_sfix;
 			yout <= zero_out_sfix;
 			zout <= zero_out_sfix;
 		else
 			ready <= ready_r;
 			valid <= valid_r;
 			xout <= xyz_out.x;
 			yout <= xyz_out.y;
 			zout <= xyz_out.z;
 		end if;	
 	end process;
 
end generate;
------------------------------------------------------------

gen_reg_in: if (reg_mode = reg_mode_in) generate

	reg_in : process (rst, clk, ce, xin, yin, zin)
	begin
		if (rst = '1') then
 			ready_r <= '0';
 			valid_r <= '0';
			xyz_in.x <= zero_in_sfix;
			xyz_in.y <= zero_in_sfix;
			xyz_in.z <= zero_in_sfix;
		elsif rising_edge(clk) then
			ready_r <= '1';
			valid_r <= ce;
			if (ce = '1') then
				xyz_in.x <= to_sfixed(xin, nbits, nbits_int);
				xyz_in.y <= to_sfixed(yin, nbits, nbits_int);
				xyz_in.z <= to_sfixed(zin, nbits, nbits_int);
			end if;
		end if;	
	end process;

 	reg_out : process (rst, ready_r, valid_r, xyz_out)
 	begin
 		if (rst = '1') then
 			ready <= '0';
 			valid <= '0';
 			xout <= zero_out_sfix;
 			yout <= zero_out_sfix;
 			zout <= zero_out_sfix;
 		else
 			ready <= ready_r;
 			valid <= valid_r;
 			xout <= xyz_out.x;
 			yout <= xyz_out.y;
 			zout <= xyz_out.z;
 		end if;	
 	end process;

end generate;

------------------------------------------------------------
gen_reg_out: if (reg_mode = reg_mode_out) generate
 
 	reg_in : process (rst, ce, xin, yin, zin)
 	begin
 		if (rst = '1') then
 			ready_r <= '0';
 			valid_r <= '0';
			xyz_in.x <= zero_in_sfix;
			xyz_in.y <= zero_in_sfix;
			xyz_in.z <= zero_in_sfix;
 		else
 			ready_r <= '1';
			valid_r <= ce;
 			xyz_in.x <= to_sfixed(xin, nbits, nbits_int);
 			xyz_in.y <= to_sfixed(yin, nbits, nbits_int);
 			xyz_in.z <= to_sfixed(zin, nbits, nbits_int);
 		end if;	
 	end process;

 	reg_out : process (rst, clk, ce, ready_r, valid_r, xyz_out)
 	begin
 		if (rst = '1') then
 			ready <= '0';
 			valid <= '0';
 			xout <= zero_out_sfix;
 			yout <= zero_out_sfix;
 			zout <= zero_out_sfix;
 		elsif rising_edge(clk) then
 			ready <= ready_r;
 			valid <= valid_r;
 			if (ce = '1') then
 				xout <= xyz_out.x;
 				yout <= xyz_out.y;
 				zout <= xyz_out.z;
 			end if;
 		end if;	
 	end process;
 
end generate;

------------------------------------------------------------
gen_reg_inout: if (reg_mode = reg_mode_inout) generate
 
	reg_in : process (rst, clk, ce, xin, yin, zin)
	begin
		if (rst = '1') then
 			ready_r <= '0';
 			valid_r <= '0';
			xyz_in.x <= zero_in_sfix;
			xyz_in.y <= zero_in_sfix;
			xyz_in.z <= zero_in_sfix;
		elsif rising_edge(clk) then
 			ready_r <= '1';
			valid_r <= ce;
			if (ce = '1') then
				xyz_in.x <= to_sfixed(xin, nbits, nbits_int);
				xyz_in.y <= to_sfixed(yin, nbits, nbits_int);
				xyz_in.z <= to_sfixed(zin, nbits, nbits_int);
			end if;
		end if;	
	end process;

 	reg_out : process (rst, clk, ce, ready_r, valid_r, xyz_out)
 	begin
 		if (rst = '1') then
 			ready <= '0';
 			valid <= '0';
 			xout <= zero_out_sfix;
 			yout <= zero_out_sfix;
 			zout <= zero_out_sfix;
 		elsif rising_edge(clk) then
 			ready <= ready_r;
 			valid <= valid_r;
 			if (ce = '1') then
 				xout <= xyz_out.x;
 				yout <= xyz_out.y;
 				zout <= xyz_out.z;
 			end if;
 		end if;	
 	end process;
 
 end generate;

------------------------------------------------------------
cordic_proc_pre_stage: process(xyz_in, cordic_mode)
begin

	xyz_out.x <= to_sfixed(xyz_in.x, nbits_out, nbits_int_out);
	xyz_out.y <= to_sfixed(xyz_in.y, nbits_out, nbits_int_out);
	xyz_out.z <= to_sfixed(xyz_in.z, nbits_out, nbits_int_out);

	case cordic_mode is

		when cordic_mode_rotate =>

		if (xyz_in.z < -pi2_in_sfix) then
			xyz_out.x <= to_sfixed(-xyz_in.x, nbits_out, nbits_int_out);
			xyz_out.y <= to_sfixed(-xyz_in.y, nbits_out, nbits_int_out);
			xyz_out.z <= to_sfixed(xyz_in.z + pi_out_sfix, nbits_out, nbits_int_out);
		elsif (xyz_in.z > pi2_in_sfix) then
			xyz_out.x <= to_sfixed(-xyz_in.x, nbits_out, nbits_int_out);
			xyz_out.y <= to_sfixed(-xyz_in.y, nbits_out, nbits_int_out);
			xyz_out.z <= to_sfixed(xyz_in.z - pi_out_sfix, nbits_out, nbits_int_out);
		end if;


		when cordic_mode_vector =>

-- 		if (xyz_in.x < zero_in_sfix) then
--			xyz_out.x <= to_sfixed(-xyz_in.x, nbits_out, nbits_int_out);
-- 			xyz_out.y <= to_sfixed(-xyz_in.y, nbits_out, nbits_int_out);
-- 			xyz_out.z <= to_sfixed(xyz_in.z - pi_out_sfix, nbits_out, nbits_int_out);
--   		end if;
		if (xyz_in.y < zero_in_sfix) then
  			xyz_out.x <= to_sfixed(-xyz_in.y, nbits_out, nbits_int_out);
  			xyz_out.y <= to_sfixed(xyz_in.x, nbits_out, nbits_int_out);
  			xyz_out.z <= to_sfixed(xyz_in.z + pi2_out_sfix, nbits_out, nbits_int_out);
  		else	
  			xyz_out.x <= to_sfixed(xyz_in.y, nbits_out, nbits_int_out);
  			xyz_out.y <= to_sfixed(-xyz_in.x, nbits_out, nbits_int_out);
  			xyz_out.z <= to_sfixed(xyz_in.z - pi2_out_sfix, nbits_out, nbits_int_out);
  		end if;

		when others => null;

	end case;
end process;
	
------------------------------------------------------------
end Behavioral;

-- if (y < 0) then
-- 	x' = -y
-- 	y' =  x
-- 	z' = z + pi/2;
-- else
-- 	x' =  y
-- 	y' = -x
-- 	z' = z - pi/2;
-- end if;
-- 
-- if (x < 0) then
-- 	x' = -x;
-- 	y' = -y;
-- 	z' = z - pi
-- else
-- 	x' = x;
-- 	y' = y;
-- 	z' = z
-- end if;
