function [z,ASAcontrol] = convol(x,y,shift1,shift2,last)
%CONVOL Convolution sum
%   Z = CONVOL(X,Y) is the convolution sum of two vectors X and Y.
%
%   CONVOL(X) is the auto-convolution sum of a vector X.
%
%   CONVOL(X,Y,SHIFT1,SHIFT2) or CONVOL(X,SHIFT1,SHIFT2) returns only the
%   elements from SHIFT1 up to SHIFT2.
%
%   CONVOL is an ARMASA main function.
%
%   See also: CONVOLREV, DECONVOL, CONV.

%Header
%=============================================================================

%Declaration of variables
%------------------------

%Declare and assign values to local variables
%according to the input argument pattern
switch nargin
case 1
   if isa(x,'struct'), ASAcontrol=x; x=[];
   else, ASAcontrol=[];
   end
   y=[]; shift1=[]; shift2=[];
case 2
   if isa(y,'struct'), ASAcontrol=y; y=[];
   else, ASAcontrol=[];
   end
   shift1=[]; shift2=[];
case 3
   if isa(shift1,'struct'), ASAcontrol=shift1; shift1=[]; shift2=[];
   else, ASAcontrol=[]; shift2=shift1; shift1=y; y=[];
   end
case 4
   if isa(shift2,'struct'), ASAcontrol=shift2; shift2=shift1; shift1=y; y=[];
   else, ASAcontrol=[];
   end
case 5
   if isa(last,'struct'), ASAcontrol=last;
   else, error(ASAerr(39))
   end
otherwise
   error(ASAerr(1,mfilename))
end

if isequal(nargin,1) & ~isempty(ASAcontrol)
      %ASAcontrol is the only input argument
   ASAcontrol.error_chk = 0;
   ASAcontrol.run = 0;
end

%ARMASA-function version information
%-----------------------------------

%This ARMASA-function is characterized by
%its current version,
ASAcontrol.is_version = [2000 12 6 12 17 20];
%and its compatability with versions down to,
ASAcontrol.comp_version = [2000 12 4 15 0 0];

%Checks
%------

if ~any(strcmp(fieldnames(ASAcontrol),'error_chk')) | ASAcontrol.error_chk
      %Perform standard error checks
   %Input argument format checks
   ASAcontrol.error_chk = 1;
   if ~isnum(x)
      error(ASAerr(11,'x'))
   end
   if ~isempty(y) & ~isnum(y)
      error(ASAerr(11,'y'))
   end
   if ~isempty(shift1) & (~isnum(shift1) | ...
         ~isintscalar(shift1) | shift1<0)
      error(ASAerr(17,'shift1'))
   end
   if ~isempty(shift2) & (~isnum(shift2) | ...
         ~isintscalar(shift2) | shift2<0)
      error(ASAerr(17,'shift2'))
   end

   %Input argument value checks
   if ~isavector(x)
      error([ASAerr(14) ASAerr(15,'x')])
   else
      l_x = length(x);
      l_y = length(x);
   end
   if ~isempty(y)
      if ~isavector(y)
         error([ASAerr(14) ASAerr(15,'y')])
      end
      l_y = length(y);
   end
   if ~(isempty(shift1) & isempty(shift2)) & ...
         (shift1 > shift2 | shift2 > l_x+l_y-1 | ...
         shift1==0)
      error(ASAerr(20,{'shift1','shift2'}))
   end
end

if ~any(strcmp(fieldnames(ASAcontrol),'version_chk')) | ASAcontrol.version_chk
      %Perform version check
   ASAcontrol.version_chk = 1;

   %Make sure the requested version of this function
   %complies with its actual version
   ASAversionchk(ASAcontrol);
end

if ~any(strcmp(fieldnames(ASAcontrol),'run')) | ASAcontrol.run
      %Run the computational kernel
   ASAcontrol.run = 1;

%Main
%========================================================================

s_x = size(x);
l_x = prod(s_x);
if isempty(y)
   s_y = s_x;
   l_y = l_x;
else
   s_y = size(y);
   l_y = prod(s_y);
end
if l_x>l_y
   l_long = l_x;
   l_short = l_y;
else
   l_long = l_y;
   l_short = l_x;
end
if isempty(shift1)
   shift1 = 1;
   shift2 = l_long+l_short-1;
end
if s_x(1)==1 & s_y(1)==1
   z = zeros(1,shift2-shift1+1);
else
   z = zeros(shift2-shift1+1,1);
end

%Determination of the fastest approach
if l_long<81
   mode=2;
else
   if isempty(y)
      l_fft = nxtppow(l_long+1+max(shift2-l_long,l_long-shift1));
      t_fft = 14.6*l_fft*log(l_fft)+6*l_fft;
      thresh = min(l_long,shift2);
      t_direct = thresh^2-shift1^2+thresh+shift1;
      if shift2>thresh
         t_direct = t_direct+4*l_long*shift2-shift2^2-3*l_long^2;
      end
      if t_fft<t_direct
         mode = 3;
      else
         mode = 1;
      end
   else
      t_direct = 0;
      start = shift1;
      if shift1<l_short
         thresh = min(l_short,shift2);
         t_direct = thresh^2-shift1^2;
         start = l_short;
      end
      if start>=l_short
         thresh = min(l_long,shift2);
         t_direct = t_direct+2*l_short*max(0,(thresh-l_short));
      end
      if shift2>l_long
         t_direct = t_direct+2*l_short*(shift2-l_long)-(shift2-l_long)^2;
      end
      if shift1<l_short
         l_eff = shift2;
      else
         l_eff = shift2-shift1+l_short;
      end
      t_filt = 0.6*l_eff*l_short+1.2*l_short^2;
      l_fft = nxtppow(l_long+l_short);
      t_fft = 14.6*l_fft*log(l_fft)+6*l_fft;
      [mode,index] = sort([t_direct t_filt t_fft]);
      mode = index(1);
   end
end

if mode==1 %Direct approach,
           %minimized number of flops
  if isempty(y)
     y = x;
  end
  if l_x>=l_y
      if s_x(2)>1
         long = x;
      else
         long = x';
      end
      if s_y(2)>1;
         short = y(l_y:-1:1)';
      else
         short = y(l_y:-1:1);
      end
      first = shift1;
      last = shift2;
      k = 1;
      k_increm = 1;
   else
      if s_x(2)>1
         short = x';
      else
         short = x;
      end
      if s_y(2)>1;
         long = y(l_y:-1:1);
      else
         long = y(l_y:-1:1)';
      end
      first = l_x+l_y-shift2;
      last = l_x+l_y-shift1;
      k = last-first+1;
      k_increm = -1;
   end
   start = first;
   run_in = l_short-first;
   if run_in>0
      index_temp = run_in+1;
      i_stop = min(last-first,run_in-1);
      for i = 0:i_stop
         z(k) = long(1:first+i)*short(index_temp-i:l_short);
         k = k+k_increm;
      end
      start = l_short;
   end
   run_center = start-l_short;
   if run_center>=0
      if last<=l_long
         stop = last;
      else
         stop = l_long;
      end
      index_temp = run_center+1;
      i_stop = stop-start;
      for i=0:i_stop
         z(k) = long(index_temp+i:start+i)*short;
         k = k+k_increm;
      end
   end
   run_out = last-l_long;
   if run_out>0
      index_temp = l_long-l_short+1;
      i_start = max(1,first-l_long);
      for i = i_start:run_out
         z(k) = long(index_temp+i:l_long)*short(1:l_short-i);
         k = k+k_increm;
      end
   end
elseif mode==2 %Filter approach,
               %non-optimal in the sense of flops but sometimes
               %faster because 'filter' is a built-in function
   if isempty(y)
      y = x;
   end
   if l_x>l_y
      if s_x(1)==1 & s_y(1)==1
         long = zeros(1,l_x+l_y-1);
         long(1:l_x) = x;
      else
         long = zeros(l_x+l_y-1,1);
         long(1:l_x) = x;
      end
      short = y;
   else
      if s_x(1)==1 & s_y(1)==1
         long = zeros(1,l_x+l_y-1);
         long(1:l_y) = y;
      else
         long = zeros(l_x+l_y-1,1);
         long(1:l_y) = y;
      end
      short = x;
   end
   first = shift1;
   last = shift2;
   if first-l_short>0
      start = first-l_short+1;
      first = l_short;
   else
      start = 1;
   end
   stop = last;
   z = filter(short,1,long(start:stop));
   z = z(first:end);
elseif mode==3 %FFT approach,
               %fast for long sequences
   if ~isempty(y) %cross-convolution
      first = shift1;
      last = shift2;
      run_in = l_short-first;
      if run_in<0
         start = 1-run_in;
         first = l_short;
      else
         start = 1;
      end
      if last<l_long
         stop = last;
      else
         stop = l_long;
      end
      first = first+1;
      last = last-start+2;
      l_long = stop-start+1;
      sig_mtx = zeros(l_fft,2);
      if l_x>l_y
         x = x(stop:-1:start);
         sig_mtx(l_fft-l_long+1:l_fft,1) = x(:);
         sig_mtx(1:l_y,2) = y(:);
      else
         y = y(stop:-1:start);
         sig_mtx(1:l_x,2) = x(:);
         sig_mtx(l_fft-l_long+1:l_fft,1,1) = y(:);
      end
      transf_mtx = fft(sig_mtx);
      z = real(fft(transf_mtx(:,1).*conj(transf_mtx(:,2))));
      if s_x(1)==1 & s_y(1)==1
         z = z(first:last)'/l_fft;
      else
         z = z(first:last)/l_fft;
      end
   else %auto-convolution
      l_sig = l_x;
      first = shift1+1;
      last = shift2+1;
      sig_mtx = zeros(l_fft,2);
      sig_mtx(1:l_x,2) = x(:);
      x = x(l_x:-1:1);
      sig_mtx(l_fft-l_x+1:l_fft,1) = x(:);
      transf_mtx = fft(sig_mtx);
      z = real(fft(transf_mtx(:,1).*conj(transf_mtx(:,2))));
      if s_x(1)==1 & s_y(1)==1
         z = z(first:last)'/l_fft;
      else
         z = z(first:last)/l_fft;
      end
   end
end

%Footer
%=====================================================

else %Skip the computational kernel
   %Return ASAcontrol as the first output argument
   if nargout>1
      warning(ASAwarn(9,mfilename,ASAcontrol))
   end
   z = ASAcontrol;
   ASAcontrol = [];
end

%Program history
%======================================================================
%
% Version                Programmer(s)          E-mail address
% -------                -------------          --------------
% [2000 12 4 15  0  0]   W. Wunderink           wwunderink01@freeler.nl
% [2000 12 6 12 17 20]   W. Wunderink           wwunderink01@freeler.nl


function [ y, m ] = month_carry_islamic ( y, m )

