library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;

entity rotator is
	 Generic (
				 Sensor_position : integer := 0; 
																	--Poloha senzoru v krocich vzhledem k počatku, jeden krok 0,9 stupnu pro HS
				 StepsMax : integer := 199								 
																	--Maximalni pocet kroku
																	-- kroky 0 az 199,  360/1.8=200 kroku 8bitu, polovicní krok
																	-- (StepsMax+1)/2 = 100 odpovida 180 stupnum pro 0-199 kroku
				 
			);								  
	 Port ( output_mot :  		out STD_LOGIC_VECTOR (3 downto 0):="0000";	--neaktivní uroven
			  clk : 		  			in  STD_LOGIC;
			  position :			in  STD_LOGIC_VECTOR (7 downto 0); -- pozice 0-199
			  position_en :      in  STD_LOGIC; 						  -- hranou spousti toceni na pozici
			  sensor_en :			in  STD_LOGIC							  -- imp ze senzoru polohy
	
			);
			
end rotator;

architecture Behavioral of rotator is
-- Signaly stavoveho automatu
	type ST_type is (s0,s1,s2,s3,s4,s5,s6,s7,s8);     
	signal now_ST : ST_type := s0;		 		      --pocatecni stav s0 
	signal next_ST : ST_type := s1;
	signal full_half : STD_LOGIC :='0';							-- 1 full step    0 half step
   signal forward_backward : STD_LOGIC :='1';				-- 1 forward      0 backward
   signal run : STD_LOGIC:='0'; 									-- 1 run				0 stop
	signal to_out :STD_LOGIC_VECTOR (3 downto 0):= (OTHERS => '0');
-- Signaly citace
	signal cnt_catch :integer range 0 to StepsMax := 0;	--poloha ze vstupu
	signal cnt_reg :integer range 0 to StepsMax := 0;		--poloha je 0 až 199
	signal sensor_set : STD_LOGIC := '1';
	
begin

-- Rozhodnuti o nataceni
-- Urceni smeru toceni a cilove pozice 
process (position_en,sensor_en,cnt_catch,position,cnt_reg)													
begin
if position_en='0' AND position_en'event then			  	   -- citlivost na hranu kvuli warningu,jinak vytvori spoustu latchu								
	if conv_integer(position) <= StepsMax then				   -- osetreni vstupni hodnoty
		cnt_catch <= conv_integer(position);    					-- ulozi cilovou pozici
	   																		-- urci smer toceni
																				-- ve vsech podminkach nefunguji zpozdene zapisy
		if  abs(conv_integer(position) - cnt_reg) < ((StepsMax +1)/2) then	
			if cnt_reg < conv_integer(position)  then				--uhel 0 az 180 
				forward_backward <= '1';								--tocim dopredu	
			else
				forward_backward <= '0';								--tocim dozadu
			end if;
		else
			if conv_integer(position) < cnt_reg  then				--uhel 180 az 360
				forward_backward <= '1' ;								--tocim dopredu 
			else
				forward_backward <= '0' ;		  						--tocim dozadu
			end if;
		end if;
	end if;
end if;	
if sensor_en = '1' then 
	if  abs(cnt_catch - sensor_position) < ((StepsMax +1)/2) then	
		if sensor_position < cnt_catch  then					--uhel 0 az 180 
			forward_backward <= '1';								--tocim dopredu	
		else
			forward_backward <= '0';								--tocim dozadu
		end if;
	else
		if cnt_catch < sensor_position  then					--uhel 180 az 360
			forward_backward <= '1' ;								--tocim dopredu 
		else
			forward_backward <= '0' ;		  						--tocim dozadu
		end if;
	end if;
end if;
end process;

--Synchronni cast,nastaveni dalsiho stavu,aktualni pozice 
process (clk,run,position_en,next_ST,sensor_en,sensor_set)
begin
if sensor_en ='1' and sensor_set = '1' then				-- když je signal ze senzoru		
		cnt_reg <= sensor_position;							-- upravi polohu
		sensor_set <= '0';
