%Read an a signal in, try to guess an FIR filter that will match it, 
%Calculate delay based on AFIR filter that was found.

AFIRLength = 50;
FIRLength = 21;

AFIR = zeros(1, AFIRLength);
Error = zeros(1,1000);
mu=.05;

x=zeros(1, 1000);
y=zeros(1, 1000);

directAmount=0;

N=0;

for Q = 0:10
    AFIR = zeros(1, AFIRLength);
    Error = zeros(1,1000);
    
    for N=1:50+(N*4):950
        x(N:1:N+20)=transpose(hanning(21)) .* sinc(-2:.2:2);
	end
	
	%11 tap delay, FIRLength Taps total
	FIR = [zeros(1, 10), .8, zeros(1,4), .5,0,0,.3,.3,.2,.1,.1,.08];
	
	
	y=zeros(1, 1000);
	% Filter Signal
	for N=FIRLength+1:1000
        for i=1:FIRLength
            y(N) = y(N) + FIR(i)*x(N-i);
        end
	end
	
	%Add a ton of normally distributed noise
	y=y+randn(size(y))/4;
	
	
	%clear out yhat
	yhat=zeros(1,1000);
	

    if directAmount>0
        AFIR(directAmount)=1;
    end
	for N=AFIRLength+1:1000
       
        yhat(N)=0;
        for i=1:AFIRLength
            yhat(N) = yhat(N) + AFIR(i)*x(N-i);
        end
        %Notice Error is just the mic input in this case
        Error(N)=yhat(N)-y(N);
        
        for i=1:AFIRLength
            %also notice the sign of mu
            AFIR(i)=AFIR(i)-mu*Error(N)*x(N-i);
        end    
	end
	
	
	directAmountValue = 0;

    oldDirectAmount=directAmount;
    
	for N = 2:AFIRLength-1
        if AFIR(N-1)<AFIR(N) 
            if AFIR(N)>AFIR(N+1)
                if AFIR(N)>.15 
                    directAmountValue=AFIR(N);
                    directAmount=N;  
                    break;
                end
                if AFIR(N)>directAmountValue    
                    if directAmountValue>.15
                        if N>=directAmount
                            %Keep current
                        else
                            directAmountValue=AFIR(N);
                            directAmount=N;                      
                        end
                    else
                        directAmountValue=AFIR(N);
                        directAmount=N;                  
                    end
                end
            end
        end
	end
	
    if oldDirectAmount==directAmount
        break;    
    end
end	

directAmount


figure(1);
plot(yhat);
title('Yhat');
figure(2);
plot(y);
title('Y');
figure(3);
plot(Error);
title('Error');
figure(4);
plot(AFIR);
title('AFIR');
figure(5);
plot(FIR);
title('FIR');
figure(6);
plot(x);
title('X');