%
% PURPOSE:  Compute vehicle altitude from the 4 Dvl beams using a least-squares
%           fit.  
% DATE:     11 Apr 2023
%
% CHANGELOG: Initial:  RSM.
%
% Assume the vehicle's Dvl data is on the stack. 
%
% Form beam range vectors in the vehicle body frame:

  c3s2 = 1./sqrt(2.)/2.;   %cos(pi/4)*sin(pi/6),   pi/6 = 30 degrees.
  s3s2 = 1./sqrt(2.)/2.;   %sin(pi/4)*sin(pi/6)
  c2   = sqrt(3.)/2.;      %cos(pi/6)

  tic
  for(i=1:length(dataDvl))
    minDist(i,1) = NaN;
    minDistC(i,1) = NaN;
    %
    % v_B is 4x3, the rows are the beam vectors.
    v_B(1,:) = dataDvl(i,dvlBeam1)*[ c3s2 -s3s2 c2];         %Beam1
    v_B(2,:) = dataDvl(i,dvlBeam2)*[-c3s2  s3s2 c2];         %Beam2
    v_B(3,:) = dataDvl(i,dvlBeam3)*[ c3s2  s3s2 c2];         %Beam3
    v_B(4,:) = dataDvl(i,dvlBeam4)*[-c3s2 -s3s2 c2];         %Beam4
    %
    A = [ -v_B(:,1:2) -[1;1;1;1] ];
    y = v_B(:,3);
    if( cond(A) < 1e6 ) 
      %
      % y = mx + b
      % y = [ x 1 ]*[m b]'
      %
      % then
      %
      % y = Ax
      % x = A\y
      %
      x = A\y;
      %
      % Eqn of a plane in 3D from CRC. Different "x" from above.
      %
      % Ax + By + z = D
      % A/D x  + B/D y + 1/D z = 1
      %
      % normal = [A/D B/D 1/D]    (not unit).
      %
      nvec = [x(1) x(2) x(3)]';
      %
      minDist(i) = norm(nvec);
    end
    ATA = A'*A;
    detATA = ATA(1, 1) * (ATA(2, 2) * ATA(3, 3) - ATA(3, 2) * ATA(2, 3)) - ...
             ATA(1, 2) * (ATA(2, 1) * ATA(3, 3) - ATA(2, 3) * ATA(3, 1)) + ...
             ATA(1, 3) * (ATA(2, 1) * ATA(3, 2) - ATA(2, 2) * ATA(3, 1));
    if( detATA > 1e-6 )
      invdet = 1 / detATA;

      invATA(1, 1) = (ATA(2, 2) * ATA(3, 3) - ATA(3, 2) * ATA(2, 3)) * invdet;
      invATA(1, 2) = (ATA(1, 3) * ATA(3, 2) - ATA(1, 2) * ATA(3, 3)) * invdet;
      invATA(1, 3) = (ATA(1, 2) * ATA(2, 3) - ATA(1, 3) * ATA(2, 2)) * invdet;
      invATA(2, 1) = (ATA(2, 3) * ATA(3, 1) - ATA(2, 1) * ATA(3, 3)) * invdet;
      invATA(2, 2) = (ATA(1, 1) * ATA(3, 3) - ATA(1, 3) * ATA(3, 1)) * invdet;
      invATA(2, 3) = (ATA(2, 1) * ATA(1, 3) - ATA(1, 1) * ATA(2, 3)) * invdet;
      invATA(3, 1) = (ATA(2, 1) * ATA(3, 2) - ATA(3, 1) * ATA(2, 2)) * invdet;
      invATA(3, 2) = (ATA(3, 1) * ATA(1, 2) - ATA(1, 1) * ATA(3, 2)) * invdet;
      invATA(3, 3) = (ATA(1, 1) * ATA(2, 2) - ATA(2, 1) * ATA(1, 2)) * invdet;
      
      xC = invATA*A'*y;
      minDistC(i,1) = norm(xC);
    end
    %
    % Now try the code to find the bug:
    %
      clear A, ATA, detATA;
      
      beamRange(1) = dataDvl(i,dvlBeam1);
      beamRange(2) = dataDvl(i,dvlBeam2);
      beamRange(3) = dataDvl(i,dvlBeam3);
      beamRange(4) = dataDvl(i,dvlBeam4);

      v_BB(1,1) =  c3s2*beamRange(1);
      v_BB(1,2) = -s3s2*beamRange(1);
      v_BB(1,3) =    c2*beamRange(1);
      
      v_BB(2,1) = -c3s2*beamRange(2);
      v_BB(2,2) =  s3s2*beamRange(2);
      v_BB(2,3) =    c2*beamRange(2);
      
      v_BB(3,1) =  c3s2*beamRange(3);
      v_BB(3,2) =  s3s2*beamRange(3);
      v_BB(3,3) =    c2*beamRange(3);
      
      v_BB(4,1) = -c3s2*beamRange(4);
      v_BB(4,2) = -s3s2*beamRange(4);
      v_BB(4,2) =    c2*beamRange(4);
      %
      % Set up the least squares:
      for( ii=1:4)
         A(ii,1) = -v_BB(ii,1);
         A(ii,2) = -v_BB(ii,2);
         A(ii,3) = -1;

         y(ii) = v_BB(ii,3);
      end
      %
      % Take the transpose:
      for( ii=1:3)
         for( j=1:4)
            AT(ii,j) = A(j,ii);
         end
      end
      %
      % Multiply A'*A:
      for( ii=1:3)
         for( j=1:3)
            ATA(ii,j) = 0.0;
            for( k=1:4 ) 
              ATA(ii,j) = ATA(ii,j) + AT(ii,k)*A(k,j); 
            end
         end
      end
      %
      % Compute the derminant:
      detATA = ATA(1,1) * (ATA(2,2) * ATA(3,3) - ATA(3,2) * ATA(2,3)) - ...
               ATA(1,2) * (ATA(2,1) * ATA(3,3) - ATA(2,3) * ATA(3,1)) + ...
               ATA(1,3) * (ATA(2,1) * ATA(3,2) - ATA(2,2) * ATA(3,1));
      if( detATA > 1e-6 )
         invdet = 1./detATA;
         invATA(1,1) = (ATA(2,2) * ATA(3,3) - ATA(3,2) * ATA(2,3)) * invdet;
         invATA(1,2) = (ATA(1,3) * ATA(3,2) - ATA(1,2) * ATA(3,3)) * invdet;
         invATA(1,3) = (ATA(1,2) * ATA(2,3) - ATA(1,3) * ATA(2,2)) * invdet;
         invATA(2,1) = (ATA(2,3) * ATA(3,1) - ATA(2,1) * ATA(3,3)) * invdet;
         invATA(2,2) = (ATA(1,1) * ATA(3,3) - ATA(1,3) * ATA(3,1)) * invdet;
         invATA(2,3) = (ATA(2,1) * ATA(1,3) - ATA(1,1) * ATA(2,3)) * invdet;
         invATA(3,1) = (ATA(2,1) * ATA(3,2) - ATA(3,1) * ATA(2,2)) * invdet;
         invATA(3,2) = (ATA(3,1) * ATA(1,2) - ATA(1,1) * ATA(3,2)) * invdet;
         invATA(3,3) = (ATA(1,1) * ATA(2,2) - ATA(2,1) * ATA(1,2)) * invdet;
         %
         % Multiply A'*y
         for( ii=1:3)
           temp(ii) = 0.0;
           for( j=1:4) temp(ii) = temp(ii) + AT(ii,j)*y(j);end
         end
         %
         % Multiply invATA*(A'*y)
         for( ii=1:3)
           x(ii) = 0.0;
           for( j=1:3) x(ii) = x(ii) + invATA(ii,j)*temp(j);end
         end
         %
         % The minimum distance to the plane is norm(x).
         
         minDistCC(i,1) =  sqrt( x(1)*x(1) + x(2)*x(2) + x(3)*x(3) );
       else
         minDistCC(i,1) =  NaN;
       end

  end
  toc