%*****************************************************************************80
%
%% MONTH_CARRY_ISLAMIC carries a year of months on the Islamic calendar.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, M, the year and month.
%    On output, M is no greater than 12.
%
  while ( 1 )

    months = year_length_months_islamic ( y );

    if ( m <= months )
      break
    end

    m = m - months;
    y = y + 1;

  end

  return
end


function k = prtRvUtilDiscreteKld(q,p)
% DISCRETEKLD

k = q.*(log(q)-log(p));
k(q==0) = 0;
k = sum(k(:));

end


function training_data = balanceReactionsInTrainingData(training_data)

if ~isfield(training_data, 'Ematrix') || isempty(training_data.Ematrix)
    [MW, Ematrix] = getMolecularWeight(training_data.nstd_inchi, 0);
    training_data.Ematrix = Ematrix(:, 2:end); % remove H, columns are [C, N, O, P, S, e-]
    conserved = training_data.Ematrix' * training_data.S;

    % need to check that all elements are balanced (except H, but including e-)
    % if only O is not balanced, add water molecules

    % check all reactions which can be checked (not NaN) and should be checked
    % (i.e. not formation or redox reactions)
    inds = find(~isnan(conserved(1,:)) .* training_data.balance');

    % first add water molecules to reactions that need it
    i_h2o = find(training_data.cids == 1);
    training_data.S(i_h2o, inds) = training_data.S(i_h2o, inds) - conserved(3, inds);

    % recalculate conservation matrix
    conserved = training_data.Ematrix' * training_data.S;

    inds_to_remove = inds(find(any(conserved(:, inds))));

    inds = setdiff(1:size(training_data.S, 2), inds_to_remove);
    training_data.S = training_data.S(:, inds);
    training_data.dG0_prime = training_data.dG0_prime(inds);
    training_data.T = training_data.T(inds);
    training_data.I = training_data.I(inds);
    training_data.pH = training_data.pH(inds);
    training_data.pMg = training_data.pMg(inds);
    training_data.weights = training_data.weights(inds);
    training_data.balance = false(size(inds));
end

fprintf('Successfully created balanced training-data structure: %d compounds and %d reactions\n',...
        size(training_data.S, 1), size(training_data.S, 2));


%%*************************************************************************
%% sqlparameters.m: set OPTIONS structure to specify default
%%                  parameters for sqlp.m
%%
%% OPTIONS.vers        : version of direction to use.
%%                       1 for HKM direction
%%                       2 for NT direction
%%                       0 for the default (which uses the HKM direction if
%%                       semidefinite blocks exist; and NT direction of SOCP problms)
%% OPTIONS.gam         : step-length parameter,
%% OPTIONS.predcorr    : whether to use Mehrotra predictor-corrector.
%% OPTIONS.expon       : exponent in decrease of centering parameter sigma.
%% OPTIONS.gaptol      : tolerance for duality gap as a fraction of the
%%                       value of the objective functions.
%% OPTIONS.inftol      : tolerance for stopping due to suspicion of
%%                       infeasibility.
%% OPTIONS.steptol     : toloerance for stopping due to small steps.
%% OPTIONS.maxit       : maximum number of iteration allowed
%% OPTIONS.printlevel  : 3, if want to display result in each iteration,
%%                       2, if want to display only summary,
%%                       1, if want to display warning message,
%%                       0, no display at all.
%% OPTIONS.stoplevel   : 2, if want to automatically detect termination;
%%                       1, if want to automatically detect termination, but
%%                          restart automatically with a new iterate
%%                          when the algorithm stagnants because of tiny step-lengths.
%%                       0, if want the algorithm to continue forever except for
%%                          successful completion, maximum number of iterations, or
%%                          numerical failures. Note, do not change this field unless
%%                          you very sure.
%% OPTIONS.scale_data  : 1, if want to scale the data before solving the problem,
%%                          else = 0
%% OPTIONS.rmdepconstr : 1, if want to remove nearly dependent constraints,
%%                          else = 0.
%% OPTIONS.smallblkdim : block-size threshold determining what method to compute the
%%                       schur complement matrix corresponding to semidefintie block.
%%                       NOTE: this number should be small, say less than 20.
%% OPTIONS.parbarrier  : parameter values of the log-barrier terms in the SQLP problem.
%%                       Default = [],  meaning that the parameter values are all 0.
%% OPTIONS.schurfun    : [], if no user supplied routine to compute the Schur matrix,
%%                       else, it is a cell array where each cell is either [],
%%                       or contains a string that is the file name where the Schur matrix
%%                       of the associated block data is computed.
%%                       For example, if the SQLP data has the block structure
%%                       blk{1,1} = '1'; blk{1,2} = 10;
%%                       blk{2,1} = 's'; blk{2,2} = 50;
%%                       and
%%                       OPTIONS.schurfun{1} = [];
%%                       OPTIONS.schurfun{2} = 'myownschur', where
%%                             'myownschur' is a function with the calling sequence:
%%                              function   schur = myownschur(X2,Z2inv,schurfun_par(2,:));
%%                       This means that for the first block, the Schur
%%                       matrix is computed by the default method in SDPT3,
%%                       and for the second block, the user supplies the
%%                       routine to compute the associated Schur matrix.
%% OPTIONS.schurfun_par: [], if no user supplied routine to compute the Schur matrix,
%%                       else, it is a cell array where the p-th row is either [],
%%                       or is a cell array containing the parameters needed in
%%                       the user supplied Schur routine OPTIONS.schurfun{p}.
%%                       For example, for the block structure described
%%                       above, we may have:
%%                       OPTIONS.schurfun_par{1} = [];
%%                       OPTIONS.schurfun_par{2,1} = par1;
%%                       OPTIONS.schurfun_par{2,2} = par2;
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

   function OPTIONS = sqlparameters

   OPTIONS.vers           = 0;
   OPTIONS.gam            = 0;
   OPTIONS.predcorr       = 1;
   OPTIONS.expon          = 1;
   OPTIONS.gaptol         = 1e-8;
   OPTIONS.inftol         = 1e-8;
   OPTIONS.steptol        = 1e-6;
   OPTIONS.maxit          = 100;
   OPTIONS.printlevel     = 3;
   OPTIONS.stoplevel      = 1; %% do not change this field unless you very sure.
   OPTIONS.scale_data     = 0;
   OPTIONS.spdensity      = 0.4;
   OPTIONS.rmdepconstr    = 0;
   OPTIONS.smallblkdim    = 50;
   OPTIONS.parbarrier     = [];
   OPTIONS.schurfun       = [];
   OPTIONS.schurfun_par   = [];
%%*************************************************************************


function [ is_ok, text_error, total_dur ] = checkTiming( system, varargin )
%checkTiming(sys, objects, ...)
%   Function checks whether timing of the specified evets is aligned to
%   the corresponding raster

    if (isempty(varargin))
        text_error=['empty or damaged block detected' ];
        is_ok=false;
        return;
    end
    total_dur=mr.calcDuration(varargin{:});
    is_ok=div_check(total_dur,system.blockDurationRaster);
    if (is_ok)
        text_error=[];
    else
        text_error=['total duration:' num2str(total_dur*1e6) 'us' ];
    end
    for i=1:length(varargin)
        e=varargin{i};
        if isnumeric(e) % special handling for blockDuration
            continue;
        end
        assert(isstruct(e), 'wrong format of the variable aguments, list of structures is expected');
        ok=true;
        if length(e)>1
            % for now this is only the case for arrays of extensions, but
            % we actually cannot check extensons anyway...
            continue;
        end
        if isfield(e, 'type') && (strcmp(e.type,'adc') || strcmp(e.type,'rf'))
            raster=system.rfRasterTime;
        else
            raster=system.gradRasterTime;
        end
        if isfield(e, 'delay')
            if e.delay<-eps
                ok=false;
            end
            if ~div_check(e.delay,raster)
                ok=false;
            end
        end
        if isfield(e, 'duration')
            if ~div_check(e.duration,raster)
                ok=false;
            end
        end
        if isfield(e, 'dwell') % special case ADC
            if e.dwell<system.adcRasterTime || abs(round(e.dwell/system.adcRasterTime)*system.adcRasterTime - e.dwell)>1e-10
                ok=false;
            end
        end
        if isfield(e, 'type') && strcmp(e.type,'trap')
            if ~div_check(e.riseTime, system.gradRasterTime) || ~div_check(e.flatTime, system.gradRasterTime) || ~div_check(e.fallTime, system.gradRasterTime)
                ok=false;
            end
        end
        if ~ok
            is_ok=false;
            if ~isempty(text_error)
                text_error = [text_error ' '];
            end
            text_error = [text_error '[ '];
            if isfield(e, 'type')
                text_error = [text_error 'type:' e.type ' ' ];
            end
            if isfield(e, 'delay')
                text_error = [text_error 'delay:' num2str(e.delay*1e6) 'us ' ];
            end
            if isfield(e, 'duration')
                text_error = [text_error 'duration:' num2str(e.duration*1e6) 'us ' ];
            end
            if isfield(e, 'dwell')
                text_error = [text_error 'dwell:' num2str(e.dwell*1e9) 'ns ' ];
            end
            if isfield(e, 'type') && strcmp(e.type,'trap')
                text_error = [text_error 'riseTime:' num2str(e.riseTime*1e6) 'us flatTime:' num2str(e.flatTime*1e6) 'us fallTime:' num2str(e.fallTime*1e6) 'us '];
            end
            text_error = [text_error ']'];
        end
    end
end

function out = div_check(a, b)
%  checks wheher a can be divided by b to an accuracy of 1e-9
    c = a / b;
    out = (abs( c - round(c) ) < 1e-9);
end


function r = reverse(x)
% reverse -- Reverse order of elements in 1-d signal
%  Usage
%    r = reverse(x)
%  Inputs
%    x     1-d signal
%  Outputs
%    r     1-d time-reversed signal
%
%  See Also
%    flipud, fliplr
%
   r = x(length(x):-1:1);

%
% Copyright (c) 1993. David L. Donoho
%

%
%  Part of Wavelab Version 850
%  Built Tue Jan  3 13:20:40 EST 2006
%  This is Copyrighted Material
%  For Copying permissions see COPYING.m
%  Comments? e-mail wavelab@stat.stanford.edu


function [sys,x0,str,ts] = nrotor_dynamics(t,x,u,flag, vehicle, x0, groundflag)
    % Flyer2dynamics lovingly coded by Paul Pounds, first coded 12/4/04
    % A simulation of idealised X-4 Flyer II flight dynamics.
    % version 2.0 2005 modified to be compatible with latest version of Matlab
    % version 3.0 2006 fixed rotation matrix problem
    % version 4.0 4/2/10, fixed rotor flapping rotation matrix bug, mirroring
    % version 5.0 8/8/11, simplified and restructured
   % version 6.0 25/10/13, fixed rotation matrix/inverse wronskian definitions, flapping cross-product bug

    % modified by PIC for N rotors
    global groundFlag

    warning off MATLAB:divideByZero

    % New in version 2:
    %   - Generalised rotor thrust model
    %   - Rotor flapping model
    %   - Frame aerodynamic drag model
    %   - Frame aerodynamic surfaces model
    %   - Internal motor model
    %   - Much coolage

    % Version 1.3
    %   - Rigid body dynamic model
    %   - Rotor gyroscopic model
    %   - External motor model

    %ARGUMENTS
    %   u       Reference inputs                1x4
    %   tele    Enable telemetry (1 or 0)       1x1
    %   crash   Enable crash detection (1 or 0) 1x1
    %   init    Initial conditions              1x12

    %INPUTS
    %   u      rotor speed                      1xN

    %CONTINUOUS STATES
    %   z      Position                         3x1   (x,y,z)
    %   v      Velocity                         3x1   (xd,yd,zd)
    %   n      Attitude                         3x1   (Y,P,R)
    %   o      Angular velocity                 3x1   (wx,wy,wz)
    %   w      Rotor angular velocity           4x1

    %CONTINUOUS STATE MATRIX MAPPING
    %   x = [z1 z2 z3 n1 n2 n3 z1 z2 z3 o1 o2 o3 w1 w2 w3 w4]

    %INITIAL CONDITIONS
    n0 = [0 0 0];               %   n0      Ang. position initial conditions    1x3
    v0 = [0 0 0];               %   v0      Velocity Initial conditions         1x3
    o0 = [0 0 0];               %   o0      Ang. velocity initial conditions    1x3
    init = [x0 n0 v0 o0];       % x0 is the passed initial position 1x3
    groundFlag = groundflag;;

    %CONTINUOUS STATE EQUATIONS
    %   z' = v
    %   v' = g*e3 - (1/m)*T*R*e3
    %   I*o' = -o X I*o + G + torq
    %   R = f(n)
    %   n' = inv(W)*o

    % basic sanity checks on number of rotors
    if ~isfield(vehicle, 'nrotors')
         error('RTB:nrotor_dynamics:bardarg', 'Number of rotors not specified in model structure')
    end
    if mod(vehicle.nrotors,2) == 1
        error('RTB:nrotor_dynamics:bardarg', 'Number of rotors must be even')
    end
    if vehicle.nrotors < 4
        error('RTB:nrotor_dynamics:bardarg', 'Number of rotors must be at least 4')
    end

    groundFlag = groundflag;

    % Dispatch the flag.
    switch flag
        case 0
            [sys,x0,str,ts]=mdlInitializeSizes(init, vehicle); % Initialization
        case 1
            sys = mdlDerivatives(t,x,u, vehicle); % Calculate derivatives
        case 3
            sys = mdlOutputs(t,x, vehicle); % Calculate outputs
        case { 2, 4, 9 } % Unused flags
            sys = [];
        otherwise
            error(['Unhandled flag = ',num2str(flag)]); % Error handling
    end
end % End of flyer2dynamics

%==============================================================
% mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the
% S-function.
%==============================================================
%
function [sys,x0,str,ts] = mdlInitializeSizes(init, vehicle)
    %
    % Call simsizes for a sizes structure, fill it in and convert it
    % to a sizes array.
    %
    sizes = simsizes;
    sizes.NumContStates  = 12;
    sizes.NumDiscStates  = 0;
    sizes.NumOutputs     = 12;
    sizes.NumInputs      = vehicle.nrotors;
    sizes.DirFeedthrough = 0;
    sizes.NumSampleTimes = 1;
    sys = simsizes(sizes);
    %
    % Initialize the initial conditions.
    x0 = init;
    %
    % str is an empty matrix.
    str = [];
    %
    % Generic timesample
    ts = [0 0];

    if vehicle.verbose
        disp(sprintf('t\t\tz1\t\tz2\t\tz3\t\tn1\t\tn2\t\tn3\t\tv1\t\tv2\t\tv3\t\to1\t\to2\t\to3\t\tw1\t\tw2\t\tw3\t\tw4\t\tu1\t\tu2\t\tu3\t\tu4'))
    end
end % End of mdlInitializeSizes.

%==============================================================
% mdlDerivatives
% Calculate the state derivatives for the next timestep
%==============================================================
%
function sys = mdlDerivatives(t,x,u, quad)
    global a1s b1s

    for i = 1:quad.nrotors
        theta = (i-1)/quad.nrotors*2*pi;
        %   Di      Rotor hub displacements (1x3)
        % first rotor is on the x-axis, clockwise order looking down from above
        D(:,i) = [ quad.d*cos(theta); quad.d*sin(theta); quad.h];
    end

    %Body-fixed frame references
    e1 = [1;0;0];               %   ei      Body fixed frame references         3x1
    e2 = [0;1;0];
    e3 = [0;0;1];

    %EXTRACT ROTOR SPEED DEMANDS FROM U
    w = u(1:quad.nrotors);

    %EXTRACT STATES FROM X
    z = x(1:3);   % position in {W}
    n = x(4:6);   % RPY angles {W}
    v = x(7:9);   % velocity in {W}
    o = x(10:12); % angular velocity in {W}

    %PREPROCESS ROTATION AND WRONSKIAN MATRICIES
    phi = n(1);    % yaw
    the = n(2);    % pitch
    psi = n(3);    % roll

    % rotz(phi)*roty(the)*rotx(psi)
    R = [cos(the)*cos(phi) sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi) cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi);   %BBF > Inertial rotation matrix
         cos(the)*sin(phi) sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi) cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi);
         -sin(the)         sin(psi)*cos(the)                            cos(psi)*cos(the)];

    %Manual Construction
    %     Q3 = [cos(phi) -sin(phi) 0;sin(phi) cos(phi) 0;0 0 1];   % RZ %Rotation mappings
    %     Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)];   % RY
    %     Q1 = [1 0 0;0 cos(psi) -sin(psi);0 sin(psi) cos(psi)];   % RX
    %     R = Q3*Q2*Q1    %Rotation matrix
    %
    %    RZ * RY * RX
    iW = [0        sin(psi)          cos(psi);             %inverted Wronskian
          0        cos(psi)*cos(the) -sin(psi)*cos(the);
          cos(the) sin(psi)*sin(the) cos(psi)*sin(the)] / cos(the);

    %ROTOR MODEL

    for i=1:quad.nrotors %for each rotor
        %Relative motion

        Vr = cross(o,D(:,i)) + v;
        mu = sqrt(sum(Vr(1:2).^2)) / (abs(w(i))*quad.r);  %Magnitude of mu, planar components
        lc = Vr(3) / (abs(w(i))*quad.r);   %Non-dimensionalised normal inflow
        li = mu; %Non-dimensionalised induced velocity approximation
        alphas = atan2(lc,mu);
        j = atan2(Vr(2),Vr(1));  %Sideslip azimuth relative to e1 (zero over nose)
        J = [cos(j) -sin(j);
            sin(j) cos(j)];  %BBF > mu sideslip rotation matrix

        %Flapping
        beta = [((8/3*quad.theta0 + 2*quad.theta1)*mu - 2*(lc)*mu)/(1-mu^2/2); %Longitudinal flapping
            0;];%sign(w) * (4/3)*((Ct/sigma)*(2*mu*gamma/3/a)/(1+3*e/2/r) + li)/(1+mu^2/2)]; %Lattitudinal flapping (note sign)
        beta = J'*beta;  %Rotate the beta flapping angles to longitudinal and lateral coordinates.
        a1s(i) = beta(1) - 16/quad.gamma/abs(w(i)) * o(2);
        b1s(i) = beta(2) - 16/quad.gamma/abs(w(i)) * o(1);

        %Forces and torques
        T(:,i) = quad.Ct*quad.rho*quad.A*quad.r^2*w(i)^2 * [-cos(b1s(i))*sin(a1s(i)); sin(b1s(i));-cos(a1s(i))*cos(b1s(i))];   %Rotor thrust, linearised angle approximations
        Q(:,i) = -quad.Cq*quad.rho*quad.A*quad.r^3*w(i)*abs(w(i)) * e3;     %Rotor drag torque - note that this preserves w(i) direction sign
        tau(:,i) = cross(T(:,i),D(:,i));    %Torque due to rotor thrust
    end

    %RIGID BODY DYNAMIC MODEL
    dz = v;

    % vehicle can't fall below ground
    if groundFlag && (z(3) > 0)
        z(3) = 0;
        dz(3) = 0;
    end

    dn = iW*o;
    dv = quad.g*e3 + R*(1/quad.M)*sum(T,2);
    do = inv(quad.J)*(cross(-o,quad.J*o) + sum(tau,2) + sum(Q,2)); %row sum of torques
    sys = [dz;dn;dv;do];   %This is the state derivative vector
