function out = quadmath(deltsampA,deltsampB, deltsampC, C,SF)

%quad
%
%   quad(deltsampA, deltsampB, deltsampC,C,SF)
%   shows the position of an incoming sound.  There is a
%   point of common reference at the center of the array,
%   which is shaped as the following (roughly).
%
%         Mic B
%
%                Mic D    Mic A
%
%         Mic C
%
%   DeltsampA is the difference in samples from MicA and
%   MicD (Td - Ta).
%   DeltsampB is the difference in samples from MicB and
%   MicD (Td - Tb).
%   DeltsampC is the difference in samples from MicC and
%   MicD (Td - Tc).
%
%   C is the distance from the center microphone to any of
%   the other three microphones. SF is the sampling freq.
%   A quantity called deltat is computed, with the delay time
%   in seconds assuming a sampling rate of SF.
%
%   See also: maxt, fshift, gowave3, go2, blargh3, quad


r = NaN;
thet = NaN;
out = [NaN,NaN];

% V = 340.83 m/s at standard temperature, pressure, etc.
V=340.83;

%compute deltatA and alpha
deltatA = deltsampA/SF;
alpha = V*(deltatA);

%compute deltatB and beta
deltatB = deltsampB/SF;
beta = V*(deltatB);

%compute deltatC and gamma
deltatC = deltsampC/SF;
gamma = V*(deltatC);

% check if possible solution satisfies triangle inequality
% not checking if any one > C since if on bisector, one could be
% epsilon from C
if ( abs(alpha) > C | abs(beta) > C | abs(gamma) > C  )
	%'There is an impossible shift value for the C you entered'
	out = [NaN,NaN];
	return
end

% check for completely nonsensical solutions
if (alpha >= 0 & beta >= 0 & gamma >= 0)
   %'That is an impossible shift value for this function'
   out = [NaN,NaN];
   return
end

if (alpha > 0 & beta < 0 & gamma < 0)
   
   %check if signal is inline with two mikes
   if (beta == gamma) & alpha < C - 0.1
      
      r1 = (C+alpha)/2;
      
      if ( abs((r1 - sqrt(C^2 + r1^2 - 2*C*r1*cos(2*pi/3))) - beta) < 0.1)
         
         out = [0,r1];
         return
                  
      else 
         
         %'These are an invalid set of shifts'  
         
         out = [NaN, NaN];
         
         return
         
      end
            
   else
      
      %calculate possible valid r/theta
      q = detect(beta, gamma, C);
      r1 = q(1);
      thet = q(2);
      r2 = q(3);
      thet2 = q(4);
      
      %adjust thet for reference frame
      thet = (2*pi/3) - thet;
      thet2 = (2*pi/3) - thet2;
      
   end
   
   % do more checking for valid shifts considering r/theta combo
   
   if (r1 > 0 &  abs(r1 - sqrt( C^2 + r1^2 - 2*r1*C*cos(thet)) - alpha) < 0.1  )
      
      out = [thet,r1];
      return
      
   elseif (r2 > 0 & abs(r2 - sqrt( C^2 + r2^2 - 2*r2*C*cos(thet2)) - alpha) < 0.1 )
      
      out = [thet2,r2];
      return
      
   else 
      
      %'These are an invalid set of shifts'
      
      out = [NaN, NaN];
      return
      
   end
      