else
	if ( clk= '1' AND clk'event ) then							

			if (position_en ='1' OR run ='1') then					-- pokud je run a position_en
				run <='1';												    	-- jede i dale 
				
				now_ST <= next_ST;										 	-- posun stav

				if (cnt_reg /= cnt_catch ) then 						-- a pokud jsou pozice ruzne
					if forward_backward = '1' then					-- aktualizuj pozici v registru
						cnt_reg <= ( cnt_reg + 1 ) ;				
						if cnt_reg = StepsMax then						-- misto MOD StepsMax + 1			
							cnt_reg <= 0;
						end if;
					else
						cnt_reg <= ( cnt_reg - 1 ) ;
						if cnt_reg = 0 then										
							cnt_reg <= StepsMax;
						end if;
					end if;
				else														-- kdyz jsou stejne pozice byl tohle poslední krok
					sensor_set <= '1';
					run <='0';
					now_ST <= now_ST;									-- neztrati jeden stav
				end if;
			end if;
	end if;		
end if;	
end process;

-- Přirazeni vystupu
process (to_out,run)
begin	
if run = '1' then
	output_mot <= to_out;
else
	output_mot <= "0000";
end if;
-- Je mozny sem priradit vypnuti vystupu kdyz upravuje pozici podle senzoru, lepsi je jet dal :-D
end process;

-- Kombinacni cast,nastaveni dalsiho stavu,prirazeni vystupu
							--Unipolární řízení s polovičním krokem
							--Cívka 1 	-	-	0	0 	0	0	0	-
							--Cívka 2 	0	-	-	- 	0	0	0	0
							--Cívka 3 	0	0	0	- 	-	-	0	0
							--Cívka 4 	0	0	0	0 	0	-	-	-
							---------------------------------------
							-- full step forward  s1 s3 s5 s7
							-- full step backward s7 s5 s3 s1
							-- half step backward s8 s7 s6 s5 s4 s3 s2 s2 
							-- half step forward  s1 s2 s3 s4 s5 s6 s7 s8 
process (now_ST,full_half,forward_backward,run)
	variable aditional: STD_LOGIC_VECTOR (1 downto 0);
begin
	aditional := full_half & forward_backward;
	next_ST <= s1;		--odstraneni latchu
		case now_ST IS
			when s0 => to_out <= "0000";				  --odpojeni vystupu
			when s1 => to_out <= "1000";
				case aditional is
					when "00" => next_ST <= s8;			  --half step backward
					when "10" => next_ST <= s7;			  --full step backward
					when "01" => next_ST <= s2;	 		  --half step forward
					when "11" => next_ST <= s3;   		  --full step forward
					when others => next_ST <= s0;
				end case	;
			when s2 => to_out <= "1100";					
				case aditional is
					when "00" => next_ST <= s1;				--half step backward
					when "10" => next_ST <= s1;				--full step backward
					when "01" => next_ST <= s3;				--half step forward
					when "11" => next_ST <= s3;   			--full step forward
					when others => next_ST <= s0; 
				end case	;
			when s3 => to_out <= "0100";
				case aditional is
					when "00" => next_ST <= s2;  		      --half step backward
					when "10" => next_ST <= s1;				--full step backward
					when "01" => next_ST <= s4;				--half step forward
					when "11" => next_ST <= s5;				--full step forward
					when others => next_ST <= s0;
				end case	;
			when s4 => to_out <= "0110";
			case aditional is
					when "00" => next_ST <= s3;			   --half step backward
					when "10" => next_ST <= s3;				--full step backward
					when "01" => next_ST <= s5;				--half step forward
					when "11" => next_ST <= s5;   			--full step forward
					when others => next_ST <= s0;
				end case	;
			when s5 => to_out <= "0010";
				case aditional is
					when "00" => next_ST <= s4;			   --half step backward
					when "10" => next_ST <= s3;				--full step backward
					when "01" => next_ST <= s6;				--half step forward
					when "11" => next_ST <= s7;				--full step forward
					when others => next_ST <= s0;
				end case	;
			when s6 => to_out <= "0011";
				case aditional is
					when "00" => next_ST <= s5;				--half step backward
					when "10" => next_ST <= s5;				--full step backward
					when "01" => next_ST <= s7;				--half step forward
					when "11" => next_ST <= s7;   			--full step forward
					when others => next_ST <= s0;
				end case	;
			when s7 => to_out <= "0001";
				case aditional is
					when "00" => next_ST <= s6;				--half step backward
					when "10" => next_ST <= s5;				--full step backward
					when "01" => next_ST <= s8;				--half step forward
					when "11" => next_ST <= s1;				--full step forward
					when others => next_ST <= s0;
				end case	;
			when s8 => to_out <= "1001";
				case aditional is
					when "00" => next_ST <= s7;				--half step backward
					when "10" => next_ST <= s7;				--full step backward
					when "01" => next_ST <= s1;				--half step forward
					when "11" => next_ST <= s1;   			--full step forward
					when others => next_ST <= s0;	
				end case	;
	end case;
end process;
 
end Behavioral;