end % End of mdlDerivatives.

%==============================================================
% mdlOutputs
% Calculate the output vector for this timestep
%==============================================================
%
function sys = mdlOutputs(t,x, quad)
    %CRASH DETECTION
    if x(3)>0
        x(3) = 0;
        if x(6) > 0
            x(6) = 0;
        end
    end
    %if (x(3)>0)&(crash)
    %    error('CRASH!')
    %end

    %TELEMETRY
    if quad.verbose
        disp(sprintf('%0.3f\t',t,x))
    end

    % compute output vector as a function of state vector
    %   z      Position                         3x1   (x,y,z)
    %   v      Velocity                         3x1   (xd,yd,zd)
    %   n      Attitude                         3x1   (Y,P,R)
    %   o      Angular velocity                 3x1   (Yd,Pd,Rd)

    n = x(4:6);   % RPY angles
    phi = n(1);    % yaw
    the = n(2);    % pitch
    psi = n(3);    % roll

    % rotz(phi)*roty(the)*rotx(psi)
    R = [cos(the)*cos(phi) sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi) cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi);   %BBF > Inertial rotation matrix
         cos(the)*sin(phi) sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi) cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi);
         -sin(the)         sin(psi)*cos(the)                            cos(psi)*cos(the)];

    iW = [0        sin(psi)          cos(psi);             %inverted Wronskian
          0        cos(psi)*cos(the) -sin(psi)*cos(the);
          cos(the) sin(psi)*sin(the) cos(psi)*sin(the)] / cos(the);

    % return velocity in the body frame
    sys = [ x(1:6);
            inv(R)*x(7:9);   % translational velocity mapped to body frame
            iW*x(10:12)];    % RPY rates mapped to body frame
    %sys = [x(1:6); iW*x(7:9);  iW*x(10:12)];
    %sys = x;
end
% End of mdlOutputs.


%parademo.m
%
% REQUIRES THE DATA SETS !!!!!
%

% Copyright (C) 1995-2006  Rasmus Bro & Claus Andersson
% Copenhagen University, DK-1958 Frederiksberg, Denmark, rb@life.ku.dk
%
% This program is free software; you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation; either version 2 of the License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more details.
% You should have received a copy of the GNU General Public License along with
% this program; if not, write to the Free Software Foundation, Inc., 51 Franklin
% Street, Fifth Floor, Boston, MA  02110-1301, USA.

close all
clear all
echo off
home
echo on
load claus;
echo off

