
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 
% Shallow-water sloshing in vessels undergoing prescribed rigid-body motion
% in three dimensions.
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

% Written by H. Alemi Ardakani & T.J. Bridges,
% Department of Mathematics, University of Surrey, UK.


clear
clc

tic
format long
g=9.81;
L=0;
R=1.0;
T=0.8;
B=0;
m=50;
n=50;
Dxi=1/m;
Deta=1/n;
Dx=R/m;
Dy=T/n;

           deltaT=0.01;
           h0=0.20;

                      omegafre1=(pi/R)*sqrt(g*h0);
                      omegafre2=(pi/T)*sqrt(g*h0);
                   
                      epsilon1=1.0*pi/180;  % theta: roll
                      epsilon2=3.0*pi/180;  % phi: pitch
                      epsilon3=0.0*pi/180;  % gama: yaw
                      omega1=0.5*omegafre1;
                      omega2=0.5*omegafre2;
                      omega3=0.0*omegafre1;
                      epsilon4=0.0;  % surge
                      epsilon5=0.0;  % sway
                      epsilon6=0.0;  % heave
                      omega4=0.0*omega2;
                      omega5=0.0*omega2;
                      omega6=0.0;

           
for i=1:m+1
    Xb(i)=(i-1)*((R-L)/m);
    Xt(i)=(i-1)*((R-L)/m);
    Yb(i)=B;
    Yt(i)=T;
end

for j=1:n+1
    Xl(j)=L;
    Xr(j)=R;
    Yl(j)=(j-1)*((T-B)/n);
    Yr(j)=(j-1)*((T-B)/n);
end


           for j=1:n+1
                eta=(j-1)*Deta;
           for i=1:m+1
               xi=(i-1)*Dxi;
               X(j,i)=((1-xi)*Xl(j))+(xi*Xr(j))+((1-eta)*Xb(i))+(eta*Xt(i))-((1-xi)*(1-eta)*Xb(1))-((1-xi)*eta*Xt(1))...
                  -((1-eta)*xi*Xb(m+1))-(xi*eta*Xt(m+1));

               Y(j,i)=((1-xi)*Yl(j))+(xi*Yr(j))+((1-eta)*Yb(i))+(eta*Yt(i))-((1-xi)*(1-eta)*Yb(1))-((1-xi)*eta*Yt(1))...
                   -((1-eta)*xi*Yb(m+1))-(xi*eta*Yt(m+1));              
           end
           end
           

% %  Initial condition without vorticity.           
  for j=1:n+1
    for i=1:m+1
        EH(j,i)=h0;
        hstar(j,i)=h0;
        Ufirst(j,i)=0;
        Vfirst(j,i)=0;
    end
  end



% % % %  Initial condition with vorticity.

% Avor=-0.1;
% mvor=1;
% nvor=1;
% 
%   for j=1:n+1
%     for i=1:m+1
%         EH(j,i)=h0;
%         hstar(j,i)=h0;
%         Ufirst(j,i)=Avor*sin((mvor*pi*X(j,i))/R)*cos((nvor*pi*Y(j,i))/T);
%         Vfirst(j,i)=(-mvor/nvor)*(T/R)*Avor*cos((mvor*pi*X(j,i))/R)*sin((nvor*pi*Y(j,i))/T);
%     end
%   end
  
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %   

% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

d=[-R/2 -T/2 -3*T];
di=[d(1);d(2);d(3)];

al=[-R/2;-T/2;d(3)];
ar=[R/2;-T/2;d(3)];
bl=[-R/2;T/2;d(3)];
br=[R/2;T/2;d(3)];

cl=[-R/2;-T/2;d(3)+2.5*h0];
cr=[R/2;-T/2;d(3)+2.5*h0];
dl=[-R/2;T/2;d(3)+2.5*h0];
dr=[R/2;T/2;d(3)+2.5*h0];

alf=[-R/2;-T/2;d(3)];
arf=[R/2;-T/2;d(3)];
blf=[-R/2;T/2;d(3)];
clf=[-R/2;-T/2;d(3)+1.5*h0];


ex1=[al(1);ar(1)];
vay1=[al(2);ar(2)];
zed1=[al(3);ar(3)];

ex2=[al(1);bl(1)];
vay2=[al(2);bl(2)];
zed2=[al(3);bl(3)];

ex3=[ar(1);br(1)];
vay3=[ar(2);br(2)];
zed3=[ar(3);br(3)];

ex4=[bl(1);br(1)];
vay4=[bl(2);br(2)];
zed4=[bl(3);br(3)];

ex5=[cl(1);cr(1)];
vay5=[cl(2);cr(2)];
zed5=[cl(3);cr(3)];

ex6=[cl(1);dl(1)];
vay6=[cl(2);dl(2)];
zed6=[cl(3);dl(3)];

ex7=[cr(1);dr(1)];
vay7=[cr(2);dr(2)];
zed7=[cr(3);dr(3)];

ex8=[dl(1);dr(1)];
vay8=[dl(2);dr(2)];
zed8=[dl(3);dr(3)];

ex9=[al(1);cl(1)];
vay9=[al(2);cl(2)];
zed9=[al(3);cl(3)];

ex10=[ar(1);cr(1)];
vay10=[ar(2);cr(2)];
zed10=[ar(3);cr(3)];

ex11=[bl(1);dl(1)];
vay11=[bl(2);dl(2)];
zed11=[bl(3);dl(3)];

ex12=[br(1);dr(1)];
vay12=[br(2);dr(2)];
zed12=[br(3);dr(3)];


ex1f=[alf(1);arf(1)];
vay1f=[alf(2);arf(2)];
zed1f=[alf(3);arf(3)];

ex2f=[alf(1);blf(1)];
vay2f=[alf(2);blf(2)];
zed2f=[alf(3);blf(3)];

ex3f=[alf(1);clf(1)];
vay3f=[alf(2);clf(2)];
zed3f=[alf(3);clf(3)];



t(1)=0;

I=[1 0 0;0 1 0;0 0 1];
Qn=I;

e1=[1;0;0];
e2=[0;1;0];
e3=[0;0;1];


p1(1)=EH(1,1);
p2(1)=EH(1,end);
p3(1)=EH(end,1);
p4(1)=EH(end,end);
p5(1)=EH(m/2,1);
p6(1)=EH(m/2,end);
p7(1)=EH(1,n/2);
p8(1)=EH(end,n/2);

% pw1{1,1}=EH(:,1);
% pw2{1,1}=EH(:,end);
% pw3{1,1}=EH(1,:);
% pw4{1,1}=EH(end,:);
% numW=1;


for ip=1:m+1
    for jp=1:n+1
        pos{jp,ip}=(Qn*([X(jp,ip);Y(jp,ip);EH(jp,ip)]+di));
    end
end
posp=cell2mat(pos);
for jp=1:n+1
    Xp(jp,:)=posp(3*(jp-1)+1,:);
    Yp(jp,:)=posp(3*(jp-1)+2,:);
    EHp(jp,:)=posp(3*(jp-1)+3,:);
end

