% function [...] = mbc(...)  % later: convert this script into a function script
% MBC: Multi-Blade Coordinate Transformation for a turbine with 3-blade rotor
%
% Developed by Gunjit Bir, NREL (303-384-6953, gunjit_bir@nrel.gov)

% Last update: 08/30/2006
%----------------------------------------------------------------------------------------

% Objectives:
% 1. Given state-space matrices (A,B,Bd) and output matrices (C,D,Dd), defined partly in the
%    rotoating frame and partly in the fixed frame, transform these matrices to the fixed
%    coordinate frame using multi-blade coordinate transformation (MBC). The transformned
%    matrices are MBC_A, MBC_B, MBC_Bd, MBC_C, MBC_D and MBC_Dd.
%
% 2. Given second-order matrices (M,Dmp,K), control input matrix (F), wind input disturbance
%    matrix (Fd), displacement output matrix (Dc), and velocity output matrix (Vc), transform
%    these to the fixed coordinate frame using multi-blade coordinate transformation (MBC).
%    The transformed matrices are MBC_M, MBC_Dmp, MBC_K, MBC_F, MBC_Fd, MBC_Dc, and MBC_Vc.
%
% 3. Azimuth-average the MBC_A matrix and compute its eigenvalues and eigenvectors.  The
%    eigenvectors, referred to the fixed coordinates, are presented in both complex and
%    amplitude-phase forms. The eigenvalues, also referred to the fixed coordinates, are
%    presented in complex and damping-ratio/damped-frequency forms.

% ***WARNING: Do not copy or modify this code, or transfer algorithms (in original or modified form)
%    from this code into another code without permission from NREL (Gunjit Bir, 303-384-6953).
%
% ***Disclaimer: This code is still in the developmental stage and no guarantee is given
%    as to its proper functioning or accuracy of its results.

