function [X,Y,Z,iobj]=moordyn(U,z,H,B,Cd,ME,V,W,rho)
% function [X,Y,Z,iobj]=moordyn(U,z,H,B,Cd,ME,V,W,rho)
%
% Calculate the mooring element positions relative to anchor, 
% X (+East), Y (+North) and Z (+Up) all in metres
%    given a velocity profile U(z) at depths z [m] (+up), with U(0)=0 
% For an oceanographic mooring from top to bottom
%    with N elemental components (including wire sections as elements),
%    dimensions H(L W D i,N) [m] of each different mooring component/element, 
%    mass/buoyancies B(N) [in kg, or kg/m for wire/chain] (+up),
%    Drag coefficients (Cd(N)). ME (modulus of elasticity indecies)
%    where 1=steel,2=Nylon,3=Dacron,4=Polyprop,5=Polyethy,6=Kevlar
% Output iobj are the idecices of the mooring "elements" not
%    including wire or chain [H(4,:)=1] elements.
%    Note: U (and V, W, and rho if provided) must extend to bottom (z=0).
% Optional inputs:
%    velocity components V(z) and W(z), rho(z)=density profile
%
% H(1,:) = Length/height of object/element including strength member [m]
%          H(1,N) is the height of the anchor [m]
% H(2,:) = Width of cylinder (0=zero if sphere) [m]
% H(3,:) = Diameter of sphere (0=zero if cylinder or wire/chain) [m]
% H(4,:) = 1 for wire/chain, 0 otherwise. Divide wire/chain into 1 m lengths
%
% May be passed with no arguments, assuming: global U z H B Cd V W rho
% RKD 12/97

if nargin == 0,
   global U V W z rho
   global H B Cd ME moorele
end
global Hs Bs Cds iss % saved for multiple float or "S" moorings
global moorele X Y Z Ti iobj jobj psi

X=[];Y=[];Z=[];Ti=[];iobj=[];jobj=[];psi=[];
if isempty(iss),
   Hs=H;Bs=B;Cds=Cd; % save the original mooring design.
end

% first change masses/buoyancies into forces (Newtons)
Bw=B*9.81; % Turn masses/buoyancies into forces
Bmax=Bw(1); % maximum buoyancy available at top of mooring
%
N=length(B); % initial number of elements, before segmenting wire/chain into 1 m lengths
%
% First determine if this is a sub-surface or surface float mooring.
% This is determined by the maximum height of the velocity profile as the water depth
Zw=max(z);  % The water depth!
S=sum(H(1,:)); % maximum length of the mooring
disp('  ');
if Zw > S, % then this is a sub-surface mooring
   ss=1;
   disp('This is (starting off as) a sub-surface mooring');
   gamma=1;
else % this is a surface float mooring
   ss=0;
   disp('This is (starting off as) a potential surface float mooring');
end
disp('Searching for a converged solution.');
%
j=1; % for node setup, segment from bottom to top
Zi(1)=H(1,N);  % height of the top of the anchor, start of mooring
Hi(:,1)=H(:,N); % setup interpolated H,B,Cd variables
Bi(:,1)=Bw(:,N);
Cdi(1)=Cd(N);
MEi(1)=ME(N);
z0=H(1,N); % height of top of first element (anchor)
for i=N-1:-1:1, % setup initial heights of interpolated elements
   j=j+1;
   if H(4,i)==1,  % this section is wire/chain
      n=fix(H(1,i));
      if n <2,
         n=2;
      elseif n > 50,
         n=50;
      end % make it at least 2 segments long, no more than 50
      dz=H(1,i)/n;
      for jj=j:j+n-1, 
         Zi(jj)=z0+dz/2;
         z0=z0+dz;
         Hi(:,jj)=[dz H(2,i) H(3,i) H(4,i)]';
         Bi(jj)=Bw(i)*dz; % the stored mass is per unit metre
         Cdi(jj)=Cd(i);
         MEi(jj)=ME(i);
      end
      j=j+n-1;
   else
      Zi(j)=z0 + H(1,i)/2;
      z0=z0+H(1,i);
      Hi(:,j)=H(:,i);
      Bi(j)=Bw(i);
      Cdi(j)=Cd(i);
      MEi(j)=ME(i);
   end