% hold on
% % box on
% surf(Xp,Yp,EHp);shading interp
% xlim([min(Xp(1,:)) max(Xp(1,:))]);ylim([min(Yp(:,1)) max(Yp(:,1))]);zlim([0 2.5*h0])
% axis off
% plot3(ex1f,vay1f,zed1f,'b','LineWidth',2)
% plot3(ex2f,vay2f,zed2f,'b','LineWidth',2)
% plot3(ex3f,vay3f,zed3f,'b','LineWidth',2)
% 
% plot3(ex1,vay1,zed1,'k','LineWidth',2.5)
% plot3(ex2,vay2,zed2,'k','LineWidth',2.5)
% plot3(ex3,vay3,zed3,'k','LineWidth',2.5)
% plot3(ex4,vay4,zed4,'k','LineWidth',2.5)
% plot3(ex5,vay5,zed5,'k','LineWidth',2.5)
% plot3(ex6,vay6,zed6,'k','LineWidth',2.5)
% plot3(ex7,vay7,zed7,'k','LineWidth',2.5)
% plot3(ex8,vay8,zed8,'k','LineWidth',2.5)
% plot3(ex9,vay9,zed9,'k','LineWidth',2.5)
% plot3(ex10,vay10,zed10,'k','LineWidth',2.5)
% plot3(ex11,vay11,zed11,'k','LineWidth',2.5)
% plot3(ex12,vay12,zed12,'k','LineWidth',2.5)
% set(gcf,'Color',[1,1,1])
% set(gca,'CameraPosition',[-0.6 -0.5 0.5])
% 
% 
% hold off
% set(gca,'nextplot','replacechildren');
% 
% Image(1)=getframe;


% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 


for num=1:350

    t(num+1)=num*deltaT;
    tT=t(num)+deltaT/2;

    theta=epsilon1*sin(omega1*tT);
    bigomega1=epsilon1*omega1*cos(omega1*tT);
    bigomega1dot=-epsilon1*(omega1^2)*sin(omega1*tT);

    %%%%%%% change of sin to cos.
%     theta=epsilon1*cos(omega1*tT);
%     bigomega1=-epsilon1*omega1*sin(omega1*tT);
%     bigomega1dot=-epsilon1*(omega1^2)*cos(omega1*tT);

    phi=epsilon2*sin(omega2*tT);
    bigomega2=epsilon2*omega2*cos(omega2*tT);
    bigomega2dot=-epsilon2*(omega2^2)*sin(omega2*tT);

%     phi=-epsilon2*sin(omega2*tT);
%     bigomega2=-epsilon2*omega2*cos(omega2*tT);
%     bigomega2dot=epsilon2*(omega2^2)*sin(omega2*tT);

    gama=epsilon3*sin(omega3*tT);
    bigomega3=epsilon3*omega3*cos(omega3*tT);
    bigomega3dot=-epsilon3*(omega3^2)*sin(omega3*tT);
   
    sur=epsilon4*sin(omega4*tT);
    surdot=epsilon4*omega4*cos(omega4*tT);
    surddot=-epsilon4*(omega4^2)*sin(omega4*tT);
    
%     sur=epsilon4*cos(omega4*tT);
%     surdot=-epsilon4*omega4*sin(omega4*tT);
%     surddot=-epsilon4*(omega4^2)*cos(omega4*tT);

    swa=epsilon5*sin(omega5*tT);
    swadot=epsilon5*omega5*cos(omega5*tT);
    swaddot=-epsilon5*(omega5^2)*sin(omega5*tT);
    
%     swa=epsilon5*cos(omega5*tT);
%     swadot=-epsilon5*omega5*sin(omega5*tT);
%     swaddot=-epsilon5*(omega5^2)*cos(omega5*tT);
   
    hea=epsilon6*sin(omega6*tT);
    headot=epsilon6*omega6*cos(omega6*tT);
    headdot=-epsilon6*(omega6^2)*sin(omega6*tT);
    
    qddot=[surddot;swaddot;headdot];
    
% % Angular velocity vector for the coupled yaw-pitch-roll motion based on
% % the Euler angles.

%     omegahat1=bigomega1-(bigomega3*sin(phi));
%     omegahat2=(bigomega3*cos(phi)*sin(theta))+(bigomega2*cos(theta));
%     omegahat3=(bigomega3*cos(phi)*cos(theta))-(bigomega2*sin(theta));
%     
%     bigomega1B=omegahat1;
%     bigomega2B=omegahat2;
%     bigomega3B=omegahat3;
%     
%     bigomega1dotB=bigomega1dot-(bigomega3dot*sin(phi))-(bigomega2*bigomega3*cos(phi));
%     bigomega2dotB=(bigomega3dot*cos(phi)*sin(theta))+(bigomega3*((-bigomega2*sin(phi)*sin(theta))+(bigomega1*cos(theta)*cos(phi))))...
%                   +(bigomega2dot*cos(theta))-(bigomega1*bigomega2*sin(theta));
%     bigomega3dotB=(bigomega3dot*cos(phi)*cos(theta))+(bigomega3*((-bigomega2*sin(phi)*cos(theta))-(bigomega1*sin(theta)*cos(phi))))...
%                   -(bigomega2dot*sin(theta))-(bigomega1*bigomega2*cos(theta));
%               
%     Qnhalf=[cos(phi)*cos(gama) (sin(theta)*sin(phi)*cos(gama))-(cos(theta)*sin(gama)) (cos(theta)*sin(phi)*cos(gama))+(sin(theta)*sin(gama));...
%             cos(phi)*sin(gama) (sin(theta)*sin(phi)*sin(gama))+(cos(theta)*cos(gama)) (cos(theta)*sin(phi)*sin(gama))-(sin(theta)*cos(gama));...
%             -sin(phi) sin(theta)*cos(phi) cos(theta)*cos(phi)];              
              
% % Pure yaw motion:
% bigomega1B=0;
% bigomega2B=0;
% bigomega3B=bigomega3;
% 
% bigomega1dotB=0;
% bigomega2dotB=0;
% bigomega3dotB=bigomega3dot;
% 
% Qnhalf=[cos(gama) -sin(gama) 0;...
%         sin(gama) cos(gama) 0;...
%         0 0 1];

% % Pure pitch motion:
% bigomega1B=0;
% bigomega2B=bigomega2;
% bigomega3B=0;
% 
% bigomega1dotB=0;
% bigomega2dotB=bigomega2dot;
% bigomega3dotB=0;
% 
% Qnhalf=[cos(phi) 0 -sin(phi);...
%         0 1 0;...
%         sin(phi) 0 cos(phi)];

    
% % Angular velocity vector for the coupled roll-pitch motion based on the
% % Euler angles.
    omegahat1=bigomega1*cos(phi);
    omegahat2=bigomega2;
    omegahat3=bigomega1*sin(phi);
   
    
    bigomega1B=omegahat1;
    bigomega2B=omegahat2;
    bigomega3B=omegahat3;
    
    bigomega1dotB=(bigomega1dot*cos(phi))-(bigomega1*bigomega2*sin(phi));
    bigomega2dotB=bigomega2dot;
    bigomega3dotB=(bigomega1dot*sin(phi))+(bigomega1*bigomega2*cos(phi));

    Qnhalf=[cos(phi) 0 sin(phi);...
            sin(theta)*sin(phi) cos(theta) -sin(theta)*cos(phi);...
            -cos(theta)*sin(phi) sin(theta) cos(theta)*cos(phi)];

Nhalf=[1 0 0;(2*bigomega2B)+((1/2)*deltaT*((bigomega2dotB)+(bigomega1B*bigomega3B))) 1 0;0 0 1];    
    
A=cell(m+1);
b=cell(m+1,1);

jj=1;