% ------------ INPUTS   (these will be accessed later via the fuction arguments) ---
% ndof       : total number of degrees of freedom
% NInputs    : total num of control inputs
% NumOuts    : total num of outputs
% ns         : number of states
% A,B,Bd     : matrices associated with the state-spece equation
% C,D,Dd     : matrices associated with the ouputs equation
% M,Dmp,K    : 2nd-order mass, stiffness, and damping/gyroscopic matrices
% F          : control input matrix (associated with 2nd-order equation)
% Fd         : wind input disturbance matrix (associated with 2nd-order equation)
% Dc, Vc     : displacement and velocity output matrices (associated with 2nd-order equation)
% azm        : vector of rotor azimuth positions wrt the the user-specified reference position [see AzimB1Up, FAST User's guide] (deg)
% omg        : rotor speed (rad/sec)
% OmgDot     : rotor acceleraton (rad/sec^2)

% RotTripletIndicesStates   : State triplets in rotating frame (matrix of size rotating_dof_types*3)
% RotTripletIndicesCntrlInpt: Control-input triplets in rotating frame (matrix of size rotating_control_input_types*3)
% RotTripletIndicesOutput   : Output triplets in rotating frame (matrix of size rotating_output_types*3)

% DescStates : description of states associated with input matrices (FAST-specific) %%

% --------------------- OUTPUTS ----------------------------------------------------
% MBC_A,MBC_B,MBC_Bd  : state-space matrices transformed to the fixed frame
% MBC_C,MBC_D, MBC_Dd : output matrices transformed to the fixed frame
% MBC_M,MBC_Dmp,MBC_K : second-order mass, damping/gyroscopic and stiffness matrices transformed to the fixed frame
% MBC_F               : control input matrix transformed to the fixed frame
% MBC_Fd              : wind input disturbance matrix transformed to the fixed frame
% MBC_Dc,MBC_Vc       : displacement and velocity output matrices (Vc) transformed to the fixed frame

% MBC_States          : description of mbc-transformed states (FAST-specific) %%

% -----------------------------------------------------------------------------------------
%**NOTE 1: All inputs and output matrices may not be present.  For example, user may supply or may be interested
%          in multi-blade coordinate transformation of A matrix only.  In this case, only MBC_A matrix along with
%          fixed-frame eigensolution will be genertaed as outputs.  The code checks for consistency and completness
%          of selected inputs and generates associated outputs.

%**NOTE 2: The code currently assumes that the following inputs are available in the Matlab worspace
%          (for example those generated by the FAST post-processor Eigenanalysis.m)

% AMat, BMat, BdMat, CMat, DMat, DdMat: 1st-order input matrices
% MassMat, DampMat, StffMat, FMat, FdMat, VelCMat, DspCMat: 2nd-order input matrices
% Omega     : Vector of rotor speeds at specified azimuths (rad/sec)
% OmegaDot  : Vector of rotor accelerations at specified azimuths (rad/sec2)
% NAzimStep : number of azimuth steps at which i/o matrices are specified/generated
% NActvDOF  : total number of degrees of freedom
% N         : number of states
% Azimuth   : vector of azimuth positions (in deg)
% -----------------------------------------------------------------------------------------
%% TBD: MBC flags/error messages / status messages / convert this script to function/

% error(nargchk(2, 3, nargin)) : implement later
%
% function [s, varargout] = mysize(x)
% msg = nargoutchk(1, 3, nargout);
% if isempty(msg)
%     nout = max(nargout, 1) - 1;
%     s = size(x);
%     for k = 1:nout, varargout(k) = {s(k)}; end
% else
%     disp(msg)      & or, error('msg') ??
% end --------------------------------------------------------------------------------------

format short g;

   ProgName = 'mbc3 (v1.00.00a-gbir, 15-Oct-2008)';

   fprintf( '\n  Running %s\n\n', ProgName );

% --- The following lines will be removed later ---
NAzSteps = NAzimStep;
ndof = NActvDOF;
ns = N;
azm = Azimuth;

new_seq_cont = 0;
new_seq_out = 0;

%---------- Multi-Blade-Coordinate transformation -------------------------------------------
id_exist = exist('RotTripletIndicesStates');
if(id_exist == 0)
 display('*** There are no rotating states. MBC transformation, therefore, cannot be performed.');
 return
end

[nm,nb] = size(RotTripletIndicesStates);
if(nb ~= 3)
 display('**ERROR: the number of colmn vectors in RotTripletIndicesStates must equal 3, the num of blades');
 return
end
if(nm*nb > ndof)
 display('**ERROR: the rotating dof exceeds the total num of dof');
 return
end
new_seq_dof = get_new_seq(RotTripletIndicesStates,ndof);
new_seq_states = [new_seq_dof  new_seq_dof+ndof];

mc=0; % number of rotating-frame control triplets
if (exist('RotTripletIndicesCntrlInpt'))
  [mc,nb] = size(RotTripletIndicesCntrlInpt);
  if(nb ~= 3)
   display('**ERROR: the number of colmn vectors in RotTripletIndicesCntrlInpt must equal 3, the num of blades');
   return
  end
  new_seq_cont = get_new_seq(RotTripletIndicesCntrlInpt,NInputs);
end

mo=0; % number of rotating-frame output triplets
if (exist('RotTripletIndicesOutput'))
  [mo,nb] = size(RotTripletIndicesOutput);
  if(nb ~= 3)
   display('**ERROR: the number of colmn vectors in RotTripletIndicesOutput must equal 3, the num of blades');
   return
  end
   new_seq_out = get_new_seq(RotTripletIndicesOutput,NumOuts);
end

ndfix = ndof-nm*nb; % fixed-frame dof
ncfix = NInputs-mc*nb; % fixed-frame controls
nofix = NumOuts-mo*nb; % fixed-frame outputs

if (exist('AMat'))
 MBC_AvgA = zeros(ns);
end

if ( size(Omega) ~= NAzSteps)
   display('**ERROR: the size of Omega vector must equal NAzSteps, the num of azimuth steps');
   return
end
if ( size(OmegaDot) ~= NAzSteps)
   display('**ERROR: the size of OmegaDot vector must equal NAzSteps, the num of azimuth steps');
   return
end

for iaz = 1:NAzSteps  % begin azimuth loop

% compute azimuth positions of blades:
  del_az = 2*pi/nb;
  az1 = azm(iaz)*pi/180.0;  % in radian
  az2 = az1+del_az;
  az3 = az2+del_az;

% get rotor speed and acceleration
  omg = Omega(iaz);
  omg2 = omg*omg;
  OmgDot = OmegaDot(iaz);

% compute transformation matrices
  c1 = cos(az1);
  c2 = cos(az2);
  c3 = cos(az3);
  s1 = sin(az1);
  s2 = sin(az2);
  s3 = sin(az3);

  cos_col = [c1;c2;c3];
  sin_col = [s1;s2;s3];

  tt = [ones(3,1), cos_col, sin_col];
  ett = 1.5*sqrt(3);

  cos231_col = [c2;c3;c1];
  cos312_col = [c3;c1;c2];
  sin231_col = [s2;s3;s1];
  sin312_col = [s3;s1;s2];

  tc1 = cos231_col.*sin312_col-sin231_col.*cos312_col;
  ttv = [tc1, (sin231_col-sin312_col), (cos312_col-cos231_col)]'/ett;
  tt2 = [zeros(3,1), -sin_col,  cos_col];
  tt3 = [zeros(3,1), -cos_col, -sin_col];

%---
  T1 = [eye(ndfix)];
  for ii = 1:nm
    T1 = blkdiag(T1, tt);
  end

  T1v = [eye(ndfix)];
  for ii = 1:nm
    T1v = blkdiag(T1v, ttv);
  end

  T2 = [zeros(ndfix)];
  for ii = 1:nm
    T2 = blkdiag(T2, tt2);
  end

  T3 = [zeros(ndfix)];
  for ii = 1:nm
    T3 = blkdiag(T3, tt3);
  end
%---
  T1c = [eye(ncfix)];
  for ii = 1:mc;
    T1c = blkdiag(T1c, tt);
  end

  T1o = [eye(nofix)];
  for ii = 1:mo
    T1o = blkdiag(T1o, tt);
  end

  T1ov = [eye(nofix)];
  for ii = 1:mo
    T1ov = blkdiag(T1ov, ttv);
  end

% mbc transformation of first-order matrices
%  if ( MBC_EqnsOrder == 1 ) % activate later

    if (exist('AMat'))
       xAMat = row_col_xtion(AMat(:,:,iaz), new_seq_states, 1, 0); %--
       xAMat = row_col_xtion(xAMat, new_seq_states, 2, 0); %--

       AK = xAMat(ndof+1:ns,1:ndof);
       AC = xAMat(ndof+1:ns,ndof+1:ns);
       MBC_A(:,:,iaz) = [zeros(ndof), eye(ndof);
            T1v*(AK*T1+omg*AC*T2-omg2*T3-OmgDot*T2), T1v*(AC*T1-2*omg*T2)];

       MBC_A(:,:,iaz) = row_col_xtion(MBC_A(:,:,iaz), new_seq_states, 2, 1); %--
       MBC_A(:,:,iaz) = row_col_xtion(MBC_A(:,:,iaz), new_seq_states, 1, 1); %--
       MBC_AvgA = MBC_AvgA + MBC_A(:,:,iaz);

       clear xAMat; %--
    end

    if (exist('BMat'))
       xBMat = row_col_xtion(BMat(:,:,iaz), new_seq_states, 1, 0); %--
       if (new_seq_cont ~= 0); xBMat = row_col_xtion(xBMat, new_seq_cont, 2, 0); end %--

       B1 = xBMat(1:ndof,:);
       B2 = xBMat(ndof+1:ns,:);
       MBC_B(:,:,iaz) = [T1v*B1;T1v*B2]*T1c;

       if (new_seq_cont ~= 0); MBC_B(:,:,iaz) = row_col_xtion(MBC_B(:,:,iaz), new_seq_cont, 2, 1); end %--
       MBC_B(:,:,iaz) = row_col_xtion(MBC_B(:,:,iaz), new_seq_states, 1, 1); %--

       clear xBMat; %--
    end

    if (exist('BdMat'))
       xBdMat = row_col_xtion(BdMat(:,:,iaz), new_seq_states, 1, 0); %--

       Bd1 = xBdMat(1:ndof,:);
       Bd2 = xBdMat(ndof+1:ns,:);
       MBC_Bd(:,:,iaz) = [T1v*Bd1;T1v*Bd2];

       MBC_Bd(:,:,iaz) = row_col_xtion(MBC_Bd(:,:,iaz), new_seq_states, 1, 1); %--

       clear xBdMat; %--
    end

    if (exist('CMat'))
       xCMat = CMat(:,:,iaz);
       if (new_seq_out ~= 0); xCMat = row_col_xtion(CMat(:,:,iaz), new_seq_out, 1, 0); end %--
       xCMat = row_col_xtion(xCMat, new_seq_states, 2, 0); %--

       MBC_C(:,:,iaz) = T1ov*xCMat*[T1, zeros(ndof); omg*T2, T1];

       MBC_C(:,:,iaz) = row_col_xtion(MBC_C(:,:,iaz), new_seq_states, 2, 1); %--
       if (new_seq_out ~= 0); MBC_C(:,:,iaz) = row_col_xtion(MBC_C(:,:,iaz), new_seq_out, 1, 1); end %--

       clear xCMat; %--
    end

    if (exist('DMat'))
       xDMat = DMat(:,:,iaz);
       if (new_seq_out ~= 0); xDMat = row_col_xtion(DMat(:,:,iaz), new_seq_out, 1, 0); end %--
       if (new_seq_cont ~= 0); xDMat = row_col_xtion(xDMat, new_seq_cont, 2, 0); end %--

       MBC_D(:,:,iaz) = T1ov*xDMat*T1c;

       if (new_seq_cont ~= 0); MBC_D(:,:,iaz) = row_col_xtion(MBC_D(:,:,iaz), new_seq_cont, 2, 1); end %--
       if (new_seq_out ~= 0); MBC_D(:,:,iaz) = row_col_xtion(MBC_D(:,:,iaz), new_seq_out, 1, 1); end %--

      clear xDMat; %--
    end

    if (exist('DdMat'))
       xDdMat = DdMat(:,:,iaz);
       if (new_seq_out ~= 0); xDdMat = row_col_xtion(DdMat(:,:,iaz), new_seq_out, 1, 0); end %--
       MBC_Dd(:,:,iaz) = T1ov*xDdMat;

       if (new_seq_out ~= 0); MBC_Dd(:,:,iaz) = row_col_xtion(MBC_Dd(:,:,iaz), new_seq_out, 1, 1); end %--

       clear xDdMat; %--
    end

%  end

% mbc transformation of second-order matrices
%  if ( MBC_EqnsOrder == 2 ) %% activate later

    if (exist('MassMat'))
       xMassMat = row_col_xtion(MassMat(:,:,iaz), new_seq_dof, 1, 0); %--
       xMassMat = row_col_xtion(xMassMat, new_seq_dof, 2, 0); %--
       MBC_M(:,:,iaz) = xMassMat*T1;
       MBC_M(:,:,iaz) = row_col_xtion(MBC_M(:,:,iaz), new_seq_dof, 2, 1); %--
       MBC_M(:,:,iaz) = row_col_xtion(MBC_M(:,:,iaz), new_seq_dof, 1, 1); %--

       xDampMat = row_col_xtion(DampMat(:,:,iaz), new_seq_dof, 1, 0); %--
       xDampMat = row_col_xtion(xDampMat, new_seq_dof, 2, 0); %--
       MBC_Dmp(:,:,iaz) = 2*omg*xMassMat*T2+xDampMat*T1;
       MBC_Dmp(:,:,iaz) = row_col_xtion(MBC_Dmp(:,:,iaz), new_seq_dof, 2, 1); %--
       MBC_Dmp(:,:,iaz) = row_col_xtion(MBC_Dmp(:,:,iaz), new_seq_dof, 1, 1); %--

       xStffMat = row_col_xtion(StffMat(:,:,iaz), new_seq_dof, 1, 0); %--
       xStffMat = row_col_xtion(xStffMat, new_seq_dof, 2, 0); %--
       MBC_K(:,:,iaz) = omg2*xMassMat*T3+OmgDot*xMassMat*T2+omg*xDampMat*T2+xStffMat*T1;
       MBC_K(:,:,iaz) = row_col_xtion(MBC_K(:,:,iaz), new_seq_dof, 2, 1); %--
       MBC_K(:,:,iaz) = row_col_xtion(MBC_K(:,:,iaz), new_seq_dof, 1, 1); %--

       clear xMassMat  xDampMat  xStffMat; %--
    end

    if (exist('FMat'))
       xFMat = row_col_xtion(FMat(:,:,iaz), new_seq_dof, 1, 0); %--
       if (new_seq_cont ~= 0); xFMat = row_col_xtion(xFMat, new_seq_cont, 2, 0); end %--

       MBC_F(:,:,iaz) = xFMat*T1c;

       if (new_seq_cont ~= 0); MBC_F(:,:,iaz) = row_col_xtion(MBC_F(:,:,iaz), new_seq_cont, 2, 1); end %--
       MBC_F(:,:,iaz) = row_col_xtion(MBC_F(:,:,iaz), new_seq_dof, 1, 1); %--

       clear xFMat; %--
    end

    if (exist('FdMat'))
       xFdMat = row_col_xtion(FdMat(:,:,iaz), new_seq_dof, 1, 0); %--

       MBC_Fd(:,:,iaz) = xFdMat;

       MBC_Fd(:,:,iaz) = row_col_xtion(MBC_Fd(:,:,iaz), new_seq_dof, 1, 1); %--

       clear xFdMat; %--
    end

    if (exist('DspCMat'))
       xVelCMat=VelCMat(:,:,iaz);
       if (new_seq_out ~= 0); xVelCMat = row_col_xtion(VelCMat(:,:,iaz), new_seq_out, 1, 0); end %--
       xVelCMat = row_col_xtion(xVelCMat, new_seq_dof, 2, 0); %--
       MBC_Vc(:,:,iaz) = T1ov*xVelCMat*T1;
       MBC_Vc(:,:,iaz) = row_col_xtion(MBC_Vc(:,:,iaz), new_seq_dof, 2, 1); %--
       if (new_seq_out ~= 0); MBC_Vc(:,:,iaz) = row_col_xtion(MBC_Vc(:,:,iaz), new_seq_out, 1, 1); end %--

       xDspCMat=DspCMat(:,:,iaz);
       if (new_seq_out ~= 0); xDspCMat = row_col_xtion(DspCMat(:,:,iaz), new_seq_out, 1, 0); end %--
       xDspCMat = row_col_xtion(xDspCMat, new_seq_dof, 2, 0); %--
       MBC_Dc(:,:,iaz) = T1ov*(omg*xVelCMat*T2+xDspCMat*T1);
       MBC_Dc(:,:,iaz) = row_col_xtion(MBC_Dc(:,:,iaz), new_seq_dof, 2, 1); %--
       if (new_seq_out ~= 0); MBC_Dc(:,:,iaz) = row_col_xtion(MBC_Dc(:,:,iaz), new_seq_out, 1, 1); end %--

       clear xDspCMat xVelCMat; %--
    end

%  end

end   % end of azimuth loop
%---------------------------- Eigensolution ---------------------------------------

if (exist('AMat'))
   MBC_AvgA = MBC_AvgA/NAzSteps;   % azimuth-average of azimuth-dependent MBC_A matrices

   % --- Eigensolution of the Azimuth-averaged MBC_AvgA matrix (in the fixed frame)
   %  call function 'eianalysis' to obtain the following
   %    nss                : number of re-sequenced states
   %    EigenVects         : nsXns
   %    Evals              : nsX1
   %    eigenVects         : ndofXnss
   %    eigenVals          : nssX1
   %    DampedFrequencies  : nssX1
   %    DampedFreqs_Hz     : nssX1
   %    NaturalFrequencies : nssX1
   %    NaturalFreqs_Hz    : nssX1
   %    DecrementRate      : nssX1
   %    DampRatios         : nssX1
   %    MagnitudeModes     : ndofXnss
   %    PhaseModes_deg     : ndofXnss

  [EigenVects,eigenVects,Evals,eigenVals,DampedFrequencies,DampedFreqs_Hz,NaturalFrequencies,...
   NaturalFreqs_Hz,DecrementRate,DampRatios,MagnitudeModes,PhaseModes_deg,nss] = eianalysis(MBC_AvgA);

   MBC_EigenVects = EigenVects;
   MBC_Evals = Evals;
   MBC_eigenVects = eigenVects;
   MBC_eigenVals = eigenVals;

   MBC_DampedFrequency    = DampedFrequencies;
   MBC_DampedFrequencyHz  = DampedFreqs_Hz;
   MBC_NaturalFrequency   = NaturalFrequencies;
   MBC_NaturalFrequencyHz = NaturalFreqs_Hz;
   MBC_DecrementRate = DecrementRate;
   MBC_DampingRatio       = DampRatios;
   MBC_ModeShapeMagnitude = MagnitudeModes;
   MBC_ModeShapePhaseDeg  = PhaseModes_deg;

end
% ---- Label fixed-coordinate states following mbc transformation
%    if (exist('DescStates'))
%     MBC_States = DescStates;
%     for is = ndfix+1:3:ndof;
%       k = strfind(MBC_States{is},'DOF');
%       MBC_States{is} = [MBC_States{is}(1:k-1) 'collective DOF'];
%       k = strfind(MBC_States{is+1},'DOF');
%       MBC_States{is+1} = [MBC_States{is+1}(1:k-1) 'cosine DOF'];
%       k = strfind(MBC_States{is+2},'DOF');
%       MBC_States{is+2} = [MBC_States{is+2}(1:k-1) 'sine DOF'];
%     end
%    end

% --- Clear non-required data and display successful completion of MBC
  clear omg NAzSteps nb ndof ndof_rot ns azm iaz ndfix ncfix nofix nm mc mo omg2 OmgDot;
  clear T1 T2 T3 T1v T1c T1o T1ov;
  clear cos_col sin_col tt ett c1 c2 c3 s1 s2 s3 del_az az1 az2 az3 is k;
  clear tc1 cos231_col ttv cos312_col tt2 sin231_col tt3 sin312_col ii;
  if (exist('Bd1'))
   clear Bd1;
  end
  if (exist('Bd2'))
   clear Bd2;
  end
  if (exist('B1'))
   clear B1;
  end
  if (exist('B2'))
   clear B2;
  end
  if (exist('AK'))
   clear AK;
  end
  if (exist('AC'))
   clear AC;
  end

% ----------- Clear unneeded variables -------------------------------
clear EigenVects eigenVects Evals eigenVals DampedFrequencies DampedFreqs_Hz NaturalFrequencies;
clear NaturalFreqs_Hz DecrementRate DampRatios MagnitudeModes PhaseModes_deg;
clear new_seq_dof  new_seq_states;

if (exist('RotTripletIndicesCntrlInpt'))
  clear new_seq_cont;
end

if (exist('RotTripletIndicesOutput'))
  clear new_seq_out;
end

  disp('  ');
  disp(' Multi-Blade Coordinate transformation completed ');
%-----------------------------------------------------------
%%%xmbc =[MBC_DecrementRate MBC_DampedFrequencyHz MBC_DampingRatio*100 MBC_NaturalFrequencyHz]'