end
%
% now interpolate the velocity profile to 1 m estimates
dz=1;
if z(1)>z(2), dz=-1; end
zi=[z(1):dz:z(length(z))];
if ~isempty(U),
   Ui=interp1(z,U,zi);
else
   Ui=interp1([120 20 0],[0 0 0],zi);
end
if ~isempty(V),
   Vi=interp1(z,V,zi);
else
   Vi=zeros(size(Ui));
end
if ~isempty(W),
   Wi=interp1(z,W,zi);
else
   Wi=zeros(size(Ui));
end
if ~isempty(rho),
   rhoi=interp1(z,rho,zi);
else
   rhoi=ones(size(Ui))*1025; % default density is 1025 kg/m^3
end
%
N=length(Bi); % the new number of "interpolated" mooring elements
% Now find the drag on each element assuming first a vertical mooring.
if ss==0, % for surface float moorings...
   Bo=-sum(Bi(2:N-1)) + sum(Cdi(2:N-1));% sum the weight of all elements except the surface float and anchor
   Zi=Zi*Zw/S; % force elements to be in water for drag, etc....
   Boo=Bo;
   gamma=Bo/Bmax;
   if gamma> 1, gamma=0.9; end
end
Umag=sqrt(Ui.^2 + Vi.^2 + Wi.^2);
for j=1:N,
   i=find(zi >= (Zi(j)-0.5) & zi <= (Zi(j)+0.5));
   i=i(1); % just in case it got more than one value/index
   if Hi(3,j)==0, % then this is a cylinder/wire/chain section
      A=Hi(1,j)*Hi(2,j); % area of cylinder/wire/chain
   else
      A=pi*(Hi(3,j)/2)^2; % area f sphere
   end
   Qx(j)=0.5*rhoi(i)*Cdi(j)*A*Umag(i)*Ui(i);
   Qy(j)=0.5*rhoi(i)*Cdi(j)*A*Umag(i)*Vi(i);
   if Hi(3,j)==0, % then this is a cylinder/wire/chain section
      A=pi*(Hi(2,j)/2)^2; % area of bottom of cylinder
      if Hi(4,j)==1, A=0; end % if wire, vertical area =0
   end   
   Qz(j)=0.5*rhoi(i)*Cdi(j)*A*Umag(i)*Wi(i);
end
% Flip mooring right side up, indecies start at top.
Qx=fliplr(Qx);Qy=fliplr(Qy);Qz=fliplr(Qz); 
Hi=fliplr(Hi);Bi=fliplr(Bi);Cdi=fliplr(Cdi);MEi=fliplr(MEi);
%
% First Pass
% Now we solve for first order wire angles, starting at the top of the mooring, 
%  where there is no tension from above, Ti=0.
% Then there are three equations and three unknowns at each element.
% 1) Qx(i) + T(i)*cos(theta(i))*sin(psi(i)) = T(i+1)*cos(theta(i+1))*sin(psi(i+1))
% 2) Qy(i) + T(i)*sin(theta(i))*sin(psi(i)) = T(i+1)*sin(theta(i+1))*sin(psi(i+1))
% 3) Wz(i) + Qz(i) + T(i)*cos(psi(i)) = T(i+1)*cos(psi(i+1))
% where the Q's are the drags, Wz is the weight/buoyancy, 
%    T(i) is the tension from above, T(i+1) is the tension from below,
%    psi(i) is the wire angle from z, theta(i) angle in x-y plane
%    Calculate T(i+1), phi(i+1) and theta(i+1) at element i, 
%    working from the top T(1)=0, to the bottom
%
%  Here it is assumed that the top of the mooring is a float, or 
%  at least has positive buoyancy and will "lift" the first few elements.
%
Ti(1)=0;
theta(1)=0;
psi(1)=0;
b=gamma*(Bi(1)+Qz(1));
theta(2)=atan2(Qy(1),Qx(1));
Ti(2)=sqrt(Qx(1)^2 + Qy(1)^2 + b^2);
psi(2)=acos(b/Ti(2));
% Now Solve from top to bottom.
for i=2:N-1,
   ip1=i+1;
   xx=Qx(i)+Ti(i)*cos(theta(i))*sin(psi(i));
   yy=Qy(i)+Ti(i)*sin(theta(i))*sin(psi(i));
   zz=Bi(i)+Qz(i)+Ti(i)*cos(psi(i));
   theta(ip1)=atan2(yy,xx);
   Ti(ip1)=sqrt(xx^2 + yy^2 + zz^2);
   if Ti(ip1) ~= 0, % this is unlikely, but...
      psi(ip1)=acos(zz/Ti(ip1));
   else
      psi(ip1)=psi(i);
   end