vstar=Vfirst(jj,:);
ustar=Ufirst(jj,:);

  for e=1:m+1
      alpha(e)=(dot((g*(Qnhalf*e3)),e3))+(dot((Qnhalf*e3),qddot))...
          -(((bigomega1B*bigomega2B)-bigomega2dotB)*(X(jj,e)+d(1)))+((Y(jj,e)+d(2))*((bigomega1dotB+(bigomega2B*bigomega3B)))-...
          ((bigomega1B^2)+(bigomega2B^2))*(d(3)));
      
      betahat(e)=-(dot((g*(Qnhalf*e1)),e3))-(dot((Qnhalf*e1),qddot))...
          +((X(jj,e)+d(1))*((bigomega2B^2)+(bigomega3B^2)))-((bigomega2dotB+(bigomega1B*bigomega3B))*(d(3)))+...
          ((bigomega3dotB-(bigomega1B*bigomega2B))*(Y(jj,e)+d(2)));
      
      betatilde(e)=-(dot((g*(Qnhalf*e2)),e3))-(dot((Qnhalf*e2),qddot))...
          -((bigomega3dotB+(bigomega1B*bigomega2B))*(X(jj,e)+d(1)))+((Y(jj,e)+d(2))*((bigomega1B^2)+(bigomega3B^2)))+...
          ((bigomega1dotB-(bigomega2B*bigomega3B))*(d(3)));
  end

A{1,1}=eye(3,3);
for i=3:m+1
    A{1,i}=zeros(3);
end
b{1,1}=[1-((deltaT/(2*Dy))*Vfirst(jj+1,1)) 0 0;0 0 0;0 0 0]*[EH(jj,1);Ufirst(jj,1);Vfirst(jj,1)];

for ii=2:m
    A{ii,ii}=Nhalf;
    for i=ii+2:m+1
        A{ii,i}=zeros(3);
    end
        b{ii,1}=([1-((deltaT/(2*Dy))*Vfirst(jj+1,ii)) 0 0;2*bigomega2B 1 0;0 0 0]*[EH(jj,ii);Ufirst(jj,ii);Vfirst(jj,ii)])...
        +((deltaT/2)*[0;betahat(ii);0]);
        if ii>2
           for i=1:ii-2
               A{ii,i}=zeros(3);
           end
        end
end

for i=1:m-1
    A{m+1,i}=zeros(3);
end
A{m+1,m+1}=eye(3,3);
b{m+1,1}=[1-((deltaT/(2*Dy))*Vfirst(jj+1,m+1)) 0 0;0 0 0;0 0 0]*[EH(jj,m+1);Ufirst(jj,m+1);Vfirst(jj,m+1)];

    id=1;
    qpp=1;
    
while qpp>1E-8
       
A{1,2}=(deltaT/(2*Dx))*[0 hstar(jj,1) 0;0 0 0;0 0 0];

for ii=2:m
    hatalphanstar=alpha(ii)-(((bigomega1B^2)+(bigomega2B^2))*hstar(jj,ii));
    
    A{ii,ii-1}=-(deltaT/(4*Dx))*[ustar(ii) hstar(jj,ii) 0;hatalphanstar ustar(ii) 0;0 0 0];
              
    A{ii,ii+1}=-A{ii,ii-1};
end

A{m+1,m}=-(deltaT/(2*Dx))*[0 hstar(jj,m+1) 0;0 0 0;0 0 0];

AA=cell2mat(A);
bb=cell2mat(b);

  %x=triblocksolve(AA,bb,3);
  x=AA\bb;
hhalf=x(1:3:end);
uhalf=x(2:3:end);
vhalf=x(3:3:end);

for i=1:m+1
    Uhalf(jj,i)=uhalf(i);
    Vhalf(jj,i)=vhalf(i);
    Hhalf(jj,i)=hhalf(i);
end

qpp=max(abs(Hhalf(jj,:)-hstar(jj,:))+abs(Uhalf(jj,:)-ustar)+abs(Vhalf(jj,:)-vstar));
id=id+1;

ustar=Uhalf(jj,:);
vstar=Vhalf(jj,:);
hstar(jj,:)=Hhalf(jj,:);

end

% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

Bnhalf=[1 0 0;(2*bigomega2B)+((1/2)*deltaT*(bigomega2dotB+(bigomega1B*bigomega3B))) 1 -deltaT*bigomega3B;...
        (-2*bigomega1B)-((1/2)*deltaT*(bigomega1dotB-(bigomega2B*bigomega3B))) deltaT*bigomega3B 1];

Enhalf=[1 0 0;0 1 0;(-2*bigomega1B)+((-1/2)*deltaT*(bigomega1dotB-(bigomega2B*bigomega3B))) 0 1];

Dnhalf=[1 0 0;2*bigomega2B 1 0;-2*bigomega1B 0 1];

Hnhalf=[1 0 0;0 0 0;-2*bigomega1B 0 1];

    for jj=2:n
        
 A=cell(m+1);
 b=cell(m+1,1);
 
vstar=Vfirst(jj,:);
ustar=Ufirst(jj,:);

  for e=1:m+1
      alpha(e)=(dot((g*(Qnhalf*e3)),e3))+(dot((Qnhalf*e3),qddot))...
          -(((bigomega1B*bigomega2B)-bigomega2dotB)*(X(jj,e)+d(1)))+((Y(jj,e)+d(2))*((bigomega1dotB+(bigomega2B*bigomega3B)))-...
          ((bigomega1B^2)+(bigomega2B^2))*(d(3)));
      
      betahat(e)=-(dot((g*(Qnhalf*e1)),e3))-(dot((Qnhalf*e1),qddot))...
          +((X(jj,e)+d(1))*((bigomega2B^2)+(bigomega3B^2)))-((bigomega2dotB+(bigomega1B*bigomega3B))*(d(3)))+...
          ((bigomega3dotB-(bigomega1B*bigomega2B))*(Y(jj,e)+d(2)));
      
      betatilde(e)=-(dot((g*(Qnhalf*e2)),e3))-(dot((Qnhalf*e2),qddot))...
          -((bigomega3dotB+(bigomega1B*bigomega2B))*(X(jj,e)+d(1)))+((Y(jj,e)+d(2))*((bigomega1B^2)+(bigomega3B^2)))+...
          ((bigomega1dotB-(bigomega2B*bigomega3B))*(d(3)));
  end

hatalphanhalf=alpha(1)-(((bigomega1B^2)+(bigomega2B^2))*EH(jj,1));    
A{1,1}=Enhalf;

for i=3:m+1
    A{1,i}=zeros(3);
end
Gjjiinhalf=(deltaT/(4*Dy))*[Vfirst(jj,1) 0 EH(jj,1);0 0 0;hatalphanhalf 0 Vfirst(jj,1)];
b{1,1}=(Gjjiinhalf*[EH(jj-1,1);Ufirst(jj-1,1);Vfirst(jj-1,1)])+(Hnhalf*[EH(jj,1);Ufirst(jj,1);Vfirst(jj,1)])+...
       (-Gjjiinhalf*[EH(jj+1,1);Ufirst(jj+1,1);Vfirst(jj+1,1)])+((deltaT/2)*[0;0;betatilde(1)]);

for ii=2:m
        
    if ii>2
       for i=1:ii-2
           A{ii,i}=zeros(3);
       end
    end

    A{ii,ii}=Bnhalf;
    for i=ii+2:m+1
        A{ii,i}=zeros(3);
    end
    tildetwoalphanhalf=alpha(ii)-(2*bigomega2B*Ufirst(jj,ii))-(((bigomega1B^2)+(bigomega2B^2))*EH(jj,ii));
        
    Cjjiinhalf=(deltaT/(4*Dy))*[Vfirst(jj,ii) 0 EH(jj,ii);2*bigomega2B*Vfirst(jj,ii) Vfirst(jj,ii) 0;tildetwoalphanhalf 0 Vfirst(jj,ii)];
    
    betanhalf=(deltaT/2)*[0;betahat(ii);betatilde(ii)];
    
    b{ii,1}=(Cjjiinhalf*[EH(jj-1,ii);Ufirst(jj-1,ii);Vfirst(jj-1,ii)])+(Dnhalf*[EH(jj,ii);Ufirst(jj,ii);Vfirst(jj,ii)])+...
            (-Cjjiinhalf*[EH(jj+1,ii);Ufirst(jj+1,ii);Vfirst(jj+1,ii)])+betanhalf;
        
    for i=1:m-1
        A{m+1,i}=zeros(3);
    end
