Finished unwrapper implementation

stitch_project/stitch/DistanceUnwrapper.vhd

@@ -39,6 +39,7 @@ signal current_state : STATE_T := IDLE_S;
 
 signal currentDistance : STD_LOGIC_VECTOR(24 downto 0) := (others => '0');
 signal prevDistance : STD_LOGIC_VECTOR(24 downto 0) := (others => '0');
+signal latchedDistance : STD_LOGIC_VECTOR(24 downto 0) := (others => '0');
 signal distanceInWide : STD_LOGIC_VECTOR(24 downto 0) := (others => '0');
 signal calcDelta : STD_LOGIC_VECTOR(24 downto 0) := (others => '0');
 
@@ -47,6 +48,8 @@ signal corrDelta : STD_LOGIC_VECTOR(24 downto 0) := "0001000000000000000000000";
 signal corrDeltaN : STD_LOGIC_VECTOR(24 downto 0) := "1111000000000000000000000";
 signal currentOffset : STD_LOGIC_VECTOR(24 downto 0) := (others => '0');
 
+signal newIsLarger : STD_LOGIC := '0';
+
 signal dr_prev : STD_LOGIC := '0';
 
 begin
@@ -54,6 +57,8 @@ begin
 distanceInWide(20 downto 0) <= distance_in;
 distanceInWide(24 downto 21) <= (others => '0');
 
+distance_out <= currentDistance;
+
 
 dr_update: process(clk100)
 begin
@@ -63,6 +68,7 @@ begin
 end process;
 
 
+
 proc: process(clk100)
 begin
 	if rising_edge(clk100) then
@@ -71,29 +77,80 @@ begin
 			when IDLE_S =>
 				if dr = '1' and dr_prev = '0' then
 					current_state <= STATE1;
+					latchedDistance <= distanceInWide;
 					--calculate relevant delta distance here
 					if prevDistance > distanceInWide then
+						newIsLarger <= '0';
 						calcDelta <= prevDistance - distanceInWide;
 					else 
+						newIsLarger <= '1';
 						calcDelta <= distanceInWide - prevDistance;
+					end if;
 				else
-
+					current_state <= IDLE_S;
+					latchedDistance <= latchedDistance;
+					newIsLarger <= newIsLarger;
+					calcDelta <= calcDelta;
 				end if;
+				currentOffset <= currentOffset;
+				currentDistance <= currentDistance;
+				dr_out <= '0';
+				prevDistance <= prevDistance;
+
 			when STATE1 =>
 				current_state <= STATE2;
 				--calculate change to offset here 
+				if calcDelta > maxDelta then
+					--we have wrapped around 
+					if newIsLarger then
+						--went negative
+						currentOffset <= currentOffset + corrDeltaN;
+					else 
+						--went more positive
+						currentOffset <= currentOffset + corrDelta;
+					end if;
+				else 
+					currentOffset <= currentOffset;
+				end if;
+				latchedDistance <= latchedDistance;
+				newIsLarger <= newIsLarger;
+				calcDelta <= calcDelta;
+				currentDistance <= currentDistance;
+				dr_out <= dr_out;
+				prevDistance <= prevDistance;
+					
 			when STATE2 => 
+			--calculate output distance, set next prevDistance here
 				current_state <= STATE3;
-				--calculate output distance here
+				currentDistance <= currentOffset + latchedDistance;
+				latchedDistance <= latchedDistance;
+				newIsLarger <= newIsLarger;
+				calcDelta <= calcDelta;
+				currentOffset <= currentOffset;
+				dr_out <= dr_out;
+				prevDistance <= latchedDistance;
+
 			when STATE3 =>
+			--set output ready here	
 				current_state <= IDLE_S;
-				--set output ready here
-				
+				dr_out <= '1';
+				latchedDistance <= latchedDistance;
+				newIsLarger <= newIsLarger;
+				calcDelta <= calcDelta;
+				currentOffset <= currentOffset;
+				currentDistance <= currentDistance;
+				prevDistance <= prevDistance;
 			end case;
 
 		else 
 			currentDistance <= (others => '0');
 			prevDistance <= (others => '0');
+			current_state <= IDLE_S;
+			latchedDistance <= (others => '0');
+			newIsLarger <= '0';
+			calcDelta <= (others => '0');
+			currentOffset <= (others => '0');
+			dr_out <= '0';
 		end if;
 
 	end if;