elseif ((alpha >= 0 & beta >= 0 & gamma < 0) | (alpha <= 0 & beta <= 0 & gamma <= 0 & ((abs(alpha) < abs(gamma) & abs(beta) < abs(gamma)) | (beta == gamma & abs(alpha) < C/2 + 0.01 ) | (beta == alpha & abs(gamma) > C/2 - 0.01 ))))
      
   %check if signal is in line with two mikes
   if (beta == gamma & abs(alpha) < C + 0.01)
      
      r1 = (C-abs(alpha))/2;
                  
      if ( r1 > 0 & abs((r1 - sqrt(C^2 + r1^2 - 2*C*r1*cos(2*pi/3))) - beta) < 0.1)
         
         out = [0,r1];
         return
                  
      else 
         
         %'These are an invalid set of shifts'  
         
         out = [NaN, NaN];
         
         return
         
      end
      
   else
      
      %calculate the two possible r/theta's
      q = detect(beta, alpha, C);
      r1 = q(1);
      thet = q(2);
      r2 = q(3);
      thet2 = q(4);
      
      %adjust thet for reference frame
      thet = (2*pi/3) - thet;
      thet2 = (2*pi/3) - thet2;
      
   end
   
   % do more checking for valid shifts considering r/theta combo
   
   %two checks for signal in line with mikes.
   if (abs(thet - pi/3) < 0.001 & abs( (r1 - C ) - gamma) < 0.1)
      
      out = [thet,r1];
      return
         
   elseif (abs(thet - pi/3) < 0.001 & abs( r1 - sqrt( C^2 + r1^2 - 2*r1*C*cos(pi/3))  - beta) < .1 );
      
      out = [pi/3,r2];
      return
      
   %two checks with branch for which half of plane the signal falls on   
   elseif (thet > pi/3 & r1 > 0 & abs( r1 - sqrt( C^2 + r1^2 - 2*r1*C*cos(4*pi/3 - thet))  - gamma) < .1 );
      
      out = [thet,r1];
      return
      
   elseif (thet < pi/3 & r1 > 0 & r1 > 0 & abs( r1 - sqrt( C^2 + r1^2 - 2*r1*C*cos(2*pi/3 + thet))  - gamma) < .1 )
      
      out = [thet,r1];
      return
      
   end
      
   %two checks for signal inline with mikes
   if (abs(thet2 - pi/3) < 0.001 & abs( (r2 - C ) - gamma) < 0.1)      
      out = [thet2,r2];
      return
      
   elseif (abs(thet2 - pi/3) < 0.001 & abs( r2 - sqrt( C^2 + r2^2 - 2*r2*C*cos(pi/3))  - beta) < .1 );
      
      out = [pi/3,r2];
      return
      
   %two checks with branch for which half of plane signal falls on   
   elseif (thet2 > pi/3 & r2 > 0 & abs( r2 - sqrt( C^2 + r2^2 - 2*r2*C*cos(4*pi/3 - thet2))  - gamma) < .1 );
      
      out = [thet2,r2];
      return
      
   elseif (thet2 < pi/3 & r2 > 0 & r2 > 0 & abs( r2 - sqrt( C^2 + r2^2 - 2*r2*C*cos(2*pi/3 + thet2))  - gamma) < .1 )
      
      out = [thet2,r2];
      return   
      
   else 
      
      %'These are an invalid set of shifts'
      
      out = [NaN, NaN];
      return
      
   end
         
elseif (alpha < 0 & beta > 0 & gamma < 0)
   
   %check for signal inline with mikes
   if (alpha == gamma) & beta < C - 0.1
      
      r1 = (C+beta)/2;
      
      if ( abs((r1 - sqrt(C^2 + r1^2 - 2*C*r1*cos(2*pi/3))) - alpha) < 0.1)
         
         out = [2*pi/3,r1];
         return
                  
      else 
         
         %'These are an invalid set of shifts'  
         
         out = [NaN, NaN];
         
         return
         
      end
      
   else
      
      %calculate the two possible r/thetas
      q = detect(gamma, alpha, C);
      r1 = q(1);
      thet = q(2);
      r2 = q(3);
      thet2 = q(4);
      
      % adjust thet and thet2 to fit into reference frame
      thet = (4*pi/3) - thet;
      thet2 = (4*pi/3) - thet2;
      
   end
   
   % do more checking for valid shifts considering r/theta combo
   
   if (r1 > 0 &  abs(r1 - sqrt( C^2 + r1^2 - 2*r1*C*cos(2*pi/3 - thet))  - beta) < 0.1  )
      
      out = [thet,r1];
      
   elseif (r2 > 0 & abs(r2 - sqrt( C^2 + r2^2 - 2*r2*C*cos(2*pi/3 - thet2)) - beta) < 0.1 )
      
      out = [thet2,r2];
      
   else 
      
      %'These are an invalid set of shifts'
      
      out = [NaN, NaN];
      
   end
      
   return
   
