
library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use ieee.numeric_std.all;
use IEEE.MATH_REAL.ALL;

library work;
use work.fixed_util_pkg.all;

-------------------------------------------------------------------------------
package filter_pkg is

	function FilterCoef_Lowpass (N : positive; omega, gain : real) return real_array_t;
	function FilterCoef_Highpass (N : positive; omega, gain : real) return real_array_t;
	function FilterCoef_Bandpass (N : positive; bw2, omega_m, gain : real) return real_array_t;
	function FilterCoef_Delta (N:positive; delay:natural; gain:real) return real_array_t;
	function Window_Hamming (x : real_array_t) return real_array_t;
	function Window_VonHann (x : real_array_t) return real_array_t;
	function Window_Blackman (x : real_array_t) return real_array_t;
	function Sinc (x : real) return real;
	function FilterEnergy (x : real_array_t) return real;
	function FilterMean (x : real_array_t) return real;
	function FilterScale (x : real_array_t; scale : real) return real_array_t;
	function FilterTestCoef (N : positive) return real_array_t;
	
end; -- package filter_pkg;

-------------------------------------------------------------------------------
package body filter_pkg is
	
	function Prototype_Lowpass (N : positive; omega : real) return real_array_t is
	variable res : real_array_t(0 to N-1);
	variable M : positive;
	variable phi : real := 0.0;
	variable dphi : real := pi*omega;
	begin
		if (N mod 2) = 0 then
			M := N/2;
			for i in 0 to M-1 loop
				phi := phi + dphi;
				res(M+i) := Sinc(phi);
				res(M-i-1) := res(M+i);
			end loop;
		else
			M := (N-1)/2;
			for i in 1 to M loop
				phi := phi + dphi;
				res(M+i) := Sinc(phi);
				res(M-i) := res(M+i);
			end loop;
			res(M) := Sinc(0.0);
		end if;
		return res;
		
	end Prototype_Lowpass;

	-------------------------------------------------------------------------------
	function FilterCoef_Lowpass (N : positive; omega, gain : real) return real_array_t is
	variable lp : real_array_t(0 to N-1);
	begin
		lp := Prototype_Lowpass(N, omega);

		return Window_VonHann(FilterScale(lp, gain/FilterEnergy(lp)));
		
	end FilterCoef_Lowpass;

	-------------------------------------------------------------------------------
	function FilterCoef_Highpass (N : positive; omega, gain : real) return real_array_t is
	variable lp, res : real_array_t(0 to N-1);
	variable phi : real := 0.0;
	variable dphi : real := pi;
	begin
		lp := Prototype_Lowpass(N, 0.5-omega);

		for i in 0 to N-1 loop
			res(i) := lp(i) * cos(phi);	
			phi := phi + dphi;
		end loop;	
		
		return Window_VonHann(FilterScale(res, gain/FilterEnergy(lp)));
			
	end FilterCoef_Highpass;

	-------------------------------------------------------------------------------
	function FilterCoef_Bandpass (N : positive; bw2, omega_m, gain : real) return real_array_t is
	variable lp, res, mean, sum : real_array_t(0 to N-1);
	variable phi : real := 0.0;
	variable dphi : real := 2.0*pi*omega_m;
	begin
		lp := Prototype_Lowpass(N, bw2/2.0);
		
		for i in 0 to N-1 loop
			res(i) := lp(i) * cos(phi);	
			phi := phi + dphi;
		end loop;	

		return Window_VonHann(FilterScale(res, gain/FilterEnergy(lp)));

	end FilterCoef_Bandpass;

	-------------------------------------------------------------------------------
	function FilterCoef_Delta (N:positive; delay:natural; gain:real) return real_array_t is
	variable res : real_array_t(0 to N-1);
	begin
			for i in 0 to N-1 loop
				res(i) := 0.0;
			end loop;
			res(delay) := gain;
			
		return res;
		
	end FilterCoef_Delta;
	
	-------------------------------------------------------------------------------
	function FilterTestCoef (N : positive) return real_array_t is
	variable phi : real := 0.0;
	variable dphi : real := 1.0/real(N);
	variable res : real_array_t(0 to N-1);
	begin
			for i in 0 to N-1 loop
				res(i) := phi;
				phi := phi + dphi;
			end loop;
		return res;
		
	end FilterTestCoef;
	
	-------------------------------------------------------------------------------
	function Window_Hamming (x : real_array_t) return real_array_t is
	variable res : real_array_t(x'range);
	variable phi : real := 0.0;
	variable dphi : real := 2.0*pi/real(x'length-1);
	begin
		for i in x'range loop
			res(i) := x(i) * (0.54 - 0.46*(cos(phi)));
			phi := phi + dphi;
		end loop;
		return res;
		
	end Window_Hamming;
	
	-------------------------------------------------------------------------------
	function Window_VonHann (x : real_array_t) return real_array_t is
	variable res : real_array_t(x'range);
	variable phi : real := 0.0;
	variable dphi : real := 2.0*pi/real(x'length-1);
	begin
		for i in x'range loop
			res(i) := 0.5 * x(i) * (1.0 - (cos(phi)));
			phi := phi + dphi;
		end loop;
		return res;
		
	end Window_VonHann;
	
	-------------------------------------------------------------------------------
	function Window_Blackman (x : real_array_t) return real_array_t is
	variable res : real_array_t(0 to x'length-1);
	variable phi : real := 0.0;
	variable dphi : real := 2.0*pi/real(x'length-1);
	begin
		for i in x'range loop
			res(i) := x(i) * (0.42 - 0.5*cos(phi) + 0.08*cos(2.0*phi));
			phi := phi + dphi;
		end loop;
		return res;
		
	end Window_Blackman;
	
	-------------------------------------------------------------------------------
	function Sinc (x : real) return real is
	variable res : real := 1.0;
	begin
		if x /= 0.0 then
			res := sin(x)/x;
		end if;
		
		return res;
		
	end Sinc;
	
	-------------------------------------------------------------------------------
	function FilterScale (x : real_array_t; scale : real) return real_array_t is
	variable res : real_array_t(x'range);
	begin
		for i in x'range loop
			res(i) := scale * x(i);
		end loop;
		
		return res;
		
	end FilterScale;
	
	-------------------------------------------------------------------------------
	function FilterEnergy (x : real_array_t) return real is
	variable res : real := 0.0;
	begin
		for i in x'range loop
			res := res + x(i)*x(i);
		end loop;
		
		return res;
		
	end FilterEnergy;
	
	-------------------------------------------------------------------------------
	function FilterMean (x : real_array_t) return real is
	variable res : real := 0.0;
	begin
		for i in x'range loop
			res := res + x(i);
		end loop;
		
		return res/real(x'length);
	
	end FilterMean;
	
-------------------------------------------------------------------------------
end; -- package filter_pkg;