disp(' ')
disp(' Fluorescence measurements ideally follow the trilinear PARAFAC')
disp(' model. We will use a simple data set of 5 mixtures of three')
disp(' amino-acids (trp,phe & tyr) to show how the pure spectra and')
disp(' concentrations can be found with parafac')
disp(' ')
disp(' First lets look at the raw data:')
disp(' Press any key to continue')
pause

echo on
figure(1);
for i=1:5,
   subplot(3,2,i)
   sample = squeeze(X(i,:,:));
   mesh(ExAx,EmAx,sample);
   title(['Raw data - sample ',num2str(i)]);
   xlabel('Excitation [nm]')
   ylabel('Emission [nm]')
   axis([ExAx(1) ExAx(end) EmAx(1) EmAx(end) 0 1000]);
   grid on
   drawnow
end;
echo off

disp(' ')
disp(' Press any key to continue')
pause
close all
home

disp(' ')
disp(' The data will be slightly reduced to save computation time. This is done')
disp(' by reducing the 2. and 3. emission and excitation mode:')
disp(' ')

echo on
X = X(:,1:5:end,1:2:end);
EmAx = EmAx(1:5:end);
ExAx = ExAx(1:5:end);
size(X)
echo off

disp(' ')
disp(' Press any key to continue')
pause
close all
home

disp(' ')
disp(' PARAFAC may be fitted to the data, but in order to investigate how')
disp(' many components are needed we will use the tool pftest to get an')
disp(' indication. We use 1 to 5 components because we assume that appr.')
disp(' 3 are reasonable but want to check that. We fit models from 1 to 5 ')
disp(' components and fit each three times. This way we can check that the')
disp(' is the same every time we fit, say a four-component model. If it is')
disp(' it is an indication that we have used too many components or that the')
disp('  data are difficult to fit')
disp(' ')
disp(' This process will take some time, but afterwards a plot is produced')
disp(' which is often very instructive to look at')
disp(' ')
disp(' ')
disp(' Press any key to continue')
disp(' ')
pause
close all
home

echo on
[ssX,Corco] = pftest(3,X,5,[0 0 0 0 NaN]);
echo off
disp(' ')
disp(' Indeed, the fit values seem to indicate that three components are ')
disp(' suitable, whereas the core consistency seems to point to a possible')
disp(' fourth component. This fourth component must be weak though considering')
disp(' the low increase in fit. If looking into the 3- and 4-component models')
disp(' it is realized that the fourth component is reflecting Rayleigh scatter.')
disp(' The problem could have been avoided if we remove emission below ')
disp(' excitation (by setting these elements to NaN), but this is not pursued')
disp(' here. Instead, we will fit a three-component parafac model.')
disp(' ')
disp(' We will turn the plotting on, so that a graphical output is produced')
disp(' ')
disp(' Press any key to continue')
pause
close all
home

echo on
model = parafac(X,3,[0 0 2]);
echo off

disp(' ')
disp(' Press any key to continue')
pause
close all
home

disp(' Using the function fac2let, we can get scores and loading out')
disp(' ')
echo on
[A,B,C] = fac2let(model);
echo off
disp(' ')
disp(' Scores')
disp(A)
disp(' ')
disp(' Concentrations')
disp(y)
disp(' ')
disp(readme)
disp(' ')

disp(' Scrutinize the scores and compare to the concentrations to see')
disp(' the relation between the two. Each score correspond to a specific')
disp(' component up to scale and permutation')
disp(' ')
disp(' END OF  PARADEMO')


function [p, s, a] = run_eval_template(nim, gtdir, esdir, sampledirs, gtsuffix, essuffix)

imlist = cell(1, nim);
count = 0;
psnrAll = zeros(nim,1);
ssimAll = zeros(nim,1);
absAll = zeros(nim,1);

for i = 1:length(imlist)
  imname = imlist{i};
  imgt = im2double(imread(fullfile(gtdir, sampledirs{i}, gtsuffix)));
  imes = im2double(imread(fullfile(esdir, sampledirs{i}, essuffix)));
  [h,w,c] = size(imgt);
  if c == 1
    tmp = zeros(h,w,3);
    tmp(:,:,1) = imgt;
    tmp(:,:,2) = imgt;
    tmp(:,:,3) = imgt;
    imgt = tmp;
  end
  [hs,ws,cs] = size(imes);
  if cs == 1
    tmps = zeros(hs,ws,3);
    tmps(:,:,1) = imes;
    tmps(:,:,2) = imes;
    tmps(:,:,3) = imes;
    imes = tmps;
  end
  imgtv = reshape(imgt, [h*w,3]);
  imesv = reshape(imes, [h*w,3]);
  psnrAll(i) = psnr(imes, imgt);
  ssimAll(i) = ssim(imes, imgt);
  absAll(i) = mean2(abs(imes - imgt));
  fprintf('%d/%d\n',i,nim);
end

%%
p = mean(psnrAll);
s = mean(ssimAll);
a = mean(absAll);
fprintf('Mean PSRN: %f\n', p);
fprintf('Mean SSIM: %f\n', s);
fprintf('Mean Abs: %f\n', a);


if ~exist('save_name'),
    save_name = 'Calib_Results';
end;

fprintf(1,'Generating the matlab script file %s.m containing the intrinsic and extrinsic parameters...\n',save_name)

fid = fopen([ save_name '.m'],'wt');

