function [enu, RotMat] = xyz_enu(vec, Heading, Pitch, Roll, enu_xyz);
% vec must have 3 rows; the column dimension should be time, corresponding
% to the row dimension of H, P, R.
% If there is an additional leading dimension to vec, it will be interpreted
% as cell number, as when vec is xyze(:,1:3,:)

%% 00/01/31 EF
%  Considerably modified for RDI from a version for Sontek profilers.
%  This version follows the RDI booklet, "ADCP Coordinate Transformation",
%  copyright 1998.
%%%%% Incomplete: does not provide for modified tilt; carefully check
%%%%% all H, P, R definitions for any particular application, referring
%%%%% to page 15.


if nargin == 3
   enu_xyz = Pitch;
elseif nargin ~= 5
   enu_xyz = '';
end


if strcmp(enu_xyz, 'inverse')
   enu_to_xyz = 1;
%   disp('inverse: enu to xyz');
else
   enu_to_xyz = 0;
%   disp('xyz to enu');
end
if nargin == 2 | nargin == 3, % second is [H P R]
   Pitch = Heading(:,2);
   Roll = Heading(:,3);
   Heading = Heading(:,1);
end



DegToRads = pi/180;

  dr =  DegToRads * Roll(:);
  dp =  DegToRads * Pitch(:);
  dh =  DegToRads * Heading(:);

if isempty(vec)
   vec = zeros(3,1);
end
sv = size(vec);
ndims = length(sv);
if ndims > 3
   error('The first argument must have no more than 3 dimensions.')
end

if ndims == 2 & sv(2) == 3 & sv(1) ~= 3
   vec = vec.';
   sv = size(vec);
end

if sv(ndims - 1) ~= 3
   error('One dimension of vec must be 3')
end


n = max([length(dh), length(dp), length(dr)]);
if n == 1,
   n = sv(end);
end
if ndims == 2 & sv(2) == 1 & n ~= 1,
   vec = vec(:,ones(n,1));
   sv(2) = n;
end
if sv(end) ~= n
   error('Dimension mismatch between vec and H, P, R')
end

if n > 1,
   if length(dh) == 1, dh = dh(ones(n,1)); end
   if length(dp) == 1, dp = dp(ones(n,1)); end
   if length(dr) == 1, dr = dr(ones(n,1)); end
end
if (length(dh)~= n | length(dp) ~= n |  length(dr) ~= n),
   error('Lengths of H, P, R don''t match.')
end

enu = zeros(sv);

  cr = cos(dr);
  sr = sin(dr);
  ch = cos(dh);
  sh = sin(dh);
  cp = cos(dp);
  sp = sin(dp);



for ii = 1:n
   MH = [ch(ii), sh(ii), 0; ...
         -sh(ii), ch(ii), 0; ...
         0, 0, 1];

   MP = [1, 0, 0; ...
         0, cp(ii), -sp(ii); ...
         0, sp(ii), cp(ii)];

   MR = [cr(ii), 0, sr(ii); ...
         0, 1, 0; ...
         -sr(ii), 0, cr(ii)];

   RM = MH * MP * MR;

   if enu_to_xyz
      RM = inv(RM);
   end
   if ndims == 3,
      enu(:,:,ii) = (RM * squeeze(vec(:,:,ii)).').';
      % dims of vec, from adp via beam_xyz, are cell, component, ping.
      % We have to transpose after squeezing so that the row dimension
      % is component; that is, each column is the 3 velocity components
      % for a given cell.
   else
      enu(:,ii) =  RM * vec(:,ii);
   end
end

