function [N1 N2 N3 CU] = p3p_distances( d12, d23, d31, c12, c23, c31  )
%P3P_DISTANCES P3P problem - points-to-camera distances.
%
% [N1 N2 N3 C] = p3p_distances( d12, d23, d31, c12, c23, c31  )
%
%   Input: 
%     d12, d23, d31 .. distances between three points in the space (scalars)
%
%     c12, c23, c31 .. appropriate cosines (scalars)
%
%   Output:
%     N1, N2, N3 .. distances, row vectors 1 x num where 
%                   num is number of solutions. Let integer i >= 1, i <= num,
%                   the N1(i), N2(i), and N3(i) is a single solution.
%     CU ..         cases used, matrix 1 x num, where C(i) is a number
%                   describing which case was used for i-th solution;
%                   either 1 for A, 2 for B1, 3 for B21, or 4 for B22.

N1 = [];
N2 = [];
N3 = [];
CU = [];

thr = 1e-4; % threshold for p3p_dverify

[a0 a1 a2 a3 a4] = p3p_polynom( d12, d23, d31, c12, c23, c31 );

% ------------------------------------------------------------------------------
% Case A

% Companion matrix ...

% Solve for n12 ...

% Use only real solutions ...

for i = 1:num_sols
  % Replace this with your results
  n1 = 0;
  n2 = 0;
  n3 = 0;

  % Use only solutions with error from p3p_dverify smaller than threshold

  e = p3p_dverify( n1, n2, n3, d12, d23, d31, c12, c23, c31 );

  if( all( abs(e) < thr ) )
    N1 = [ N1 n1 ];
    N2 = [ N2 n2 ];
    N3 = [ N3 n3 ];
    CU = [ CU 1 ];
  end
end

% ------------------------------------------------------------------------------
if( c23 ~= 0 ) % Case B1
  
  % quadratic equation...
  
  % Use only solutions with error from p3p_dverify smaller than threshold
  % CU .. 2.
 
% ------------------------------------------------------------------------------
else % Case B2
  if( d23^2 ~= d31^2 )
    % Use only solutions with error from p3p_dverify smaller than threshold
    % CU .. 3.
  else
    % Use only solutions with error from p3p_dverify smaller than threshold
    % CU .. 4.
  end
end