end
%
% Now integrate from the bottom to the top to get the first order [x,y,z]
% Allow wire/rope sections to stretch under tension
%
for ii=1:2,
X(N)=0;Y(N)=0;Z(N)=Hi(1,N);  % reference things to top of anchor.
dx0=0;dy0=0;dz0=0;
for i=N-1:-1:1,
   if Hi(2,i)~=0,
      dL=1+(Ti(i)*4/(pi*Hi(2,i)^2*MEi(i)));
   else
      dL=1;
   end
   LpdL=Hi(1,i)*dL;
   X(i)=X(i+1) + LpdL*cos(theta(i))*sin(psi(i))/2 + dx0;
   Y(i)=Y(i+1) + LpdL*sin(theta(i))*sin(psi(i))/2 + dy0;
   Z(i)=Z(i+1) + LpdL*cos(psi(i))/2 + dz0;
   dx0=LpdL*cos(theta(i))*sin(psi(i))/2;
   dy0=LpdL*sin(theta(i))*sin(psi(i))/2;
   dz0=LpdL*cos(psi(i))/2;
   if Z(i) > Zw & Hi(4,i) == 1 & Bi(i) >0, Z(i)=Zw; psi(i)=pi/2; end % surface line
   if Z(i) <= Z(N), Z(i)= Z(N); psi(i)=pi/2; end % bottom chain
end
end
if max(Z)>Zw & ss==1, ss=0; gamma=0.8; end % this may be a surface mooring after all 
% Now with the first order positions, we must re-estimate the new
% drags at the new heights (Zi) and for cylinders tilted by psi in flow.
% If this is a surface float mooring, then increase the amount of the
% surface float that is submerged until the height to the bottom of the float is 
% within the range Zw > Zf > (Zw - H(1,1)) 
breaknow=0;
icnt=0;
iavg=0;
isave=0;Zfs=0;
dg=0.1;
gamma0=Ti(2)*cos(psi(2))/Bi(1); % estimate required lift
gammas=1;
if gamma < gamma0, gammas=-1;end
%
while breaknow==0,  % loop through a few times to converge solution.
%
isave=isave+1;
Zf=Z(1)-Hi(1,1)/2;  % height to bottom of surface float 
if ss==0, % working on surface float solution (harder problem).
   %disp(num2str([Zw max(Z) Zf gamma gamma0])); % display to test convergence problems
   izm=find(Z<0); % just in case
   Z(izm)=zeros(size(izm));
   gamma0=Ti(2)*cos(psi(2))/Bi(1); % estimate required lift
   if (Zf+Hi(1,1)) < Zw,  % then we're completely submerged! Increase buoyancy.
      if gammas==-1, gammas=1; dg=dg/10; end
      gamma=gamma+gammas*dg;
      if gamma>1, gamma=1; ss=1; end % this is now a subsurface mooring.
      Zfs=Zf;
      gammas=1;
   elseif Zw > Zf & Zw < (Zf+Hi(1,1)),  % then we're floating
      if gamma > gamma0,
         if gammas==1, gammas=-1; dg=dg/10; end
         gamma=gamma+gammas*dg;
      else
         if gammas==-1, gammas=1; dg=dg/10; end
         gamma=gamma+gammas*dg;
      end
      Zfs=Zf;
      izz=find(Hi(4,:)==0);
      if abs(dg)<0.00001 & max(Z(izz))>(Zf+Hi(1,1)), % then there is still part of this mooring above water!
         iss=1;
         NN=length(B);
         inext=find(B>0);
         if length(inext)>1,
            H=H(:,inext(2):NN);
            B=B(:,inext(2):NN);
            Cd=Cd(inext(2):NN);
            ME=ME(inext(2):NN);
            moorele=moorele(inext(2):NN,:);
            disp('!! Top link(s) in mooring removed !!');
            Z=[];
            dismoor;
            moordyn;
            return;
         else
            error('This mooring''s not working! Please examine.');
         end
      end
   elseif Zf > Zw,  % we're in trouble, the float is flying!
      if gammas==1, gammas=-1; dg=dg/10; end
      gamma=gamma+gammas*dg;
      if gamma>=1, gamma=Boo/Bmax;end
      if gamma < 0.01, % then this float is not required,
         iss=1;
         NN=length(B);
         inext=find(B>0);
         if length(inext)>1,
            H=H(:,inext(2):NN);
            B=B(inext(2):NN);
            Cd=Cd(inext(2):NN);
            ME=ME(inext(2):NN);
            moorele=moorele(inext(2):NN,:);
            disp('!! Top link(s) in mooring removed !!');
            Z=[];
            dismoor;
            moordyn;
            return
         else
            error('This mooring''s not working! Please examine.');
         end
      end
      Zfs=Zf;
   end
   if max(Z)> Zw, Z=Z*(Zw/max(Z)); end