fprintf(fid,'%% Intrinsic and Extrinsic Camera Parameters\n');
fprintf(fid,'%%\n');
fprintf(fid,'%% This script file can be directly excecuted under Matlab to recover the camera intrinsic and extrinsic parameters.\n');
fprintf(fid,'%% IMPORTANT: This file contains neither the structure of the calibration objects nor the image coordinates of the calibration points.\n');
fprintf(fid,'%%            All those complementary variables are saved in the complete matlab data file Calib_Results.mat.\n');
fprintf(fid,'%% For more information regarding the calibration model visit http://www.vision.caltech.edu/bouguetj/calib_doc/\n');
fprintf(fid,'\n\n');
fprintf(fid,'%%-- Focal length:\n');
fprintf(fid,'fc = [ %5.15f ; %5.15f ];\n',fc);
fprintf(fid,'\n');
fprintf(fid,'%%-- Principal point:\n');
fprintf(fid,'cc = [ %5.15f ; %5.15f ];\n',cc);
fprintf(fid,'\n');
fprintf(fid,'%%-- Skew coefficient:\n');
fprintf(fid,'alpha_c = %5.15f;\n',alpha_c);
fprintf(fid,'\n');
fprintf(fid,'%%-- Distortion coefficients:\n');
fprintf(fid,'kc = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc);
fprintf(fid,'\n');
fprintf(fid,'%%-- Focal length uncertainty:\n');
fprintf(fid,'fc_error = [ %5.15f ; %5.15f ];\n',fc_error);
fprintf(fid,'\n');
fprintf(fid,'%%-- Principal point uncertainty:\n');
fprintf(fid,'cc_error = [ %5.15f ; %5.15f ];\n',cc_error);
fprintf(fid,'\n');
fprintf(fid,'%%-- Skew coefficient uncertainty:\n');
fprintf(fid,'alpha_c_error = %5.15f;\n',alpha_c_error);
fprintf(fid,'\n');
fprintf(fid,'%%-- Distortion coefficients uncertainty:\n');
fprintf(fid,'kc_error = [ %5.15f ; %5.15f ; %5.15f ; %5.15f ; %5.15f ];\n',kc_error);
fprintf(fid,'\n');
fprintf(fid,'%%-- Image size:\n');
fprintf(fid,'nx = %d;\n',nx);
fprintf(fid,'ny = %d;\n',ny);
fprintf(fid,'\n');
fprintf(fid,'\n');
fprintf(fid,'%%-- Various other variables (may be ignored if you do not use the Matlab Calibration Toolbox):\n');
fprintf(fid,'%%-- Those variables are used to control which intrinsic parameters should be optimized\n');
fprintf(fid,'\n');
fprintf(fid,'n_ima = %d;\t\t\t\t\t\t%% Number of calibration images\n',n_ima);
fprintf(fid,'est_fc = [ %d ; %d ];\t\t\t\t\t%% Estimation indicator of the two focal variables\n',est_fc);
fprintf(fid,'est_aspect_ratio = %d;\t\t\t\t%% Estimation indicator of the aspect ratio fc(2)/fc(1)\n',est_aspect_ratio);
fprintf(fid,'center_optim = %d;\t\t\t\t\t%% Estimation indicator of the principal point\n',center_optim);
fprintf(fid,'est_alpha = %d;\t\t\t\t\t\t%% Estimation indicator of the skew coefficient\n',est_alpha);
fprintf(fid,'est_dist = [ %d ; %d ; %d ; %d ; %d ];\t%% Estimation indicator of the distortion coefficients\n',est_dist);
fprintf(fid,'\n\n');
fprintf(fid,'%%-- Extrinsic parameters:\n');
fprintf(fid,'%%-- The rotation (omc_kk) and the translation (Tc_kk) vectors for every calibration image and their uncertainties\n');
fprintf(fid,'\n');
for kk = 1:n_ima,
    fprintf(fid,'%%-- Image #%d:\n',kk);
    eval(['omckk = omc_' num2str(kk) ';']);
    eval(['Tckk = Tc_' num2str(kk) ';']);
    fprintf(fid,'omc_%d = [ %d ; %d ; %d ];\n',kk,omckk);
    fprintf(fid,'Tc_%d  = [ %d ; %d ; %d ];\n',kk,Tckk);
    if (exist(['Tc_error_' num2str(kk)])==1) & (exist(['omc_error_' num2str(kk)])==1),
        eval(['omckk_error = omc_error_' num2str(kk) ';']);
        eval(['Tckk_error = Tc_error_' num2str(kk) ';']);
        fprintf(fid,'omc_error_%d = [ %d ; %d ; %d ];\n',kk,omckk_error);
        fprintf(fid,'Tc_error_%d  = [ %d ; %d ; %d ];\n',kk,Tckk_error);
    end;
    fprintf(fid,'\n');
end;

fclose(fid);


function z = isequal(x,y)
%ISEQUAL for sptensors.
%
%   ISEQUAL(A,B) compares the sparse tensors A and B for equality.
%
%   See also SPTENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Observations for sparse matrix case.
% The result of isequal(a,full(a)) is true!

%%
if ~isequal(x.size,y.size)
    z = false;
elseif isa(x,'sptensor') && isa(y,'sptensor')
    z = (nnz(x-y) == 0);
elseif isa(y,'tensor')
    z = isequal(full(x),y);
else
    z = false;
end


function [  ] = plotAxialSagittalCoronal( vol, scale_fig, title_fig )
%PLOTAXIALSAGITTALCORONAL Summary of this function goes here
%
%   Code refractored from Berkin Bilgic's scripts: "script_Laplacian_unwrap_Sharp_Fast_TV_gre3D.m"
%   and "script_Laplacian_unwrap_Sharp_Fast_TV_gre3D.m"
%   Original source: https://martinos.org/~berkin/software.html
%
%   Original reference:
%   Bilgic et al. (2014), Fast quantitative susceptibility mapping with
%   L1-regularization and automatic parameter selection. Magn. Reson. Med.,
%   72: 1444-1459. doi:10.1002/mrm.25029
im_size = round(size(vol)/2);

figure(), subplot(1,3,1), imagesc( vol(:,:,im_size(3)), scale_fig  ), axis square off, colormap gray, axis image
figure(get(gcf,'Number')), subplot(1,3,3), imagesc( imrotate(squeeze(vol(:,im_size(2),:)),90), scale_fig  ), axis square off, colormap gray, axis image
figure(get(gcf,'Number')), subplot(1,3,2), imagesc( imrotate(squeeze(vol(im_size(1),:,:)),90), scale_fig  ), axis square off, colormap gray, axis image

if nargin == 3
    title(title_fig)
end

drawnow

end


% MSH_REFINE: construct a refined mesh from a given one. The function only
%                refines the mesh, it does not refine any space.
%
%     msh_fine = msh_refine (msh, nsub);
%
% INPUTS:
%
%     msh:  an object of the msh_cartesian class (see msh_cartesian)
%     nsub: number of uniform subdivisions to apply on the elements, and for each direction
%
% OUTPUT:
%
%     msh_fine: an object of the class msh_cartesian (see msh_cartesian)
%
% Copyright (C) 2015 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function msh_fine = msh_refine (msh, nsub)

  rule = msh_gauss_nodes (msh.nqn_dir);
  [~, zeta] = kntrefine (msh.breaks, nsub-1, ones(1, msh.ndim), zeros(1, msh.ndim));
  [qn, qw] = msh_set_quad_nodes (zeta, rule);

  auxiliary_geometry.rdim = msh.rdim;
  auxiliary_geometry.map = msh.map;
  auxiliary_geometry.map_der = msh.map_der;
  if (isfield (struct (msh), 'map_der2'))
    auxiliary_geometry.map_der2 = msh.map_der2;
  end

  boundary = ~isempty (msh.boundary);
  if (msh.ndim > 1)
    bnd = [];
    for ii = 1:numel (msh.boundary)
      bnd(ii).rdim = msh.boundary(ii).rdim;
      bnd(ii).map = msh.boundary(ii).map;
      bnd(ii).map_der = msh.boundary(ii).map_der;
    end
    auxiliary_geometry.boundary = bnd;
  end
  msh_fine = msh_cartesian (zeta, qn, qw, auxiliary_geometry, 'boundary', boundary);

end


function varargout = dsxy2figxy(varargin)
% dsxy2figxy -- Transform point or position from axis to figure coords
% Transforms [axx axy] or [xypos] from axes hAx (data) coords into coords
% wrt GCF for placing annotation objects that use figure coords into data
% space. The annotation objects this can be used for are
%    arrow, doublearrow, textarrow
%    ellipses (coordinates must be transformed to [x, y, width, height])
% Note that line, text, and rectangle anno objects already are placed
% on a plot using axes coordinates and must be located within an axes.
% Usage: Compute a position and apply to an annotation, e.g.,
%   [axx axy] = ginput(2);
%   [figx figy] = getaxannopos(gca, axx, axy);
%   har = annotation('textarrow',figx,figy);
%   set(har,'String',['(' num2str(axx(2)) ',' num2str(axy(2)) ')'])

%% Obtain arguments (only limited argument checking is performed).
% Determine if axes handle is specified
if length(varargin{1})== 1 && ishandle(varargin{1}) && ...
  strcmp(get(varargin{1},'type'),'axes')
 hAx = varargin{1};
 varargin = varargin(2:end);
else
 hAx = gca;
end;
% Parse either a position vector or two 2-D point tuples
if length(varargin)==1 % Must be a 4-element POS vector
 pos = varargin{1};
else
 [x,y] = deal(varargin{:});  % Two tuples (start & end points)
end
%% Get limits
axun = get(hAx,'Units');
set(hAx,'Units','normalized');  % Need normaized units to do the xform
axpos = get(hAx,'Position');
axlim = axis(hAx);              % Get the axis limits [xlim ylim (zlim)]
axwidth = diff(axlim(1:2));
axheight = diff(axlim(3:4));
yisreversed = strcmpi(get(hAx,'ydir'),'reverse');
xisreversed = strcmpi(get(hAx,'xdir'),'reverse');

%% Transform data from figure space to data space
if exist('x','var')     % Transform a and return pair of points

  xout = (x-axlim(1))/axwidth;
  yout = (y-axlim(3))/axheight;
  if xisreversed,
    xout = 1 - xout;
  end
  if yisreversed
    yout = 1 - yout;
  end

  varargout{1} = xout*axpos(3) + axpos(1);
  varargout{2} = yout*axpos(4) + axpos(2);
else                    % Transform and return a position rectangle
  xout = (pos(1)-axlim(1))/axwidth;
  yout = (pos(2)-axlim(3))/axheight;
  if xisreversed,
    xout = 1 - xout;
  end
  if yisreversed,
    yout = 1 - yout;
  end
  pos(1) = xout*axpos(3) + axpos(1);
  pos(2) = yout*axpos(4) + axpos(2);
  pos(3) = pos(3)*axpos(3)/axwidth;
  pos(4) = pos(4)*axpos(4)/axheight;
  varargout{1} = pos;
end
%% Restore axes units
set(hAx,'Units',axun)


% Y = VEC(x)  Given an m x n matrix x, this produces the vector Y of length
%   m*n that contains the columns of the matrix x, stacked below each other.
%
% See also mat.

function x = vec(X)

%
% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA
%

[m n] = size(X);
x = reshape(X,m*n,1);


function RGB=showmask(V,M,display_flag);
% showmask(V,M);
%
% M is a nonneg. mask
% Jianbo Shi, 1997

V=V-min(V(:));
V=V/max(V(:));
V=.25+0.75*V; %brighten things up a bit

M=M-min(M(:));
M=M/max(M(:));

H=0.0+zeros(size(V));
S=M;
RGB=hsv2rgb(H,S,V);

%if nargin>2
   image(RGB)
   axis('image')
%end


 function x = eml_psca(x, G, yi, ci, ri, nsubiter)
%function x = eml_psca(x, G, yi, ci, ri, nsubiter)
% Runs one iteration of the ML-PSCA algorithm for emission Poisson problem
% (paraboloidal surrogates coordinate ascent)
% model: Y_i ~ Poisson(c_i [G x]_i + r_i)
% in
%	see em_fbp.m for model, G, yi, ci, ri
% out
%	x [np,1]	updated image
%
% This is for TESTING ONLY.
% IT IS NOT USEFUL because it is unregularized and slow
%
% Copyright Mar 2000, Jeff Fessler, The University of Michigan

if nargin < 3, ir_usage, end

[nb na] = size(yi);

if ~isvar('ci') || isempty(ci)
	ci = ones(size(yi));
end
if ~isvar('ri') || isempty(ri)
	ri = zeros(size(yi));
end
if ~isvar('nsubiter') || isempty(nsubiter)
	nsubiter = 1;
end

%
% compute surrogate curvatures
%
li0 = reshape(G * x(:), size(yi));		% l=G*x "line integrals"
yb = ci .* li0 + ri;				% predicted measurement means

doth = ci .* (yi ./ yb - 1);			% first derivative
ni = eml_curvature(yi, ci, ri, li0, yb, 'oc');	% curvatures
ni = ni(:);
%disp(range(ni)')

% backproject curvatures for denominator
dj = (G').^2 * ni(:);

%
% now we are maximizing the quadratic surrogate function:
% Q(x) == P(G*x ; G*x0)
%	where P(l;l0) = doth' (l-l0) - 1/2 (l-l0)' D(n) (l-l0)
% d/dli P(l;l0) = dothi - ni (li-l0i)
% Q(x) == doth' G (x-x0) - 1/2 (x-x0)' G' D(n) G (x-x0)
% ??? d/dx_j Q(x) = \sumi \gij \dothi - \sumi \gij \ni [G(x-x0)]_i
% d/dx_j Q(x) = \sumi \gij [d/dli P(l;l0)]
% -d^2/dx_j^2 Q(x) = \sumi \gij^2 \ni
%
dqi = doth(:);	% initial surrogate derivatives

%xs = zeros([length(x) nsubiter]);	% save and return the subiterations too

%x0 = x;		% save initial image

like0 = eql_obj(x, G, yi(:), ci(:), ri(:));

%
% loop over subiterations of paraboloid
%
for is=1:nsubiter

	%
	% loop over pixels
	%
	for jj=1:numel(x)
		g = G(:,jj);
		x0j = x(jj);
		x(jj) = x0j + (dqi' * g) / dj(jj);
		x(jj) = max(x(jj),0);
		dqi = dqi - ni .* (g * (x(jj) - x0j));

	end
%	xs(:,is) = x;
end


function varargout = expexpintinv(varargin)

switch class(varargin{1})

    case 'double'
        z = varargin{1};
        varargout{1} = exp(1./z).*expint(1./z);

    case 'sdpvar'
        varargout{1} = InstantiateElementWise(mfilename,varargin{:});

    case 'char'

        operator = CreateBasicOperator('concave','increasing','positive','callback');
        operator.derivative = @derivative;
        operator.range = [0 inf];
        operator.domain = [0 inf];

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error([upper(mfilename) ' called with weird argument']);
end

function d = derivative(z);
d = (-1./z.^2).*exp(1./z).*expint(1./z)+exp(1./z).*(-z.*exp(-1./z)).*(-1./z.^2);


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
% This is a testing environment for the files in the folder kernel/data
% 1. Based on data/contents, a list of required files is generated and it
%    is verified, that all files are present; additional files are listed.
% 2. All c-files are compiled.
% 3. All files are executed.
% 4. Check administrative modul distance.m
% 5. Test explicitly several specific distances
%==============================================================================

FAIRcheckFiles(mfilename);

%% Test module distance, start with syntax
distance('reset','distance','MIcc',...
  'tol',1e-7,'minT',0,'maxT',256,'nT',60,'minR',0,'maxR',256,'nR',60);
distance('disp');
[scheme,parameter] = distance
distance('clear');
distance('disp');

%% 5. setup test cases and run these
fprintf(' ... %s\n','generate hand data and setup transformation model:')
setup2DhandData
imgModel('reset','imgModel','splineInter','regularizer','moments','theta',1e0);
level = 4; omega = ML{level}.omega; m = ML{level}.m;
[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega(1,:),'out',0);
xc  = getCellCenteredGrid(omega(1,:),m);
Rc  = imgModel(R,omega(end,:),xc);

FAIRfigure(2);
subplot(1,2,1); viewImage2Dsc(T,omega,m,'title','template','colormap','gray')
subplot(1,2,2); viewImage2Dsc(R,omega,m,'title','reference','colormap','gray')

%% test several distances
distances = {'SSD','NCC','MIspline','MImex','NGFdot'};
clear fig; l = 0;

for k=1:length(distances);

  distance('reset','distance',distances{k});
  switch distance,
    case {'SSD','NCC'},
    case 'MIcc',
      %setup default parameter
      distance('set','tol',1e-7,...
        'minT',0,'maxT',256,'nT',60,'minR',0,'maxR',256,'nR',40);
      distance('disp');
    case 'NGFdot',
      %setup default parameter
      distance('set','edge',100);
      distance('disp');
  end;

  type = {'function','derivative','Gauss-Newton'};
  for k = 1:length(type),
    fctn = @(x) parseDistance(type{k},T,Rc,omega(end,:),m,x);
    l = l+1;
    fig(l)=checkDerivative(fctn,xc);
    ylabel([distance,'-',type{k}])
    builtin('pause',1);
  end;
end;

testEnd;
%==============================================================================


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  battistn@tcnj.edu
% Date Modified: April 2021
% Current Institution: TCNJ
% Date Created: May 27th, 2015
% Institution Created: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (torsional springs or non-invariant beams)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (battistn@tcnj.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the RUBBERBAND-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Rubberband(Nx,Ny)

%------------------------------------------------------------
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%------------------------------------------------------------
%Nx =  128;      % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
%Ny =  128;      % # of Eulerian Grid Pts. in y-Direction (MUST BE EVEN!!!)
Lx = 1.0;        % Length of Eulerian Grid in x-Direction
Ly = 1.0;        % Length of Eulerian Grid in y-Direction
dx=Lx/Nx;

%---------------------------------------------------------------
% Necessary grid info for ** Concentration ** input file
%  (doesn't need to be changed unless rectangular grid is used)
%---------------------------------------------------------------
x=0:dx:Lx-dx;
y=x;

%------------------------------------------------------------
% Immersed Structure Geometric / Dynamic Parameters %
%------------------------------------------------------------
a = 0.4;         % Length of semi-major axis.
b = 0.2;         % Length of semi-minor axis.
ds_Rest = 0;     % Resting length of springs

%------------------------------------------------------------
% Rubberband overall perimeter length and Lagrangian spacing
%------------------------------------------------------------
L=1.93768964;
ds=Lx/(2*Nx);
s = 0:ds:L;
N=length(s);

%------------------------------------------------------------
% Structure name
%------------------------------------------------------------
struct_name = 'rubberband'; % Name for .vertex, .spring, etc files.

%------------------------------------------------------------
% Call function to construct geometry
%------------------------------------------------------------
[xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(N,a,b);

%------------------------------------------------------------
% Plot Geometry to test
%------------------------------------------------------------
plot(xLag,yLag,'r-'); hold on;
plot(xLag,yLag,'*'); hold on;
xlabel('x'); ylabel('y');
axis square;

%------------------------------------------------------------
% Prints .vertex file!
%------------------------------------------------------------
print_Lagrangian_Vertices(xLag,yLag,struct_name);

%------------------------------------------------
% Prints .spring file!
%       ->> find k_Spring for certain resolution
%------------------------------------------------
ds128 = 0.5*Lx/128;   % Lag. Spacing in 128x128 Resolution
F_Spr_128 = 4e5;      % Force used in 128x128 Resolution
%
% Get Spring Stiffness Coefficient void of any particular resolution
k_Spring = F_Spr_128 * ds128^3 / ds128;
%
% Get Spring Force based on particular resolution
F_Spring = k_Spring * ds / ds^3;
%
print_Lagrangian_Springs(xLag,yLag,F_Spring,ds_Rest,struct_name);

%------------------------------------------------------------
% Call function for initial concentration
%------------------------------------------------------------
[Concentration,X,Y]= give_Me_Initial_Concentration(Lx,Ly,Nx,Ny,dx,x,y);

%------------------------------------------------------------
% Prints .concentration file!
%------------------------------------------------------------
print_Concentration_Info(Nx,Nx,Concentration,struct_name);

%------------------------------------------------------------
% Prints .geo_connect file!
%------------------------------------------------------------
print_Geometry_Connections(xLag,struct_name);

% Prints .beam file!
%k_Beam = 0.5; C = 0.0;
%print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name);

% Prints .target file!
%k_Target = 1e7;
%print_Lagrangian_Target_Pts(xLag,k_Target,struct_name);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called rubberband.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints GEOMETRY CONNECTIONS to a file called
%                               'struct_name'.geo_connect
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Geometry_Connections(xLag,struct_name)

    N = length(xLag); % Total # of Lag. Pts

    geo_fid = fopen([struct_name '.geo_connect'], 'w');

    % Loops over all Lagrangian Pts.
    for i = 1:N

        if i<N

            s1 = i;
            s2 = i+1;
            fprintf(geo_fid, '%d %d\n', s1, s2);
            fprintf(geo_fid, '%d %d\n', s2, s1);

        elseif i==N

            s1 = N;
            s2 = 1;
            fprintf(geo_fid, '%d %d\n', s1, s2);
            fprintf(geo_fid, '%d %d\n', s2, s1);

        end

    end

    fclose(geo_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints TARGET points to a file called rubberband.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name)

    N = length(xLag);

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (Torsional Spring) points to a file called rubberband.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

    N = length(xLag); % NOTE: Total number of beams = Number of Total Lag Pts. - 2

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %BEAMS BETWEEN VERTICES
    for s = 2:N-1
            if  s <= N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C);
            else
                %Case s=N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 1,   k_Beam, C);
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called rubberband.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Rest,struct_name)

    N = length(xLag);

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %SPRINGS BETWEEN VERTICES
    for s = 1:N
            if s < N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            else
                %Case s=N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, 1,   k_Spring, ds_Rest);
            end
    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints initial CONCENTRATION to rubberband.concentration
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Concentration_Info(Nx,Ny,C,struct_name)

    con_fid = fopen([struct_name '.concentration'], 'w');

    for i=1:Ny
        for j=1:Nx
            fprintf(con_fid, '%1.16e ', C(i,j) );
        end
        fprintf(con_fid,'\n');
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry(N,rmin,rmax)

% The immsersed structure is an ellipse %
for i=1:N

    xLag(i) = 0.5 + rmax * cos( 2*pi/N*(i-1) );
    yLag(i) = 0.5 + rmin * sin( 2*pi/N*(i-1) );

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: gives initial concentration in Eulerian domain
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [C,X,Y] = give_Me_Initial_Concentration(Lx,Ly,Nx,Ny,dx,x,y)

[X,Y]=meshgrid(x,y);

C=zeros(Nx,Nx);


   function y = autopilot_quad(drone, t)
    % AUTOPILOT_QUAD - autopilot used for the quadcopter

    p_drone = drone.p_drone;
    p_sim = drone.p_sim;
    autopilot_version = drone.autopilot_version;

    % State variables
    pn = drone.z_hat(1); % position north
    pe = drone.z_hat(2); % position east
    h = drone.z_hat(3); % altitude
    vx = drone.z_hat(4); % x velocity
    vy = drone.z_hat(5); % y velocity
    vz = drone.z_hat(6); % z velocity
    Va = drone.z_hat(7); % airspeed
    phi = drone.z_hat(10); % roll angle
    theta = drone.z_hat(11); % pitch angle
    psi = drone.z_hat(12); % yaw angle
    p = drone.z_hat(14); % body frame roll rate
    q = drone.z_hat(15); % body frame pitch rate
    r = drone.z_hat(16); % body frame yaw rate

    %%%%% Autopilot version %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % autopilot_version == 1 <- attitude controller
    % autopilot_version == 2 <- speed controller
    % autopilot_version == 3 <- acceleration controller
    % TO IMPLEMENT: autopilot_version == 4 <- position controller

    % Commanded values
    if autopilot_version == 1% attitude autopilot
        h_c = drone.command(1); % commanded altitude        [m]
        phi_c = drone.command(2); % commanded roll          [rad]
        theta_c = drone.command(3); % commanded pitch       [rad]
        psi_c = drone.command(4); % commanded yaw           [rad]

    elseif autopilot_version == 2% velocity autopilot
        psi_c = drone.command(1); % commanded yaw            [rad]
        vn_c = drone.command(2); % commanded north velocity  [m/s]
        ve_c = drone.command(3); % commanded east velocity   [m/s]
        vd_c = drone.command(4); % commanded down velocity   [m/s]

    elseif autopilot_version == 3% acceleration autopilot
        psi_c = drone.command(1); % commanded yaw            [rad]
        an_c = drone.command(2); % commanded north accell    [m/s^2]
        ae_c = drone.command(3); % commanded east accell     [m/s^2]
        ad_c = drone.command(4); % commanded down accell     [m/s^2]

    else %if autopilot_version == 4% position autopilot
        psi_c = drone.command(1); % commanded yaw            [rad]
        pn_c = drone.command(2); % commanded north position  [m]
        pe_c = drone.command(3); % commanded east position   [m]
        pd_c = drone.command(4); % commanded down position   [m]
    end

    %Forces
    F_xyz = drone.forces;

    a_ned = rotate_b2i(F_xyz, phi, theta, psi) / p_drone.mass;
    an = a_ned(1);
    ae = a_ned(2);
    ad = a_ned(3);

    switch autopilot_version
        case 1
            [delta, x_command] = autopilot_attitude(h_c, phi_c, theta_c, psi_c, ...
                h, phi, theta, psi, p, q, r, t, p_drone, p_sim, drone);

        case 2
            [delta, x_command] = autopilot_velocity(psi_c, vn_c, ve_c, vd_c, ...
                vx, vy, vz, phi, theta, psi, ...
                p, q, r, h, t, p_drone, p_sim, drone);
        case 3
            [delta, x_command] = autopilot_acceleration(psi_c, an_c, ae_c, ad_c, ...
                an, ae, ad, phi, theta, psi, ...
                p, q, r, h, t, p_drone, p_sim, drone);
        case 4
            [delta, x_command] = autopilot_position(psi_c, pn_c, pe_c, pd_c, ...
                vx, vy, vz, phi, theta, psi, ...
                p, q, r, h, pn, pe, t, p_drone, p_sim, drone);
    end

    y = [delta; x_command];

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%           Autopilot versions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [delta, x_command] = autopilot_attitude(h_c, phi_c, theta_c, ...
        psi_c, h, phi, theta, psi, p, q, r, t, p_drone, p_sim, drone)
% AUTOPILOT_ATTITUDE

    if h <= p_drone.altitude_take_off_zone
        drone.altitude_state = 2;
    else
        drone.altitude_state = 2;
    end

    if t == 0 || (drone.altitude_state ~= drone.altitude_state_prev)
        drone.initialize_integrator = 1;
    else
        drone.initialize_integrator = 0;
    end

    % Implement state machine
    switch drone.altitude_state
        case 1% in take-off zone
            d1 = 1;
            d2 = 1;
            d3 = 1;
            d4 = 1;

        case 2% outside take-off zone
            [thrust, drone.P_h_thrust] = h_thrust_hold(h_c, h, ...
                drone.initialize_integrator, p_drone, p_sim, drone.P_h_thrust);
            [torque_phi, drone.P_roll_torque] = roll_torque_hold(phi_c, ...
                phi, p / 180 * pi, drone.initialize_integrator, p_drone, p_sim, ...
                drone.P_roll_torque);
            [torque_theta, drone.P_pitch_torque] = pitch_torque_hold(...
                theta_c, theta, q / 180 * pi, ...
                drone.initialize_integrator, p_drone, p_sim, drone.P_pitch_torque);
            [torque_psi, drone.P_psi_torque] = yaw_torque_hold(psi_c, ...
                psi, r / 180 * pi, drone.initialize_integrator, p_drone, p_sim, ...
                drone.P_psi_torque);

            [d1, d2, d3, d4] = mixer(thrust, torque_phi, torque_theta, ...
                torque_psi, p_drone, p_sim);

            % artificially saturation delta_t
            d1 = saturate(d1, p_drone.dt_max, 0);
            d2 = saturate(d2, p_drone.dt_max, 0);
            d3 = saturate(d3, p_drone.dt_max, 0);
            d4 = saturate(d4, p_drone.dt_max, 0);
    end

    % Control outputs
    delta = [d1; d2; d3; d4];

    % Commanded (desired) states
    x_command = [...
                0; ...% pn
            0; ...% pe
            h_c; ...% h
            0; ...% vn
            0; ...% ve
            0; ...% vd
            0; % an
            0; % ae
            0; % ad
            0; ...% Va
            0; ...% alpha
            0; ...% beta
            phi_c; ...% phi
            theta_c; % theta
            psi_c; ...% psi
            0; % chi
            0; ...% p
            0; ...% q
            0; ...% r
            ];

    y = [delta; x_command];
    drone.altitude_state_prev = drone.altitude_state;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [delta, x_command] = autopilot_velocity(psi_c, vn_c, ve_c, ...
        vd_c, vx, vy, vz, phi, theta, psi, p, q, r, h, t, p_drone, p_sim, drone)
% AUTOPILOT_VELOCITY

    Rbi = Rb2i(phi, theta, psi);
    v_ned = Rbi * [vx vy vz]';
    vn = v_ned(1);
    ve = v_ned(2);
    vd = v_ned(3);

    if h <= p_drone.altitude_take_off_zone
        drone.altitude_state = 2;
    else
        drone.altitude_state = 2;
    end

    if t == 0 || (drone.altitude_state ~= drone.altitude_state_prev)
        drone.initialize_integrator = 1;
    else
        drone.initialize_integrator = 0;
    end

    % Implement state machine
    switch drone.altitude_state
        case 1% in take-off zone
            d1 = 1;
            d2 = 1;
            d3 = 1;
            d4 = 1;
        case 2% outside take-off zone
            [thrust, drone.P_vd_thrust] = vd_thrust_hold(vd_c, vd, ...
                drone.initialize_integrator, p_drone, p_sim, drone.P_vd_thrust);
            [phi_c, drone.P_ve_roll] = ve_roll_hold(ve_c, ve, ...
                drone.initialize_integrator, p_drone, p_sim, drone.P_ve_roll);
            [torque_phi, drone.P_roll_torque] = roll_torque_hold(phi_c, ...
                phi, p / 180 * pi, drone.initialize_integrator, p_drone, p_sim, ...
                drone.P_roll_torque);
            [theta_c, drone.P_vn_pitch] = vn_pitch_hold(vn_c, vn, ...
                drone.initialize_integrator, p_drone, p_sim, drone.P_vn_pitch);
            [torque_theta, drone.P_pitch_torque] = pitch_torque_hold(...
                theta_c, theta, q / 180 * pi, ...
                drone.initialize_integrator, p_drone, p_sim, drone.P_pitch_torque);
            [torque_psi, drone.P_psi_torque] = yaw_torque_hold(psi_c, ...
                psi, r / 180 * pi, drone.initialize_integrator, p_drone, p_sim,...
                drone.P_psi_torque);

            [d1, d2, d3, d4] = mixer(thrust, torque_phi, torque_theta, ...
                torque_psi, p_drone, p_sim);

            % artificially saturation delta_t
            d1 = saturate(d1, p_drone.dt_max, 0);
            d2 = saturate(d2, p_drone.dt_max, 0);
            d3 = saturate(d3, p_drone.dt_max, 0);
            d4 = saturate(d4, p_drone.dt_max, 0);
    end

    % Control outputs
    delta = [d1; d2; d3; d4];

    % Commanded (desired) states
    x_command = [...
                0; ...% pn
            0; ...% pe
            0; ...% h
            vn_c; ...% vn
            ve_c; ...% ve
            vd_c; ...% vd
            0; % an
            0; % ae
            0; % ad
            0; ...% Va
            0; ...% alpha
            0; ...% beta
            phi_c; ...% phi
            theta_c; % theta
            psi_c; ...% psi
            0; % chi
            0; ...% p
            0; ...% q
            0; ...% r
            ];

    y = [delta; x_command];
    drone.altitude_state_prev = drone.altitude_state;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% // TODO: Check it
function [delta, x_command] = autopilot_acceleration(psi_c, an_c, ...
        ae_c, ad_c, an, ae, ad, phi, theta, psi, p, q, r, h, t, p_drone, p_sim, drone)
% AUTOPILOT_ACCELERATION

    if h <= p_drone.altitude_take_off_zone
        drone.altitude_state = 2;
    else
        drone.altitude_state = 2;
    end

    if t == 0 || (drone.altitude_state ~= drone.altitude_state_prev)
        drone.initialize_integrator = 1;
    else
        drone.initialize_integrator = 0;
    end

    % Implement state machine
    switch drone.altitude_state
        case 1% in take-off zone
            d1 = 1;
            d2 = 1;
            d3 = 1;
            d4 = 1;

        case 2% outside take-off zone
            [thrust, drone.P_ad_thrust] = ad_thrust_hold(ad_c, ad, ...
                drone.initialize_integrator, p_drone, drone.P_ad_thrust);
            %[thrust, drone.P_h_thrust] = h_thrust_hold(15, h, ...
                % drone.initialize_integrator, p_drone, drone.P_h_thrust);
            [phi_c, drone.P_ae_roll] = ae_roll_hold(ae_c, ae, ...
                drone.initialize_integrator, p_drone, drone.P_ae_roll);
            [torque_phi, drone.P_roll_torque] = roll_torque_hold(phi_c, ...
                phi, deg2rad(p), drone.initialize_integrator, p_drone, p_sim, ...
                drone.P_roll_torque);
            [theta_c, drone.P_an_pitch] = an_pitch_hold(an_c, an, ...
                drone.initialize_integrator, p_drone, drone.P_an_pitch);
            [torque_theta, drone.P_pitch_torque] = pitch_torque_hold(...
                theta_c, theta, deg2rad(q), ...
                drone.initialize_integrator, p_drone, drone.P_pitch_torque);
            [torque_psi, drone.P_psi_torque] = yaw_torque_hold(psi_c, ...
                psi, deg2rad(r), drone.initialize_integrator, p_drone, p_sim,  ...
                drone.P_psi_torque);

            [d1, d2, d3, d4] = mixer(thrust, torque_phi, torque_theta, ...
                torque_psi, p_drone, p_sim);

            % artificially saturation delta_t
            d1 = saturate(d1, p_drone.dt_max, 0);
            d2 = saturate(d2, p_drone.dt_max, 0);
            d3 = saturate(d3, p_drone.dt_max, 0);
            d4 = saturate(d4, p_drone.dt_max, 0);
    end

    % Control outputs
    delta = [d1; d2; d3; d4];

    % Commanded (desired) states
    x_command = [...
                0; ...% pn
            0; ...% pe
            0; ...% h
            0; ...% vn
            0; ...% ve
            0; ...% vd
            an_c;
            ae_c;
            ad_c;
            0; ...% Va
            0; ...% alpha
            0; ...% beta
            phi_c; ...% phi
            theta_c; % theta
            psi_c; ...% psi
            0; % chi
            0; ...% p
            0; ...% q
            0; ...% r
            ];

    y = [delta; x_command];
    drone.altitude_state_prev = drone.altitude_state;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [delta, x_command] = autopilot_position(psi_c, pn_c, pe_c, ...
        pd_c, vx, vy, vz, phi, theta, psi, p, q, r, h, pn, pe, t, p_drone, p_sim, drone)
% AUTOPILOT_POSITION
% // TODO: Check it
    Rbi = Rb2i(phi, theta, psi);
    v_ned = Rbi * [vx vy vz]';
    vn = v_ned(1);
    ve = v_ned(2);
    vd = v_ned(3);

    if h <= p_drone.altitude_take_off_zone
        drone.altitude_state = 2;
    else
        drone.altitude_state = 2;
    end

    if t == 0 || (drone.altitude_state ~= drone.altitude_state_prev)
        drone.initialize_integrator = 1;
    else
        drone.initialize_integrator = 0;
    end

    % Implement state machine
    switch drone.altitude_state
        case 1% in take-off zone
            d1 = 1;
            d2 = 1;
            d3 = 1;
            d4 = 1;
        case 2% outside take-off zone
            [vd_c, drone.P_pd_thrust] = pd_vd_hold(pd_c, h, ...
                drone.initialize_integrator, p_drone, drone.P_pd_thrust);
            [thrust, drone.P_vd_thrust] = vd_thrust_hold(vd_c, vd, ...
                drone.initialize_integrator, p_drone, p_sim, drone.P_vd_thrust);
            [ve_c, drone.P_pe_roll] = pe_ve_hold(pe_c, pe, ...
                drone.initialize_integrator, p_drone, drone.P_pe_thrust);
            [phi_c, drone.P_ve_roll] = ve_roll_hold(ve_c, ve, ...
                drone.initialize_integrator, p_drone, p_sim, drone.P_ve_roll);
            [torque_phi, drone.P_roll_torque] = roll_torque_hold(phi_c, ...
                phi, deg2rad(p), drone.initialize_integrator, p_drone, p_sim, ...
                drone.P_roll_torque);
            [vn_c, drone.P_pn_pitch] = pn_vn_hold(pn_c, pn, ...
                drone.initialize_integrator, p_drone, drone.P_pn_pitch);
            [theta_c, drone.P_vn_pitch] = vn_pitch_hold(vn_c, vn, ...
                drone.initialize_integrator, p_drone, p_sim, drone.P_vn_pitch);
            [torque_theta, drone.P_pitch_torque] = pitch_torque_hold(...
                theta_c, theta, deg2rad(q), ...
                drone.initialize_integrator, p_drone, drone.P_pitch_torque);
            [torque_psi, drone.P_psi_torque] = yaw_torque_hold(psi_c, ...
                psi, deg2rad(r), drone.initialize_integrator, p_drone, p_sim, ...
                drone.P_psi_torque);

            [d1, d2, d3, d4] = mixer(thrust, torque_phi, torque_theta, ...
                torque_psi, p_drone, p_sim);

            % artificially saturation delta_t
            d1 = saturate(d1, p_drone.dt_max, 0);
            d2 = saturate(d2, p_drone.dt_max, 0);
            d3 = saturate(d3, p_drone.dt_max, 0);
            d4 = saturate(d4, p_drone.dt_max, 0);
    end

    % Control outputs
    delta = [d1; d2; d3; d4];

    % Commanded (desired) states
    x_command = [...
                pn_c; ...% pn
            pe_c; ...% pe
            pd_c; ...% h
            vn_c; ...% vn
            ve_c; ...% ve
            vd_c; ...% vd
            0; % an
            0; % ae
            0; % ad
            0; ...% Va
            0; ...% alpha
            0; ...% beta
            phi_c; ...% phi
            theta_c; % theta
            psi_c; ...% psi
            0; % chi
            0; ...% p
            0; ...% q
            0; ...% r
            ];

    y = [delta; x_command];
    drone.altitude_state_prev = drone.altitude_state;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Autopilot functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [vd_c, P_pd_thrust] = pd_vd_hold(pd_c, pd, reinit, p_drone, p_sim, P_pd_thrust)

    if reinit || isempty(P_pd_thrust)
        P_pd_thrust.int = 0;
        P_pd_thrust.diff = 0;
        P_pd_thrust.error_prev = 0;
        P_pd_thrust.y_c_prev = 0;
    end

    [vd_c, P_pd_thrust] = pid_loop(pd_c, pd, Inf, p_drone.kp_vd, p_drone.ki_vd, p_drone.kd_vd, ...
        p_drone.vd_max, p_sim.dt, 0.1, P_pd_thrust);
end

function [ve_c, P_pe_roll] = pe_ve_hold(pe_c, pe, reinit, p_drone, P_pe_roll)

    if reinit || isempty(P_pe_roll)
        P_pe_roll.int = 0;
        P_pe_roll.diff = 0;
        P_pe_roll.error_prev = 0;
        P_pe_roll.y_c_prev = 0;
    end

    [ve_c, P_pe_roll] = pid_loop(pe_c, pe, Inf, p_drone.kp_ve, p_drone.ki_ve, p_drone.kd_ve, ...
        p_drone.v_max, p_sim.dt, 0.1, P_pe_roll);
end

function [vn_c, P_pn_pitch] = pn_vn_hold(pn_c, pn, reinit, p_drone, p_sim, P_pn_pitch)

    if reinit || isempty(P_pn_pitch)
        P_pn_pitch.int = 0;
        P_pn_pitch.diff = 0;
        P_pn_pitch.error_prev = 0;
        P_pn_pitch.y_c_prev = 0;
    end

    [vn_c, P_pn_pitch] = pid_loop(pn_c, pn, Inf, p_drone.kp_vn, p_drone.ki_vn, p_drone.kd_vn, ...
        p_drone.v_max, p_sim.dt, 0.1, P_pn_pitch);
end

function [thrust, P_h_thrust] = h_thrust_hold(h_c, h, reinit, p_drone, p_sim, P_h_thrust)

    if reinit || isempty(P_h_thrust)
        P_h_thrust.int = 0;
        P_h_thrust.diff = 0;
        P_h_thrust.error_prev = 0;
        P_h_thrust.y_c_prev = 0;
    end

    [thrust, P_h_thrust] = pid_loop(h_c, h, Inf, p_drone.kp_h, p_drone.ki_h, p_drone.kd_h, ...
        p_drone.thrust_max, p_sim.dt, 0.1, P_h_thrust);
end

function [torque_phi, P_roll_torque] = roll_torque_hold(phi_c, phi, p, ...
        reinit, p_drone, p_sim, P_roll_torque)

    if reinit || isempty(P_roll_torque)
        P_roll_torque.int = 0;
        P_roll_torque.diff = 0;
        P_roll_torque.error_prev = 0;
        P_roll_torque.y_c_prev = 0;
    end

    [torque_phi, P_roll_torque] = pid_loop(phi_c, phi, p, p_drone.kp_phi, 0, ...
        p_drone.kd_phi, p_drone.torque_max, p_sim.dt, 0.1, P_roll_torque);
end

function [torque_theta, P_pitch_torque] = pitch_torque_hold(theta_c, ...
        theta, q, reinit, p_drone, p_sim, P_pitch_torque)

    if reinit || isempty(P_pitch_torque)
        P_pitch_torque.int = 0;
        P_pitch_torque.diff = 0;
        P_pitch_torque.error_prev = 0;
        P_pitch_torque.y_c_prev = 0;
    end

    [torque_theta, P_pitch_torque] = pid_loop(theta_c, theta, q, ...
        p_drone.kp_theta, 0, p_drone.kd_theta, p_drone.torque_max, p_sim.dt, 0.1, ...
        P_pitch_torque);
end

function [torque_psi, P_psi_torque] = yaw_torque_hold(psi_c, psi, r, ...
        reinit, p_drone, p_sim, P_psi_torque)

    if reinit || isempty(P_psi_torque)
        P_psi_torque.int = 0;
        P_psi_torque.diff = 0;
        P_psi_torque.error_prev = 0;
        P_psi_torque.y_c_prev = 0;
    end

    [torque_psi, P_psi_torque] = pid_loop(psi_c, psi, r, p_drone.kp_psi, 0, ...
        p_drone.kd_psi, p_drone.torque_max, p_sim.dt, 0.1, P_psi_torque);
end

function [theta_c, P_vn_pitch] = vn_pitch_hold(vn_c, vn, reinit, p_drone, p_sim, ...
        P_vn_pitch)

    if reinit || isempty(P_vn_pitch)
        P_vn_pitch.int = 0;
        P_vn_pitch.diff = 0;
        P_vn_pitch.error_prev = 0;
        P_vn_pitch.y_c_prev = 0;
    end

    [theta_c, P_vn_pitch] = pid_loop(vn_c, vn, Inf, p_drone.kp_vn, p_drone.ki_vn, 0, ...
        p_drone.theta_max, p_sim.dt, 0.1, P_vn_pitch);
end

function [phi_c, P_ve_roll] = ve_roll_hold(ve_c, ve, reinit, p_drone, p_sim, P_ve_roll)

    if reinit || isempty(P_ve_roll)
        P_ve_roll.int = 0;
        P_ve_roll.diff = 0;
        P_ve_roll.error_prev = 0;
        P_ve_roll.y_c_prev = 0;
    end

    [phi_c, P_ve_roll] = pid_loop(ve_c, ve, Inf, p_drone.kp_ve, p_drone.ki_ve, 0, ...
        p_drone.phi_max, p_sim.dt, 0.1, P_ve_roll);
end

function [thrust, P_vd_thrust] = vd_thrust_hold(vd_c, vd, reinit, p_drone, p_sim, ...
        P_vd_thrust)

    if reinit || isempty(P_vd_thrust)
        % P_vd_thrust.int = -0.5;
        P_vd_thrust.int = 0;
        P_vd_thrust.diff = 0;
        P_vd_thrust.error_prev = 0;
        P_vd_thrust.y_c_prev = 0;
    end

    [thrust, P_vd_thrust] = pid_loop(vd_c, vd, Inf, p_drone.kp_vd, p_drone.ki_vd, 0, ...
        p_drone.thrust_max, p_sim.dt, 0.1, P_vd_thrust);
end

function [thrust, P_ad_thrust] = ad_thrust_hold(ad_c, ad, reinit, p_drone, p_sim, ...
        P_ad_thrust)

    if reinit || isempty(P_ad_thrust)
        P_ad_thrust.int = 0;
        P_ad_thrust.diff = 0;
        P_ad_thrust.error_prev = 0;
        P_ad_thrust.y_c_prev = 0;
    end

    [thrust, P_ad_thrust] = pid_loop(ad_c, ad, Inf, p_drone.kp_ad, p_drone.ki_ad, 0, ...
        p_drone.thrust_max, p_sim.dt, 0.1, P_ad_thrust);
end

function [theta_c, P_an_pitch] = an_pitch_hold(an_c, an, reinit, p_drone, ...
        P_an_pitch)

    if reinit || isempty(P_an_pitch)
        P_an_pitch.int = 0;
        P_an_pitch.diff = 0;
        P_an_pitch.error_prev = 0;
        P_an_pitch.y_c_prev = 0;
    end

    [theta_c, P_an_pitch] = pid_loop(an_c, an, Inf, p_drone.kp_an, p_drone.ki_an, 0, ...
        p_drone.theta_max, p_sim.dt, 0.1, P_an_pitch);
end

function [phi_c, P_ae_roll] = ae_roll_hold(ae_c, ae, reinit, p_drone, P_ae_roll)

    if reinit || isempty(P_ae_roll)
        P_ae_roll.int = 0;
        P_ae_roll.diff = 0;
        P_ae_roll.error_prev = 0;
        P_ae_roll.y_c_prev = 0;
    end

    [phi_c, P_ae_roll] = pid_loop(ae_c, ae, Inf, p_drone.kp_ae, p_drone.ki_ae, 0, ...
        p_drone.phi_max, p_sim.dt, 0.1, P_ae_roll);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Mixer
%     - mixes the effects of thrust and torques
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [d1, d2, d3, d4] = mixer(thrust, torque_phi, torque_theta, ...
        torque_psi, p_drone, p_sim)

    omega1_sqd = (thrust / (4 * p_drone.C_prop) + ...
        torque_theta / (2 * p_drone.C_prop * p_drone.l_arm) - ...
        torque_psi / (4 * p_drone.CD));

    if omega1_sqd > 0
        d1 = sqrt(omega1_sqd) / p_drone.k_omega;
    else
        d1 = 0;
    end

    omega2_sqd = (thrust / (4 * p_drone.C_prop) - ...
        torque_phi / (2 * p_drone.C_prop * p_drone.l_arm) + ...
        torque_psi / (4 * p_drone.CD));

    if omega2_sqd > 0
        d2 = sqrt(omega2_sqd) / p_drone.k_omega;
    else
        d2 = 0;
    end

    omega3_sqd = (thrust / (4 * p_drone.C_prop) - ...
        torque_theta / (2 * p_drone.C_prop * p_drone.l_arm) - ...
        torque_psi / (4 * p_drone.CD));

    if omega3_sqd > 0
        d3 = sqrt(omega3_sqd) / p_drone.k_omega;
    else
        d3 = 0;
    end

    omega4_sqd = (thrust / (4 * p_drone.C_prop) + ...
        torque_phi / (2 * p_drone.C_prop * p_drone.l_arm) + ...
        torque_psi / (4 * p_drone.CD));

    if omega4_sqd > 0
        d4 = sqrt(omega4_sqd) / p_drone.k_omega;
    else
        d4 = 0;
    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Saturate
%       - saturation function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function out = saturate(in, up_limit, low_limit)

    if in > up_limit
        out = up_limit;
    elseif in < low_limit
        out = low_limit;
    else
        out = in;
    end

end