end

hatalphanhalf=alpha(m+1)-(((bigomega1B^2)+(bigomega2B^2))*EH(jj,m+1));    
A{m+1,m+1}=Enhalf;
Gjjiinhalf=(deltaT/(4*Dy))*[Vfirst(jj,m+1) 0 EH(jj,m+1);0 0 0;hatalphanhalf 0 Vfirst(jj,m+1)];
b{m+1,1}=(Gjjiinhalf*[EH(jj-1,m+1);Ufirst(jj-1,m+1);Vfirst(jj-1,m+1)])+(Hnhalf*[EH(jj,m+1);Ufirst(jj,m+1);Vfirst(jj,m+1)])+...
       (-Gjjiinhalf*[EH(jj+1,m+1);Ufirst(jj+1,m+1);Vfirst(jj+1,m+1)])+((deltaT/2)*[0;0;betatilde(m+1)]);         

    id=1;
    qpp=1;
    
while qpp>1E-8
    
A{1,2}=(deltaT/(2*Dx))*[0 hstar(jj,1) 0;0 0 0;0 0 0];

for ii=2:m
    
    tildeonealphanhalfstar=alpha(ii)+(2*bigomega1B*vstar(ii))-(((bigomega1B^2)+(bigomega2B^2))*hstar(jj,ii));
    
    A{ii,ii-1}=-(deltaT/(4*Dx))*[ustar(ii) hstar(jj,ii) 0;tildeonealphanhalfstar ustar(ii) 0;-2*bigomega1B*ustar(ii) 0 ustar(ii)];
                                  
    A{ii,ii+1}=-A{ii,ii-1};
        
end

A{m+1,m}=(-deltaT/(2*Dx))*[0 hstar(jj,m+1) 0;0 0 0;0 0 0];

AA=cell2mat(A);
bb=cell2mat(b);

  %x=triblocksolve(AA,bb,2);
   x=AA\bb;
              
hhalf=x(1:3:end);
uhalf=x(2:3:end);
vhalf=x(3:3:end);

 for i=1:m+1
    Uhalf(jj,i)=uhalf(i);
    Vhalf(jj,i)=vhalf(i);
    Hhalf(jj,i)=hhalf(i);
 end
 
qpp=max(abs(Hhalf(jj,:)-hstar(jj,:))+abs(Uhalf(jj,:)-ustar)+abs(Vhalf(jj,:)-vstar));
% qpp=max(abs(Hhalf(jj,:)-hstar(jj,:)));
id=id+1;

ustar=Uhalf(jj,:);
vstar=Vhalf(jj,:);
hstar(jj,:)=Hhalf(jj,:);

end
    end
    
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

 A=cell(m+1);
 b=cell(m+1,1);


jj=n+1;

vstar=Vfirst(jj,:);
ustar=Ufirst(jj,:);

  for e=1:m+1
      alpha(e)=(dot((g*(Qnhalf*e3)),e3))+(dot((Qnhalf*e3),qddot))...
          -(((bigomega1B*bigomega2B)-bigomega2dotB)*(X(jj,e)+d(1)))+((Y(jj,e)+d(2))*((bigomega1dotB+(bigomega2B*bigomega3B)))-...
          ((bigomega1B^2)+(bigomega2B^2))*(d(3)));
      
      betahat(e)=-(dot((g*(Qnhalf*e1)),e3))-(dot((Qnhalf*e1),qddot))+((X(jj,e)+d(1))*((bigomega2B^2)+(bigomega3B^2)))...
          -((bigomega2dotB+(bigomega1B*bigomega3B))*(d(3)))+...
          ((bigomega3dotB-(bigomega1B*bigomega2B))*(Y(jj,e)+d(2)));
      
      betatilde(e)=-(dot((g*(Qnhalf*e2)),e3))-(dot((Qnhalf*e2),qddot))...
          -((bigomega3dotB+(bigomega1B*bigomega2B))*(X(jj,e)+d(1)))+((Y(jj,e)+d(2))*((bigomega1B^2)+(bigomega3B^2)))+...
          ((bigomega1dotB-(bigomega2B*bigomega3B))*(d(3)));
  end

A{1,1}=eye(3,3);
for i=3:m+1
    A{1,i}=zeros(3);
end
b{1,1}=[1+((deltaT/(2*Dy))*Vfirst(jj-1,1)) 0 0;0 0 0;0 0 0]*[EH(jj,1);Ufirst(jj,1);Vfirst(jj,1)];

for ii=2:m
    if ii>2
       for i=1:ii-2
           A{ii,i}=zeros(3);
       end
    end

    A{ii,ii}=Nhalf;
    for i=ii+2:m+1
        A{ii,i}=zeros(3);
    end
    b{ii,1}=([1+((deltaT/(2*Dy))*Vfirst(jj-1,ii)) 0 0;2*bigomega2B 1 0;0 0 0]*[EH(jj,ii);Ufirst(jj,ii);Vfirst(jj,ii)])...
        +((deltaT/2)*[0;betahat(ii);0]);    
end

    for i=1:m-1
        A{m+1,i}=zeros(3);
    end
    A{m+1,m+1}=eye(3,3);
    b{m+1,1}=[1+((deltaT/(2*Dy))*Vfirst(jj-1,m+1)) 0 0;0 0 0;0 0 0]*[EH(jj,m+1);Ufirst(jj,m+1);Vfirst(jj,m+1)];

  
    id=1;
    qpp=1;
while qpp>1E-8
       
A{1,2}=(deltaT/(2*Dx))*[0 hstar(jj,1) 0;0 0 0;0 0 0];

for ii=2:m
    hatalphanstar=alpha(ii)-(((bigomega1B^2)+(bigomega2B^2))*hstar(jj,ii));

    A{ii,ii-1}=-(deltaT/(4*Dx))*[ustar(ii) hstar(jj,ii) 0;hatalphanstar ustar(ii) 0;0 0 0];
              
    A{ii,ii+1}=-A{ii,ii-1};
end

A{m+1,m}=-(deltaT/(2*Dx))*[0 hstar(jj,m+1) 0;0 0 0;0 0 0];
   
AA=cell2mat(A);
bb=cell2mat(b);

  %x=triblocksolve(AA,bb,2);
   x=AA\bb;
hhalf=x(1:3:end);
uhalf=x(2:3:end);
vhalf=x(3:3:end);

 for i=1:m+1
    Uhalf(jj,i)=uhalf(i);
    Vhalf(jj,i)=vhalf(i);
    Hhalf(jj,i)=hhalf(i);
 end

qpp=max(abs(Hhalf(jj,:)-hstar(jj,:))+abs(Uhalf(jj,:)-ustar)+abs(Vhalf(jj,:)-vstar));
id=id+1;

ustar=Uhalf(jj,:);
vstar=Vhalf(jj,:);
hstar(jj,:)=Hhalf(jj,:);

end
    
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

tT=tT+(deltaT/2);


    theta=epsilon1*sin(omega1*tT);
    bigomega1=epsilon1*omega1*cos(omega1*tT);
    bigomega1dot=-epsilon1*(omega1^2)*sin(omega1*tT);

    %%%%%%% change of sin to cos.