elseif ss==1,
   Zf=max(Z);
   iavg=iavg+1;
   Zfs=Zfs+Zf;
   if iavg > 10,
      Z=Z*(Zfs/(iavg*Zf));
   end
end
icnt=icnt+1;
fprintf(1,'.');
if icnt>=60,
   icnt=0;
   disp(' ');
end
% Note drag on tilted cylinders/wire is Cd(psi)=Cd(0)*cos^3(phi), 
phix=atan2((Ti.*cos(theta).*sin(psi)),(Ti.*cos(psi)));
phiy=atan2((Ti.*sin(theta).*sin(psi)),(Ti.*cos(psi)));
Umag=sqrt(Ui.^2 + Vi.^2 + Wi.^2);
for j=1:N,
   i=find(zi>=(Z(j)-0.50)&zi<=(Z(j)+0.50));
   if isempty(i), 
      [Bi(j) j Z(j)]
      error(' Can''t find the velocity at this element!');
   end
   i=i(1); % just in case it got more than one value/index
   if Hi(3,j)==0, % then this is a cylinder/wire/chain section
      Ax=Hi(1,j)*cos(phix(j))*Hi(2,j); % area of cylinder/wire/chain
      Ay=Hi(1,j)*cos(phiy(j))*Hi(2,j); % area of cylinder/wire/chain
      Cdjx=Cdi(j)*cos(phix(j))^3;
      Cdjy=Cdi(j)*cos(phiy(j))^3;
   else
      A=pi*(Hi(3,j)/2)^2;
      Ax=A;Ay=A;
      Cdjx=Cdi(j);
      Cdjy=Cdjx;
   end
   Qx(j)=0.5*rhoi(i)*Cdjx*Ax*Umag(i)*Ui(i);
   Qy(j)=0.5*rhoi(i)*Cdjy*Ay*Umag(i)*Vi(i);
   if Hi(3,j)==0, % then this is a tilted cylinder/wire/chain section
      A=Hi(1,j)*sin(psi(j))*Hi(2,j); % area of tilted cylinder looking up
      Cdj=Cdi(j)*sin(psi(j))^3;
   else
      A=pi*(Hi(3,j)/2)^2;
      Cdj=Cdi(j);
   end   
   Qz(j)=0.5*rhoi(i)*Cdj*A*Umag(i)*Wi(i); 
