% p =prototype signal
% r =returned signal
% W = W in chirp calc
% N = length of total burst (usually 2^15)

% Assumes v < 937.5 mph
% Returned in m/s

function vel=velcalc(p,r,W,N)
  
  % Determine threshold location and value in prototype signal
    [pind1,pind2]=threshind(p,N,0);
    [rind1,rind2]=threshind(r,N,1);
    if rind2 < rind1,
      vel1=(W*3e8*(rind1-pind1))/(N*40e9);
      vel2=(W*3e8*(rind2-pind2))/(N*40e9);
      vel = mean([vel1 vel2]);
    elseif rind1 < N/2,
      vel1=(W*3e8*(length(p)-pind1+rind1))/(N*40e9);
      vel2=(W*3e8*(rind2-pind2))/(N*40e9);
      vel = mean([vel1 vel2]);
    else
      vel1=(W*3e8*(rind1-pind1))/(N*40e9);
      vel2=(W*3e8*(-pind2-length(p)+rind2))/(N*40e9);
      vel = mean([vel1 vel2]);
    end
    
    
    
    

function [ind1,ind2]=threshind(p,N,plt)
    P=fft(p);
    Pmag=abs(P);
    Pmovavg = movavg(400,Pmag);
    %threshmin= mean(Pmovavg(4200:5400));
    %threshmax= mean([mean(Pmovavg(1:600))
    %mean(Pmovavg(9000:9600))]);
    
    [maxval, maxind] = max(Pmovavg);
    rotamt = N/2 - maxind;
    lpma = length(Pmovavg);
    %subplot(6,1,3*plt+1),plot(Pmovavg);
    if rotamt > 0
      Pmovavg = [Pmovavg(lpma-rotamt+1:lpma) Pmovavg];
      Pmovavg = Pmovavg(1:lpma);
    end
    if rotamt < 0
      Pmovavg = [Pmovavg Pmovavg(1:abs(rotamt))];
      Pmovavg = Pmovavg(abs(rotamt)+1:length(Pmovavg));
    end
    %subplot(6,1,3*plt+2),plot(Pmovavg);
    threshval = mean([min(Pmovavg) maxval]);
    %threshval = mean([threshmin threshmax]);
    threshmatch = abs(Pmovavg - threshval);
    %subplot(6,1,3*plt+3),plot(threshmatch);
    [tva,tind1] = min(threshmatch);
    tind2=tind1;
    while abs(tind2-tind1) < 1000,
      threshmatch(tind2) = threshmatch(tind2) + 100;
      [tva,tind2] = min(threshmatch);
    end
    if tind2<tind1,
      indtemp = tind2;
      tind2=tind1;
      tind1=indtemp;
    end
    tind1 = tind1-rotamt;
    if tind1 < 1,
      tind1 = tind1 + lpma;
    elseif tind1 > N
      tind1 = tind1 - lpma;
    end
    tind2 = tind2-rotamt;
    if tind2 < 1,
      tind2 = tind2 + lpma;
    elseif tind2 > N
      tind2 = tind2 - lpma; 
    end 
    ind1 = tind1;
    ind2 = tind2;