%     theta=epsilon1*cos(omega1*tT);
%     bigomega1=-epsilon1*omega1*sin(omega1*tT);
%     bigomega1dot=-epsilon1*(omega1^2)*cos(omega1*tT);

    phi=epsilon2*sin(omega2*tT);
    bigomega2=epsilon2*omega2*cos(omega2*tT);
    bigomega2dot=-epsilon2*(omega2^2)*sin(omega2*tT);

%     phi=-epsilon2*sin(omega2*tT);
%     bigomega2=-epsilon2*omega2*cos(omega2*tT);
%     bigomega2dot=epsilon2*(omega2^2)*sin(omega2*tT);

    gama=epsilon3*sin(omega3*tT);
    bigomega3=epsilon3*omega3*cos(omega3*tT);
    bigomega3dot=-epsilon3*(omega3^2)*sin(omega3*tT);
   
    sur=epsilon4*sin(omega4*tT);
    surdot=epsilon4*omega4*cos(omega4*tT);
    surddot=-epsilon4*(omega4^2)*sin(omega4*tT);
    
%     sur=epsilon4*cos(omega4*tT);
%     surdot=-epsilon4*omega4*sin(omega4*tT);
%     surddot=-epsilon4*(omega4^2)*cos(omega4*tT);

    swa=epsilon5*sin(omega5*tT);
    swadot=epsilon5*omega5*cos(omega5*tT);
    swaddot=-epsilon5*(omega5^2)*sin(omega5*tT);
    
%     swa=epsilon5*cos(omega5*tT);
%     swadot=-epsilon5*omega5*sin(omega5*tT);
%     swaddot=-epsilon5*(omega5^2)*cos(omega5*tT);
   
    hea=epsilon6*sin(omega6*tT);
    headot=epsilon6*omega6*cos(omega6*tT);
    headdot=-epsilon6*(omega6^2)*sin(omega6*tT);

% % Angular velocity vector for the coupled yaw-pitch-roll motion based on
% % the Euler angles.

%     omegahat1=bigomega1-(bigomega3*sin(phi));
%     omegahat2=(bigomega3*cos(phi)*sin(theta))+(bigomega2*cos(theta));
%     omegahat3=(bigomega3*cos(phi)*cos(theta))-(bigomega2*sin(theta));
%     
%     bigomega1B=omegahat1;
%     bigomega2B=omegahat2;
%     bigomega3B=omegahat3;
%     
%     bigomega1dotB=bigomega1dot-(bigomega3dot*sin(phi))-(bigomega2*bigomega3*cos(phi));
%     bigomega2dotB=(bigomega3dot*cos(phi)*sin(theta))+(bigomega3*((-bigomega2*sin(phi)*sin(theta))+(bigomega1*cos(theta)*cos(phi))))...
%                   +(bigomega2dot*cos(theta))-(bigomega1*bigomega2*sin(theta));
%     bigomega3dotB=(bigomega3dot*cos(phi)*cos(theta))+(bigomega3*((-bigomega2*sin(phi)*cos(theta))-(bigomega1*sin(theta)*cos(phi))))...
%                   -(bigomega2dot*sin(theta))-(bigomega1*bigomega2*cos(theta));
%               
%     Qnsec=[cos(phi)*cos(gama) (sin(theta)*sin(phi)*cos(gama))-(cos(theta)*sin(gama)) (cos(theta)*sin(phi)*cos(gama))+(sin(theta)*sin(gama));...
%             cos(phi)*sin(gama) (sin(theta)*sin(phi)*sin(gama))+(cos(theta)*cos(gama)) (cos(theta)*sin(phi)*sin(gama))-(sin(theta)*cos(gama));...
%             -sin(phi) sin(theta)*cos(phi) cos(theta)*cos(phi)];              
    
% % Pure yaw motion:
% bigomega1B=0;
% bigomega2B=0;
% bigomega3B=bigomega3;
% 
% bigomega1dotB=0;
% bigomega2dotB=0;
% bigomega3dotB=bigomega3dot;
% 
% Qnsec=[cos(gama) -sin(gama) 0;...
%         sin(gama) cos(gama) 0;...
%         0 0 1];


% % Pure pitch motion:
% bigomega1B=0;
% bigomega2B=bigomega2;
% bigomega3B=0;
% 
% bigomega1dotB=0;
% bigomega2dotB=bigomega2dot;
% bigomega3dotB=0;
% 
% Qnsec=[cos(phi) 0 -sin(phi);...
%         0 1 0;...
%         sin(phi) 0 cos(phi)];

    

% % Angular velocity vector for the coupled roll-pitch motion based on the
% % Euler angles.
    omegahat1=bigomega1*cos(phi);
    omegahat2=bigomega2;
    omegahat3=bigomega1*sin(phi);
   
    
    bigomega1B=omegahat1;
    bigomega2B=omegahat2;
    bigomega3B=omegahat3;
    
    bigomega1dotB=(bigomega1dot*cos(phi))-(bigomega1*bigomega2*sin(phi));
    bigomega2dotB=bigomega2dot;
    bigomega3dotB=(bigomega1dot*sin(phi))+(bigomega1*bigomega2*cos(phi));

    Qnsec=[cos(phi) 0 sin(phi);...
            sin(theta)*sin(phi) cos(theta) -sin(theta)*cos(phi);...
            -cos(theta)*sin(phi) sin(theta) cos(theta)*cos(phi)];
    
    qddot=[surddot;swaddot;headdot];
    
    Nbar=[1 0 0;0 1 0;(-2*bigomega1B)+((-1/2)*deltaT*((bigomega1dotB)-(bigomega2B*bigomega3B))) 0 1];

ii=1;

ustar=Uhalf(:,ii);
vstar=Vhalf(:,ii);
hstar=Hhalf;

A=cell(n+1);
b=cell(n+1,1);

  for e=1:n+1
      alpha(e)=(dot((g*(Qnsec*e3)),e3))+(dot((Qnsec*e3),qddot))-(((bigomega1B*bigomega2B)-bigomega2dotB)*(X(e,ii)+d(1)))...
          +((Y(e,ii)+d(2))*((bigomega1dotB+(bigomega2B*bigomega3B)))-...
          ((bigomega1B^2)+(bigomega2B^2))*(d(3)));
      
      betahat(e)=-(dot((g*(Qnsec*e1)),e3))-(dot((Qnsec*e1),qddot))+((X(e,ii)+d(1))*((bigomega2B^2)...
          +(bigomega3B^2)))-((bigomega2dotB+(bigomega1B*bigomega3B))*(d(3)))+...
          ((bigomega3dotB-(bigomega1B*bigomega2B))*(Y(e,ii)+d(2)));
      
      betatilde(e)=-(dot((g*(Qnsec*e2)),e3))-(dot((Qnsec*e2),qddot))...
          -((bigomega3dotB+(bigomega1B*bigomega2B))*(X(e,ii)+d(1)))+((Y(e,ii)+d(2))*((bigomega1B^2)+(bigomega3B^2)))+...
          ((bigomega1dotB-(bigomega2B*bigomega3B))*(d(3)));
  end

A{1,1}=eye(3,3);
for j=3:n+1
    A{1,j}=zeros(3);
end
b{1,1}=[1-((deltaT/(2*Dx))*Uhalf(1,ii+1)) 0 0;0 0 0;0 0 0]*[Hhalf(1,ii);Uhalf(1,ii);Vhalf(1,ii)];

