function[lattice]=wingrotation2(wingno,geo,lattice,Raxle,hinge_pos,alpha,symmetry)
%%%
% This function is a part of TORNADO, the vortex lattice method.
%
% wingrotation2 rotates the lattice of a wing according to user input.
% This might be used if the incidence of an entire wing changes, as would
% be the case with tailerons, all-moving canards, and other suchlike.
%
% WINGROTATION2(WINGNO,GEO,LATTICE,RAXLE,HINGE_POS,ALPHA) rotates wing
% number WINGNO, ALPHA radians around the rotaion axis RAXLE, with the
% origin in HINGE_POS.
%
% GEO and LATTICE are the standard Tornado geometry data.
%
% Output is the new, rotated lattice to be used en the tornado solver.
%
%Example:
% 
%   [lattice,ref]=fLattice_setup(geo,state);
%   geometryplot(lattice,geo,ref);
% 
%  Calls:
%       None.
% 
%  Author: Tomas Melin <dr.tomas.melin@gmail.com>
%  Keywords: Tornado text based user interface
% 
%  Revision History:
%    Bristol,    2008 12 07:  Function created. T.M.
%    Spånga, 2021 08 05: Modded to cater for assymetric canard deflection
%   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


    
    
    lemma1=((geo.nx+geo.fnx).*geo.ny);      %How many panels on SB side
    lemma2=(geo.symetric+1)';               %is wing symetric? 
    

    [a b]=size(lemma1);
    for i=1:b
        npan_m1(:,i)=lemma1(:,i).*lemma2;   %Total number of panels to work with
    end

    npan=sum(npan_m1,2);                    %Total number of panels to work with

    if symmetry
        alpha_v=alpha*ones(npan(wingno),1);     %deflection angle
    else
        alpha_v=alpha*[ones(npan(wingno)/2,1); -ones(npan(wingno)/2,1)]; %deflection angle)
    end
    
    if geo.symetric(wingno)
        %if symmetry
        %    hinge_pos_v=ones(npan(wingno),3).*hinge_pos;
        %else
            hinge_pos_v=[ones(npan(wingno)/2,3).*hinge_pos; ones(npan(wingno)/2,3).*(hinge_pos.*[1 -1 1])];
        %end
    end
    

    if wingno==1;                           %Which index to sart and stop with
        startindex=1;
        endindex=npan(1);
    else
        startindex_m1=cumsum(npan);
        startindex=startindex_m1(wingno-1)+1;
        endindex=startindex_m1(wingno);
    end


    %%Move to rotation centre
    l.V(:,:,1)=lattice.VORTEX(startindex:endindex,:,1)-hinge_pos_v(:,1);
    l.V(:,:,2)=lattice.VORTEX(startindex:endindex,:,2)-hinge_pos_v(:,2);
    l.V(:,:,3)=lattice.VORTEX(startindex:endindex,:,3)-hinge_pos_v(:,3);

    l.C(:,1)=lattice.COLLOC(startindex:endindex,1)-hinge_pos_v(:,1);
    l.C(:,2)=lattice.COLLOC(startindex:endindex,2)-hinge_pos_v(:,2);
    l.C(:,3)=lattice.COLLOC(startindex:endindex,3)-hinge_pos_v(:,3);

    l.XYZ(:,:,1)=lattice.XYZ(startindex:endindex,:,1)-hinge_pos_v(:,1);
    l.XYZ(:,:,2)=lattice.XYZ(startindex:endindex,:,2)-hinge_pos_v(:,2);
    l.XYZ(:,:,3)=lattice.XYZ(startindex:endindex,:,3)-hinge_pos_v(:,3);

    l.N(:,:,:)=lattice.N(startindex:endindex,:,:);


    [ai bi ci]=size(l.V);

    l2.C=trot4(Raxle,l.C,alpha_v)';   %Rotating collocationpoints
    l2.N=trot4(Raxle,l.N,alpha_v)';   %Rotating Normals    

    for i=2:(bi-1)
        for j=1:ai;
            A(j,:)=l.V(j,i,:);
        end


        B=trot4(Raxle,A,alpha_v)';
        l2.V(:,i,:)=B;
    end

        l2.V(:,1,:)=l.V(:,1,:);
        l2.V(:,bi,:)=l.V(:,bi,:);


    for i=1:5
        for j=1:ai;
            A(j,:)=l.XYZ(j,i,:);
        end

        B=trot4(Raxle,A,alpha_v)';
        l2.XYZ(:,i,:)=B;
    end

    %%Move back from rotation centre
    l2.V(:,:,1)=l2.V(:,:,1)+hinge_pos_v(:,1);
    l2.V(:,:,2)=l2.V(:,:,2)+hinge_pos_v(:,2);
    l2.V(:,:,3)=l2.V(:,:,3)+hinge_pos_v(:,3);

    l2.C(:,1)=l2.C(:,1)+hinge_pos_v(:,1);
    l2.C(:,2)=l2.C(:,2)+hinge_pos_v(:,2);
    l2.C(:,3)=l2.C(:,3)+hinge_pos_v(:,3);

    l2.XYZ(:,:,1)=l2.XYZ(:,:,1)+hinge_pos_v(:,1);
    l2.XYZ(:,:,2)=l2.XYZ(:,:,2)+hinge_pos_v(:,2);
    l2.XYZ(:,:,3)=l2.XYZ(:,:,3)+hinge_pos_v(:,3);


    lattice.VORTEX(startindex:endindex,:,:)=l2.V;
    lattice.COLLOC(startindex:endindex,:,:)=l2.C;
    lattice.XYZ(startindex:endindex,:,:)=l2.XYZ;
    lattice.N(startindex:endindex,:,:)=l2.N;

    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    
    

end%function wingrotation
function[p2]=trot4(hinge,p,alpha)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% TROT: Auxillary rotation function			
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% rotates point p around hinge alpha rads.%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ref: 	Råde, Westergren, BETA 4th ed,   
%			studentlitteratur, 1998			    	
%			pp:107-108							   	
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Author: 	Tomas Melin, KTH,Department of%
% 				aeronautics, Copyright 2000	
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Context:	Auxillary function for			
%				TORNADO.								
% Called by: setrudder, normals			
% Calls:		norm (MATLAB std fcn)			
%				sin			"						
%				cos			"						
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% HELP:		Hinge=vector around rotation  
%						takes place.				
%				p=point to be rotated			
%				alpha=radians of rotation		
%				3D-workspace						
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
a=hinge(1);
b=hinge(2);
c=hinge(3);

rho=sqrt(a^2+b^2);
r=sqrt(a^2+b^2+c^2);

if r==0
   cost=0
   sint=1;
else
   cost=c/r;
   sint=rho/r;
end

if rho==0
   cosf=0;
   sinf=1;
else
   cosf=a/rho;
	sinf=b/rho;
end   

for i=1:size(alpha)
    cosa=cos(alpha(i));
    sina=sin(alpha(i));

    RZF=[[cosf -sinf 0];[sinf cosf 0];[0 0 1]];
    RYT=[[cost 0 sint];[0 1 0];[-sint 0 cost]];
    RZA=[[cosa -sina 0];[sina cosa 0];[0 0 1]];
    RYMT=[[cost 0 -sint];[0 1 0];[sint 0 cost]];
    RZMF=[[cosf sinf 0];[-sinf cosf 0];[0 0 1]];

    P=RZF*RYT*RZA*RYMT*RZMF;
    p2(:,i)=P*p(i,:)';
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end%function wingrotation