elseif ((alpha < 0 & beta >=0 & gamma >= 0) | (alpha <= 0 & beta <= 0 & gamma <= 0 & ((abs(beta) < abs(alpha) & abs(gamma) < abs(alpha)) | (alpha == gamma & abs(beta) < C/2 +0.01) | (beta == gamma & abs(alpha) > C/2 -0.01))))
   
   %check if signal inline with two mikes
   if (alpha == gamma & abs(beta) < C +0.01)
      
      r1 = (C-abs(beta))/2;
      
      if ( abs((r1 - sqrt(C^2 + r1^2 - 2*C*r1*cos(2*pi/3))) - alpha) < 0.1)
         
         out = [2*pi/3,r1];
         return
                  
      else 
         
         %'These are an invalid set of shifts'  
         
         out = [NaN, NaN];
         
         return
         
      end
      
   else
      
      %calculate the two possible r/thetas
      q = detect(gamma, beta, C);
      r1 = q(1);
      thet = q(2);
      r2 = q(3);
      thet2 = q(4);   
      
      % adjust thet to fit into reference frame
      thet = (4*pi/3) - thet;
      thet2 = (4*pi/3) - thet2;
      
   end
      
   % do more checking for valid shifts considering r/theta combo
   
   %two checks if signal inline with two mikes
   if (abs(thet - pi) < 0.001 & abs( (r1 - C ) - alpha) < 0.1)      
      out = [thet,r1];
      return
      
   elseif (abs(thet - pi) < 0.001 & abs( r1 - sqrt( C^2 + r1^2 - 2*r1*C*cos(pi/3))  - beta) < .1 );
      
      out = [pi,r1];
      return
      
   %two checks depending on which half of plane signal falls on   
   elseif (thet > pi & r1 > 0 & abs( r1 - sqrt( C^2 + r1^2 - 2*r1*C*cos(-thet))  - alpha) < .1 );
      
      out = [thet,r1];
      return
      
   elseif (thet < pi & r1 > 0 & r1 > 0 & abs( r1 - sqrt( C^2 + r1^2 - 2*r1*C*cos(thet))  - alpha) < .1 )
      
      out = [thet,r1];
      return
      
   end
      
   %two checks if signal in line with two mikes
   if (abs(thet2 - pi) < 0.001 & abs( (r2 - C ) - alpha) < 0.1)
      
      out = [thet2,r2];
      return
      
   elseif (abs(thet2 - pi) < 0.001 & abs( r2 - sqrt( C^2 + r2^2 - 2*r2*C*cos(pi/3))  - beta) < .1 );
      
      out = [pi,r2];
      return
   
   %two checks depending which half of plane signal falls on
   elseif (thet2 > pi & r2 > 0 & abs( r2 - sqrt( C^2 + r2^2 - 2*r2*C*cos(-thet2))  - alpha) < .1 );
      
      out = [thet2,r2];
      return
      
   elseif (thet2 < pi & r2 > 0 & r2 > 0 & abs( r2 - sqrt( C^2 + r2^2 - 2*r2*C*cos(thet2))  - alpha) < .1 )
      
      out = [thet2,r2];
      return   
      
   else 
      
      %'These are an invalid set of shifts'
      
      out = [NaN, NaN];
      return
      
   end
   
   return
      
elseif (alpha < 0 & beta < 0 & gamma > 0)
   
   %check if signal inline with two mikes
   if (beta == alpha) & gamma < C - 0.1
      
      r1 = (C+gamma)/2;
      
      if ( abs((r1 - sqrt(C^2 + r1^2 - 2*C*r1*cos(2*pi/3))) - beta) < 0.1)
         
         out = [4*pi/3,r1];
         return
                  
      else 
         
         %'These are an invalid set of shifts'  
         
         out = [NaN, NaN];
         
         return
         
      end
      
   else
      
      %calculate the two possible r/theta combos
      q = detect(alpha, beta, C);
      r1 = q(1);
      thet = q(2);
      r2 = q(3);
      thet2 = q(4);
      
      % adjust thet to fit into reference frame
      thet = -thet;
      thet2 = -thet2;
      
   end
   
   % do more checking for valid shifts considering r/theta combo
   
   if (r1 > 0 &  abs(r1 - sqrt( C^2 + r1^2 - 2*r1*C*cos(4*pi/3 - thet)) - gamma) < 0.1  )
     
      out = [thet,r1];
      
   elseif (r2 > 0 & abs(r2 - sqrt( C^2 + r2^2 - 2*r2*C*cos(4*pi/3 - thet2)) - gamma) < 0.1 )
      
      out = [thet2,r2];
      
   else 
      
      %'These are an invalid set of shifts'
      
      out = [NaN, NaN];
      
   end
   
   return
     