for jj=2:n
    if jj>2
       for j=1:jj-2
           A{jj,j}=zeros(3);
       end
    end

    A{jj,jj}=Nbar;
    
    for j=jj+2:n+1
        A{jj,j}=zeros(3);
    end
    b{jj,1}=([1-((deltaT/(2*Dx))*Uhalf(jj,ii+1)) 0 0;0 0 0;-2*bigomega1B 0 1]*[Hhalf(jj,ii);Uhalf(jj,ii);Vhalf(jj,ii)])...
        +((deltaT/2)*[0;0;betatilde(jj)]);    
    
    for j=1:n-1
        A{n+1,j}=zeros(3);
    end
end

    A{n+1,n+1}=eye(3,3);
    b{n+1,1}=[1-((deltaT/(2*Dx))*Uhalf(n+1,ii+1)) 0 0;0 0 0;0 0 0]*[Hhalf(n+1,ii);Uhalf(n+1,ii);Vhalf(n+1,ii)];

    id=1;
    qpp=1;

while qpp>1E-8
       

A{1,2}=(deltaT/(2*Dy))*[0 0 hstar(1,ii);0 0 0;0 0 0];

for jj=2:n
    hatalphanstar=alpha(jj)-(((bigomega1B^2)+(bigomega2B^2))*hstar(jj,ii));
    
    A{jj,jj-1}=-(deltaT/(4*Dy))*[vstar(jj) 0 hstar(jj,ii);0 0 0;hatalphanstar 0 vstar(jj)];
                      
    A{jj,jj+1}=-A{jj,jj-1};
    
end

A{n+1,n}=-(deltaT/(2*Dy))*[0 0 hstar(n+1,ii);0 0 0;0 0 0];

AA=cell2mat(A);
bb=cell2mat(b);

  %x=triblocksolve(AA,bb,2);
   x=AA\bb;
hsec=x(1:3:end);
usec=x(2:3:end);
vsec=x(3:3:end);

for j=1:n+1
    Usec(j,ii)=usec(j);
    Vsec(j,ii)=vsec(j);
    Hsec(j,ii)=hsec(j);
end

qpp=max(abs(Hsec(:,ii)-hstar(:,ii))+abs(Usec(:,ii)-ustar)+abs(Vsec(:,ii)-vstar));
id=id+1;

ustar=Usec(:,ii);
vstar=Vsec(:,ii);
hstar(:,ii)=Hsec(:,ii);

end

% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 
Pnsec=[1 0 0;(2*bigomega2B)+((1/2)*deltaT*(bigomega2dotB+(bigomega1B*bigomega3B))) 1 0;...
       0 0 1];

Lnsec=[1 0 0;2*bigomega2B 1 0;0 0 0];

Bnsec=[1 0 0;(2*bigomega2B)+((1/2)*deltaT*(bigomega2dotB+(bigomega1B*bigomega3B))) 1 -deltaT*bigomega3B;...
        (-2*bigomega1B)-((1/2)*deltaT*(bigomega1dotB-(bigomega2B*bigomega3B))) deltaT*bigomega3B 1];

Dnsec=[1 0 0;2*bigomega2B 1 0;-2*bigomega1B 0 1];


    for ii=2:m
        
 A=cell(n+1);
 b=cell(n+1,1);
    
ustar=Uhalf(:,ii);
vstar=Vhalf(:,ii);


  for e=1:n+1
      alpha(e)=(dot((g*(Qnsec*e3)),e3))+(dot((Qnsec*e3),qddot))-(((bigomega1B*bigomega2B)-bigomega2dotB)*(X(e,ii)+d(1)))...
          +((Y(e,ii)+d(2))*((bigomega1dotB+(bigomega2B*bigomega3B)))-...
          ((bigomega1B^2)+(bigomega2B^2))*(d(3)));
      
      betahat(e)=-(dot((g*(Qnsec*e1)),e3))-(dot((Qnsec*e1),qddot))+((X(e,ii)+d(1))*((bigomega2B^2)...
          +(bigomega3B^2)))-((bigomega2dotB+(bigomega1B*bigomega3B))*(d(3)))+...
          ((bigomega3dotB-(bigomega1B*bigomega2B))*(Y(e,ii)+d(2)));
      
      betatilde(e)=-(dot((g*(Qnsec*e2)),e3))-(dot((Qnsec*e2),qddot))...
          -((bigomega3dotB+(bigomega1B*bigomega2B))*(X(e,ii)+d(1)))+((Y(e,ii)+d(2))*((bigomega1B^2)+(bigomega3B^2)))+...
          ((bigomega1dotB-(bigomega2B*bigomega3B))*(d(3)));
  end
  
hatalphansec=alpha(1)-(((bigomega1B^2)+(bigomega2B^2))*Hhalf(1,ii));    
A{1,1}=Pnsec;
for j=3:n+1
    A{1,j}=zeros(3);
end
Kjjiinsec=(deltaT/(4*Dx))*[Uhalf(1,ii) Hhalf(1,ii) 0;hatalphansec Uhalf(1,ii) 0;0 0 0];
b{1,1}=(Kjjiinsec*[Hhalf(1,ii-1);Uhalf(1,ii-1);Vhalf(1,ii-1)])+(Lnsec*[Hhalf(1,ii);Uhalf(1,ii);Vhalf(1,ii)])+...
       (-Kjjiinsec*[Hhalf(1,ii+1);Uhalf(1,ii+1);Vhalf(1,ii+1)])+((deltaT/2)*[0;betahat(1);0]);
   
   
for jj=2:n
    
    if jj>2
       for j=1:jj-2
           A{jj,j}=zeros(3);
       end
    end
    
    A{jj,jj}=Bnsec;
    for j=jj+2:n+1
        A{jj,j}=zeros(3);
    end
    
    tildeonealphansec=alpha(jj)+(2*bigomega1B*Vhalf(jj,ii))-(((bigomega1B^2)+(bigomega2B^2))*Hhalf(jj,ii));
        
    Ajjiinsec=(deltaT/(4*Dx))*[Uhalf(jj,ii) Hhalf(jj,ii) 0;tildeonealphansec Uhalf(jj,ii) 0;-2*bigomega1B*Uhalf(jj,ii) 0 Uhalf(jj,ii)];
    
    betansec=(deltaT/2)*[0;betahat(jj);betatilde(jj)];

    b{jj,1}=(Ajjiinsec*[Hhalf(jj,ii-1);Uhalf(jj,ii-1);Vhalf(jj,ii-1)])+(Dnsec*[Hhalf(jj,ii);Uhalf(jj,ii);Vhalf(jj,ii)])+...
            (-Ajjiinsec*[Hhalf(jj,ii+1);Uhalf(jj,ii+1);Vhalf(jj,ii+1)])+betansec;
        
for j=1:n-1
    A{n+1,j}=zeros(3);
end

end

hatalphansec=alpha(n+1)-(((bigomega1B^2)+(bigomega2B^2))*Hhalf(n+1,ii));    
A{n+1,n+1}=Pnsec;
Kjjiinsec=(deltaT/(4*Dx))*[Uhalf(n+1,ii) Hhalf(n+1,ii) 0;hatalphansec Uhalf(n+1,ii) 0;0 0 0];
b{n+1,1}=(Kjjiinsec*[Hhalf(n+1,ii-1);Uhalf(n+1,ii-1);Vhalf(n+1,ii-1)])+(Lnsec*[Hhalf(n+1,ii);Uhalf(n+1,ii);Vhalf(n+1,ii)])+...
       (-Kjjiinsec*[Hhalf(n+1,ii+1);Uhalf(n+1,ii+1);Vhalf(n+1,ii+1)])+((deltaT/2)*[0;betahat(n+1);0]);         

    id=1;
    qpp=1;
while qpp>1E-8
    
A{1,2}=(deltaT/(2*Dy))*[0 0 hstar(1,ii);0 0 0;0 0 0];
    