end
% Now re-solve for displacements with new positions/drags.
Ti(1)=0;
theta(1)=0;
psi(1)=0;
b=gamma*(Bi(1)+Qz(1));
if gamma<0.1, b=gamma*Bi(1)+(0.1+gamma)*Qz(1); end % don't let drag of float get too small
theta(2)=atan2(Qy(1),Qx(1));
Ti(2)=sqrt(Qx(1)^2 + Qy(1)^2 + b^2);
psi(2)=acos(b/Ti(2));
% Now Solve from top to bottom.
for i=2:N-1,
   ip1=i+1;
   xx=Qx(i)+Ti(i)*cos(theta(i))*sin(psi(i));
   yy=Qy(i)+Ti(i)*sin(theta(i))*sin(psi(i));
   zz=Bi(i)+Qz(i)+Ti(i)*cos(psi(i));
   theta(ip1)=atan2(yy,xx);
   Ti(ip1)=sqrt(xx^2 + yy^2 + zz^2);
   if Ti(ip1) ~= 0,
      psi(ip1)=acos(zz/Ti(ip1));
   else
      psi(ip1)=psi(i);
   end
end
%
% Now integrate from the bottom to the top to get the second order [x,y,z]
% Allow wire/rope to stretch under tension
%
for ii=1:2
X(N)=0;Y(N)=0;Z(N)=Hi(1,N);  % reference things to top of anchor.
dx0=0;dy0=0;dz0=0;
for i=N-1:-1:1,
   if Hi(2,i)~=0,
      dL=1+(Ti(i)*4/(pi*Hi(2,i)^2*MEi(i)));
   else
      dL=1;
   end
   LpdL=Hi(1,i)*dL;
   X(i)=X(i+1) + LpdL*cos(theta(i))*sin(psi(i))/2 + dx0;
   Y(i)=Y(i+1) + LpdL*sin(theta(i))*sin(psi(i))/2 + dy0;
   Z(i)=Z(i+1) + LpdL*cos(psi(i))/2 + dz0;
   dx0=LpdL*cos(theta(i))*sin(psi(i))/2;
   dy0=LpdL*sin(theta(i))*sin(psi(i))/2;
   dz0=LpdL*cos(psi(i))/2;
   if Z(i) > Zw & Hi(4,i) == 1 & Bi(i) >0, Z(i)=Zw; psi(i)=pi/2; end
   if Z(i) <= Z(N), Z(i)= Z(N); psi(i)=pi/2; end
end
psi(N)=psi(N-1);
end
if max(Z)>Zw & ss==1, ss=0; gamma=0.8; end % this may be a surface mooring after all 
%
deltaz=0.01;  % how close to you want the convergence!
%
if isave > 2,
   if abs(Zsave(isave-1)-Z(1)) < deltaz & abs(Zsave(isave-2)-Zsave(isave-1)) < deltaz,
      if ss==1,
         breaknow=1; % then the solution has converged.
      else
         if Zw >= Zf & Zw <= (Zf+Hi(1,1)),
            breaknow=1;
         end
      end
   end
end
Zsave(isave)=Z(1);
%
end % end convergence loop
I=(2:N-1);
iobj=find(Hi(4,:) == 0); % indices of the non-wire/joiner elements
jobj=1+find(Hi(4,I) == 1 & (Hi(4,I-1) ~= 1|Hi(4,I+1) ~= 1)); % indices of wire/rope/chain elements
ba=psi(N-1);
Wa=Ti(N)/9.81;
VWa=Wa*cos(ba);
HWa=sqrt(Wa^2 - VWa^2);
disp('  ');
disp(['Total Tension on Anchor [kg] = ',num2str(fix(Wa))]);
disp(['Vertical load [kg] = ',num2str(fix(VWa))]);
disp(['Horizontal load [kg] = ',num2str(fix(HWa))]);
disp(['After applying a WHOI saftey factor:']);
TWa=1.5*(VWa + HWa/0.6);
disp(['Safe wet anchor mass = ',num2str(fix(TWa)),' [kg] = ',num2str(fix(TWa*2.2)),' [lb]']);
disp(['Safe dry steel anchor mass = ',num2str(fix(TWa/0.87)),' [kg] = ',num2str(fix(TWa*2.2/0.87)),' [lb]']);
disp(['Safe dry concrete anchor mass = ',num2str(fix(TWa/0.65)),' [kg] = ',num2str(fix(TWa*2.2/0.65)),' [lb]']);
if ss==0,
   disp(['Using about ',num2str(fix(gamma*100)),'% of the surface float.']);
end
% fini