elseif ((alpha >= 0 & beta < 0 & gamma >= 0)  | (alpha <= 0 & beta <= 0 & gamma <= 0 & ((abs(gamma) < abs(beta) & abs(alpha) < abs(beta)) | (alpha == beta & abs(gamma) < C/2 + 0.01) | (gamma == alpha & abs(beta) > C/2 - 0.01))))
   
   %check if signal inline with two mikes
   if (alpha == beta) & abs(gamma) < C + 0.01     
      
      r1 = (C-abs(gamma))/2;
      
      if ( abs((r1 - sqrt(C^2 + r1^2 - 2*C*r1*cos(2*pi/3))) - beta) < 0.1)
         
         out = [4*pi/3,r1];
         return
                  
      else 
         
         %'These are an invalid set of shifts'  
         
         out = [NaN, NaN];
         
         return
         
      end
      
   else
      
      %calculate the two possible r/thetas
      q = detect(alpha, gamma, C);
      r1 = q(1);
      thet = q(2);
      r2 = q(3);
      thet2 = q(4);
      
      % adjust thet to fit into reference frame
      thet = -thet;
      thet2 = -thet2;
      
   end
      
   % do more checking for valid shifts considering r/theta combo
   
   %two checks for signal inline with two mikes
   if (abs(thet - 5*pi/3) < 0.001 & abs( (r1 - C ) - beta) < 0.1)
      
      out = [thet,r1];
      return
      
   elseif (abs(thet - 5*pi/3) < 0.001 & abs( r1 - sqrt( C^2 + r1^2 - 2*r1*C*cos(pi/3))  - gamma) < .1 );
      
      out = [-pi/3,r2];
      return
      
   %two checks depending on which half of plane signal falls on   
   elseif (thet > 5*pi/3 & r1 > 0 & abs( r1 - sqrt( C^2 + r1^2 - 2*r1*C*cos(8*pi/3 - thet))  - beta) < .1 );
      
      out = [thet,r1];
      return
      
   elseif (thet < 5*pi/3 & r1 > 0 & r1 > 0 & abs( r1 - sqrt( C^2 + r1^2 - 2*r1*C*cos(thet - 2*pi/3))  - beta) < .1 )      
      out = [thet,r1];
      return
      
   end
      
   %two checks for signal inline with two mikes
   if (abs(thet2 - 5*pi/3) < 0.001 & abs( (r2 - C ) - beta) < 0.1)
      
      out = [thet2,r2];
      return
      
   elseif (abs(thet2 - 5*pi/3) < 0.001 & abs( r2 - sqrt( C^2 + r2^2 - 2*r2*C*cos(pi/3))  - gamma) < .1 );
      
      out = [-pi/3,r2];
      return
   
   %two checks depending on which half of plane signal falls on
   elseif (thet2 > 5*pi/3 & r2 > 0 & abs( r2 - sqrt( C^2 + r2^2 - 2*r2*C*cos(8*pi/3 - thet2))  - beta) < .1 );
      
      out = [thet2,r2];
      return
      
   elseif (thet2 < 5*pi/3 & r2 > 0 & r2 > 0 & abs( r2 - sqrt( C^2 + r2^2 - 2*r2*C*cos(thet2 - 2*pi/3))  - beta) < .1 )      
      out = [thet2,r2];
      return   
      
   else
      
      %'These are an invalid set of shifts'
      
      out = [NaN, NaN];
      
   end
   
end

%---------------------------------------------------------
function temp = detect(shift1, shift2, C)

%compute r

bel = 12*shift1*(C^2-shift1^2) + 2*(4*shift2 + 2*shift1)*(3*C^2 - (2*shift2^2 + shift1^2));

ack = 12*shift1^2 + (4*shift2 + 2*shift1)^2 - 12*C^2;

crn = 3*(C^2 - shift1^2)^2 + (3*C^2 - (2*shift2^2 + shift1^2))^2;


rtemp = (- bel + sqrt( bel^2 - 4*ack*crn ))/(2*ack);
   
rtemp2 = (- bel - sqrt( bel^2 - 4*ack*crn ))/(2*ack);


%compute value of theta for this point
thetemp = acos( (C^2 + 2*rtemp*shift1 - shift1^2)/(2*rtemp*C) );
thetemp2 = acos( (C^2 + 2*rtemp2*shift1 - shift1^2)/(2*rtemp2*C) );


temp = [rtemp, thetemp, rtemp2, thetemp2];