for jj=2:n
    
    tildetwoalphansecstar=alpha(jj)-(2*bigomega2B*ustar(jj))-(((bigomega1B^2)+(bigomega2B^2))*hstar(jj,ii));

    A{jj,jj-1}=-(deltaT/(4*Dy))*[vstar(jj) 0 hstar(jj,ii);2*bigomega2B*vstar(jj) vstar(jj) 0;tildetwoalphansecstar 0 vstar(jj)];
                                  
    A{jj,jj+1}=-A{jj,jj-1};
    
end

A{n+1,n}=(-deltaT/(2*Dy))*[0 0 hstar(n+1,ii);0 0 0;0 0 0];
      
AA=cell2mat(A);
bb=cell2mat(b);

  %x=triblocksolve(AA,bb,2);
   x=AA\bb;
   
hsec=x(1:3:end);
usec=x(2:3:end);
vsec=x(3:3:end);

for j=1:n+1
    Usec(j,ii)=usec(j);
    Vsec(j,ii)=vsec(j);
    Hsec(j,ii)=hsec(j);
end

qpp=max(abs(Hsec(:,ii)-hstar(:,ii))+abs(Usec(:,ii)-ustar)+abs(Vsec(:,ii)-vstar));
id=id+1;

ustar=Usec(:,ii);
vstar=Vsec(:,ii);
hstar(:,ii)=Hsec(:,ii);
        
end
    end
    
% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %
ii=m+1;

ustar=Uhalf(:,ii);
vstar=Vhalf(:,ii);
hstar=Hhalf;

A=cell(n+1);
b=cell(n+1,1);

  for e=1:n+1
      alpha(e)=(dot((g*(Qnsec*e3)),e3))+(dot((Qnsec*e3),qddot))-(((bigomega1B*bigomega2B)-bigomega2dotB)*(X(e,ii)+d(1)))...
          +((Y(e,ii)+d(2))*((bigomega1dotB+(bigomega2B*bigomega3B)))-...
          ((bigomega1B^2)+(bigomega2B^2))*(d(3)));
      
      betahat(e)=-(dot((g*(Qnsec*e1)),e3))-(dot((Qnsec*e1),qddot))+((X(e,ii)+d(1))*((bigomega2B^2)+(bigomega3B^2)))...
          -((bigomega2dotB+(bigomega1B*bigomega3B))*(d(3)))+...
          ((bigomega3dotB-(bigomega1B*bigomega2B))*(Y(e,ii)+d(2)));
      
      betatilde(e)=-(dot((g*(Qnsec*e2)),e3))-(dot((Qnsec*e2),qddot))...
          -((bigomega3dotB+(bigomega1B*bigomega2B))*(X(e,ii)+d(1)))+((Y(e,ii)+d(2))*((bigomega1B^2)+(bigomega3B^2)))+...
          ((bigomega1dotB-(bigomega2B*bigomega3B))*(d(3)));
  end

A{1,1}=eye(3,3);
for j=3:n+1
    A{1,j}=zeros(3);
end
b{1,1}=[1+((deltaT/(2*Dx))*Uhalf(1,ii-1)) 0 0;0 0 0;0 0 0]*[Hhalf(1,ii);Uhalf(1,ii);Vhalf(1,ii)];

for jj=2:n
    if jj>2
       for j=1:jj-2
           A{jj,j}=zeros(3);
       end
    end

    A{jj,jj}=Nbar;
    for j=jj+2:n+1
        A{jj,j}=zeros(3);
    end
    b{jj,1}=([1+((deltaT/(2*Dx))*Uhalf(jj,ii-1)) 0 0;0 0 0;-2*bigomega1B 0 1]*[Hhalf(jj,ii);Uhalf(jj,ii);Vhalf(jj,ii)])...
        +((deltaT/2)*[0;0;betatilde(jj)]);
    
end
    A{n+1,n+1}=eye(3,3);
    b{n+1,1}=[1+((deltaT/(2*Dx))*Uhalf(n+1,ii-1)) 0 0;0 0 0;0 0 0]*[Hhalf(n+1,ii);Uhalf(n+1,ii);Vhalf(n+1,ii)];

    id=1;
    qpp=1;

while qpp>1E-8
       
A{1,2}=(deltaT/(2*Dy))*[0 0 hstar(1,ii);0 0 0;0 0 0];

for jj=2:n
    hatalphanstar=alpha(jj)-(((bigomega1B^2)+(bigomega2B^2))*hstar(jj,ii));

    A{jj,jj-1}=-(deltaT/(4*Dy))*[vstar(jj) 0 hstar(jj,ii);0 0 0;hatalphanstar 0 vstar(jj)];
          
    A{jj,jj+1}=-A{jj,jj-1};
    
end

for j=1:n-1
    A{n+1,j}=zeros(3);
end

A{n+1,n}=-(deltaT/(2*Dy))*[0 0 hstar(n+1,ii);0 0 0;0 0 0];

AA=cell2mat(A);
bb=cell2mat(b);

  %x=triblocksolve(AA,bb,2);
   x=AA\bb;
hsec=x(1:3:end);
usec=x(2:3:end);
vsec=x(3:3:end);

for j=1:n+1
    Usec(j,ii)=usec(j);
    Vsec(j,ii)=vsec(j);
    Hsec(j,ii)=hsec(j);
end

qpp=max(abs(Hsec(:,ii)-hstar(:,ii))+abs(Usec(:,ii)-ustar)+abs(Vsec(:,ii)-vstar));
id=id+1;

ustar=Usec(:,ii);
vstar=Vsec(:,ii);
hstar(:,ii)=Hsec(:,ii);

end

% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

Ufirst=Usec;
Vfirst=Vsec;
Qn=Qnsec;

EH=Hsec;
hstar=Hsec;

% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

al=(Qnsec*([-R/2;-T/2;d(3)]))+([sur;swa;hea]);
ar=(Qnsec*([R/2;-T/2;d(3)]))+([sur;swa;hea]);
bl=(Qnsec*([-R/2;T/2;d(3)]))+([sur;swa;hea]);
br=(Qnsec*([R/2;T/2;d(3)]))+([sur;swa;hea]);

cl=(Qnsec*([-R/2;-T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
cr=(Qnsec*([R/2;-T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
dl=(Qnsec*([-R/2;T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
dr=(Qnsec*([R/2;T/2;d(3)+2.5*h0]))+([sur;swa;hea]);


ex1=[al(1);ar(1)];
vay1=[al(2);ar(2)];
zed1=[al(3);ar(3)];

ex2=[al(1);bl(1)];
vay2=[al(2);bl(2)];
zed2=[al(3);bl(3)];

ex3=[ar(1);br(1)];
vay3=[ar(2);br(2)];
zed3=[ar(3);br(3)];

ex4=[bl(1);br(1)];
vay4=[bl(2);br(2)];
zed4=[bl(3);br(3)];

ex5=[cl(1);cr(1)];
vay5=[cl(2);cr(2)];
zed5=[cl(3);cr(3)];

ex6=[cl(1);dl(1)];
vay6=[cl(2);dl(2)];
zed6=[cl(3);dl(3)];

ex7=[cr(1);dr(1)];
vay7=[cr(2);dr(2)];
zed7=[cr(3);dr(3)];

ex8=[dl(1);dr(1)];
vay8=[dl(2);dr(2)];
zed8=[dl(3);dr(3)];

ex9=[al(1);cl(1)];
vay9=[al(2);cl(2)];
zed9=[al(3);cl(3)];

ex10=[ar(1);cr(1)];
vay10=[ar(2);cr(2)];
zed10=[ar(3);cr(3)];

ex11=[bl(1);dl(1)];
vay11=[bl(2);dl(2)];
zed11=[bl(3);dl(3)];

ex12=[br(1);dr(1)];
vay12=[br(2);dr(2)];
zed12=[br(3);dr(3)];


ex1f=[alf(1);arf(1)];
vay1f=[alf(2);arf(2)];
zed1f=[alf(3);arf(3)];

ex2f=[alf(1);blf(1)];
vay2f=[alf(2);blf(2)];
zed2f=[alf(3);blf(3)];

ex3f=[alf(1);clf(1)];
vay3f=[alf(2);clf(2)];
zed3f=[alf(3);clf(3)];

for ip=1:m+1
    for jp=1:n+1
        pos{jp,ip}=(Qnsec*([X(jp,ip);Y(jp,ip);EH(jp,ip)]+di))+[sur;swa;hea];
    end
end
posp=cell2mat(pos);
for jp=1:n+1
    Xp(jp,:)=posp(3*(jp-1)+1,:);
    Yp(jp,:)=posp(3*(jp-1)+2,:);
    EHp(jp,:)=posp(3*(jp-1)+3,:);
end


% % box on
% surf(Xp,Yp,EHp);shading interp
% hold on
% xlim([min(Xp(1,:)) max(Xp(1,:))]);ylim([min(Yp(:,1)) max(Yp(:,1))]);zlim([0 2.5*h0])
% axis off
% plot3(ex1f,vay1f,zed1f,'b','LineWidth',2)
% plot3(ex2f,vay2f,zed2f,'b','LineWidth',2)
% plot3(ex3f,vay3f,zed3f,'b','LineWidth',2)
% 
% plot3(ex1,vay1,zed1,'k','LineWidth',2.5)
% plot3(ex2,vay2,zed2,'k','LineWidth',2.5)
% plot3(ex3,vay3,zed3,'k','LineWidth',2.5)
% plot3(ex4,vay4,zed4,'k','LineWidth',2.5)
% plot3(ex5,vay5,zed5,'k','LineWidth',2.5)
% plot3(ex6,vay6,zed6,'k','LineWidth',2.5)
% plot3(ex7,vay7,zed7,'k','LineWidth',2.5)
% plot3(ex8,vay8,zed8,'k','LineWidth',2.5)
% plot3(ex9,vay9,zed9,'k','LineWidth',2.5)
% plot3(ex10,vay10,zed10,'k','LineWidth',2.5)
% plot3(ex11,vay11,zed11,'k','LineWidth',2.5)
% plot3(ex12,vay12,zed12,'k','LineWidth',2.5)
% set(gcf,'Color',[1,1,1])
% set(gca,'CameraPosition',[-0.6 -0.5 0.5])
% 
% 
% hold off
% 
% Image(num+1)=getframe;

% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 

if num==50
    figure
    subplot(2,2,1);surf(X,Y,EH);shading interp
    xlim([0 R]);ylim([0 T]);zlim([0 2.5*h0])
    title('t=0.5 sec')
    set(gcf,'Color',[1,1,1])
    set(gca,'DataAspectRatio',[1 1 1])
    box on
    grid off
    EH50=EH;
    Usec50=Usec;
    Vsec50=Vsec;
    
EHp50=EHp;
Xp50=Xp;
Yp50=Yp;
al50=(Qnsec*([-R/2;-T/2;d(3)]))+([sur;swa;hea]);
ar50=(Qnsec*([R/2;-T/2;d(3)]))+([sur;swa;hea]);
bl50=(Qnsec*([-R/2;T/2;d(3)]))+([sur;swa;hea]);
br50=(Qnsec*([R/2;T/2;d(3)]))+([sur;swa;hea]);

cl50=(Qnsec*([-R/2;-T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
cr50=(Qnsec*([R/2;-T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
dl50=(Qnsec*([-R/2;T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
dr50=(Qnsec*([R/2;T/2;d(3)+2.5*h0]))+([sur;swa;hea]);

end

if num==140
    subplot(2,2,3);surf(X,Y,EH);shading interp
    xlim([0 R]);ylim([0 T]);zlim([0 2.5*h0])
    title('t=1.4 sec')
    set(gcf,'Color',[1,1,1])
    set(gca,'DataAspectRatio',[1 1 1])
    box on
    grid off
    EH140=EH;
    Usec140=Usec;
    Vsec140=Vsec;
    
EHp140=EHp;
Xp140=Xp;
Yp140=Yp;
al140=(Qnsec*([-R/2;-T/2;d(3)]))+([sur;swa;hea]);
ar140=(Qnsec*([R/2;-T/2;d(3)]))+([sur;swa;hea]);
bl140=(Qnsec*([-R/2;T/2;d(3)]))+([sur;swa;hea]);
br140=(Qnsec*([R/2;T/2;d(3)]))+([sur;swa;hea]);

cl140=(Qnsec*([-R/2;-T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
cr140=(Qnsec*([R/2;-T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
dl140=(Qnsec*([-R/2;T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
dr140=(Qnsec*([R/2;T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
    
end

if num==260
    subplot(2,2,2);surf(X,Y,EH);shading interp
    xlim([0 R]);ylim([0 T]);zlim([0 2.5*h0])
    title('t=2.6 sec')    
    set(gcf,'Color',[1,1,1])
    set(gca,'DataAspectRatio',[1 1 1])
    box on
    grid off
    EH260=EH;
    Usec260=Usec;
    Vsec260=Vsec;
    
EHp260=EHp;
Xp260=Xp;
Yp260=Yp;
al260=(Qnsec*([-R/2;-T/2;d(3)]))+([sur;swa;hea]);
ar260=(Qnsec*([R/2;-T/2;d(3)]))+([sur;swa;hea]);
bl260=(Qnsec*([-R/2;T/2;d(3)]))+([sur;swa;hea]);
br260=(Qnsec*([R/2;T/2;d(3)]))+([sur;swa;hea]);

cl260=(Qnsec*([-R/2;-T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
cr260=(Qnsec*([R/2;-T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
dl260=(Qnsec*([-R/2;T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
dr260=(Qnsec*([R/2;T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
    
end

if num==350
    subplot(2,2,4);surf(X,Y,EH);shading interp
    xlim([0 R]);ylim([0 T]);zlim([0 2.5*h0])
    title('t=3.5 sec')    
    set(gcf,'Color',[1,1,1])
    set(gca,'DataAspectRatio',[1 1 1])
    box on
    grid off
    EH350=EH;
    Usec350=Usec;
    Vsec350=Vsec;
    
EHp350=EHp;
Xp350=Xp;
Yp350=Yp;
al350=(Qnsec*([-R/2;-T/2;d(3)]))+([sur;swa;hea]);
ar350=(Qnsec*([R/2;-T/2;d(3)]))+([sur;swa;hea]);
bl350=(Qnsec*([-R/2;T/2;d(3)]))+([sur;swa;hea]);
br350=(Qnsec*([R/2;T/2;d(3)]))+([sur;swa;hea]);

cl350=(Qnsec*([-R/2;-T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
cr350=(Qnsec*([R/2;-T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
dl350=(Qnsec*([-R/2;T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
dr350=(Qnsec*([R/2;T/2;d(3)+2.5*h0]))+([sur;swa;hea]);
     
end


% % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 


num


p1(num+1)=EH(1,1);
p2(num+1)=EH(1,end);
p3(num+1)=EH(end,1);
p4(num+1)=EH(end,end);
p5(num+1)=EH(m/2,1);
p6(num+1)=EH(m/2,end);
p7(num+1)=EH(1,n/2);
p8(num+1)=EH(end,n/2);


end


toc

Tcpu=cputime;

ElapsedTime=toc;
