function [M0,M1,L,M2] = spm_mfa_bi_multi(S,C)
% Bilinear form for multiple Gibb's ensembles
% FORMAT [M0,M1,L,M2] = spm_mfa_bi_multi(S,C)
%--------------------------------------------------------------------------
% S(s)   - MFA system specification structure for s ensembles
% Required fields:
%     S(i).M:  [1x1 struct]    - dynamic model structure
%     S(i).J0: [n x n double]  - Jacobian
%     S(i).J1: {1 x M.m cell}  - dJ0/du
%     S(i).L : [l x n double]  - d<y>/dp
%     S(i).u:  [n x m double]  - probability modes
%     S(i).v:  [m x n double]  - v*u = 1
%     S(i).X:  [n x d double]  - evaluation points of state [d]-space
%     S(i).x:  {1 x d cell}    - range of state [d]-space
%     S(i).p0: [n x 1 sparse]  - expansion point
%
% C{s x s cell} - coupling cell = dP/d<y> (change in parameters of S(i).M with
%                                   mean outputs of S(j).M - [p x l double])
%
% M0  [ns + 1 x ns + 1double]  - 1st order Bilinear matrix dq/dt;
% M1  {M.m x s}                - 2nd order Bilinear matrix dM0/du
% M2  {s x s}                  - 2nd order Bilinear matrix dM0/dC
% L   [1s x ns + 1 double]     - output matrix          <y> = L*q;
%
% Transformed probability states:  q = [1; v*(p(X) - p0)];
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_mfa_bi_multi.m 4936 2012-09-18 19:47:55Z karl $

% indices
%--------------------------------------------------------------------------
s     = length(S);                      % number of ensembles
m     = length(S(1).J1);                % number of inputs
I     = {(1:size(S(1).v,1)) + 1};       % ensemble indices
for i = 2:s
    I{i} = (1:size(S(i).v,1)) + I{i - 1}(end);
end
q     = I{s}(end);
M0    = sparse(q,q);
M1    = {};
L     = sparse(0,q);

% compute M0 = df/dx. M1 = d(df/dx)/du amd L = dy/dx
%--------------------------------------------------------------------------
for i = 1:s

    % Ist order bilinear operator M0
    %======================================================================
    M0(I{i},1)       = S(i).v*S(i).J0*S(i).p0;
    M0(I{i},I{i})    = S(i).v*S(i).J0*S(i).u;

    % 2nd order bilinear operators M1{i} - dM0/du
    %======================================================================
    for j = 1:m
        M            = sparse(q,q);
        M(I{i},1)    = S(i).v*S(i).J1{j}*S(i).p0;
        M(I{i},I{i}) = S(i).v*S(i).J1{j}*S(i).u;
        M1{m,i}      = M;
    end

    % output matrix <y>
    %======================================================================
    j         = (1:size(S(i).L,1)) + size(L,1);
    L(j,1)    = S(i).L*S(i).p0;
    L(j,I{i}) = S(i).L*S(i).u;

end

% return unless coupling matrices are required
%--------------------------------------------------------------------------
if nargout < 4
    return
end

% 2nd order bilinear operators M2{i} - dM0/dC
%==========================================================================
M2    = cell(s,s);
dP    = 1e-1;
for i = 1:s

    % target ensemble df/dP = dJ/dP*p0
    %----------------------------------------------------------------------
    M     = S(i).M;
    p     = length(M.pE);
    for k = 1:p
        M.pE      = S(i).M.pE;
        M.pE(k)   = M.pE(k) + dP;
        dJdP      = (spm_mfa(M,S(i).x) - S(i).J0)/dP;
        dfdP(:,k) = S(i).v*dJdP*S(i).p0;
    end

    for j = 1:s

        % source ensemble dP/dx = dP/d<y>*d<y>/dx = C*L
        %------------------------------------------------------------------
        dfdx      = sparse(q,q);
        if ~isempty(C{i,j})
            dPdx            = C{i,j}*S(j).L*S(j).u;
            dfdx(I{i},I{j}) = dfdP*dPdx;
        end

        % M2 = df/dx = df/dP*dP/dx
        %------------------------------------------------------------------
        M2{i,j} = dfdx;

    end
end


function Lmk = lmkJacobians(Lmk)

% LMKJACOBIANS Compute Jacobians for projection onto the manifold.
%   LMKJACOBIANS computes all the Jacobians of the landmarks in structure
%   array Lmk for the projection of the landmark errors into the landmark
%   manifolds. The computed Jacobians are stored in Lmk.state.M. Its
%   expression depends on the landmark type.
%
%   Landmark types are specifyed by Lmk.type. In the case of Euclidean
%   points 'eucPnt', details follow.
%
%   The landmark's position manifold is defined trivially, so that
%       dp = [dpx dpy dpz]'
%   and the composition is just a sum,
%       p+ = p + dp.
%   In such case, the Jacobian
%       M = d p+ / d dp
%   is the identity matrix. To save some computations, this function
%   returns just M = 1 (scalar 'one').
%
%   For details on the Jacobians of other landmark parametrizations, please
%   refer to the code by editing the file lmkJacobians.m.
%
%   See also FRMJACOBIANS.

% Copyright 2015-     Joan Sola @ IRI-UPC-CSIC.

for lmk = [Lmk([Lmk.used]).lmk]
    switch Lmk(lmk).type
        case 'eucPnt'
            Lmk(lmk).state.M = 1; % trivial Jac
        case 'hmgPnt'
            [~,~,H_dh] = composeHmgPnt(Lmk(lmk).state.x, zeros(3,1));
            Lmk(lmk).state.M = H_dh;
        otherwise
            error('??? Unknown landmark type ''%s'' or Jacobian not implemented.',Lmk.type)
    end
end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function[box,flag] = getBox2(theta,thetaDot,x,xDot)
theta = wrapTo180(rad2deg(theta));
thetaDot = rad2deg(thetaDot);
flag = 0;
if(theta>=-180&&theta<-150)
    thetaBucket = 1;
    flag = -1;
elseif(theta>=-150&&theta<-120)
    thetaBucket = 2;
    flag = -1;
elseif(theta>=-120&&theta<-90)
    thetaBucket = 3;
    flag = -1;
elseif(theta>=-90&&theta<-60)
    thetaBucket = 4;
    flag = -1;
elseif(theta>=-60&&theta<-30)
    thetaBucket = 5;
    flag = -1;
elseif(theta>=-30&&theta<-24)
    thetaBucket = 6;
    flag = -1;
elseif(theta>=-24&&theta<-18)
    thetaBucket = 7;
    flag = -1;
elseif(theta>=-18&&theta<-12)
    thetaBucket = 8;
    flag = -1;
elseif(theta>=-12&&theta<-6)
    thetaBucket = 9;
elseif(theta>=-6&&theta<-1)
    thetaBucket = 10;
elseif(theta>=-1&&theta<0)
    thetaBucket = 11;
elseif(theta>=0&&theta<1)
    thetaBucket  =12;
elseif(theta>=1&&theta<6)
    thetaBucket = 13;
elseif(theta>=6&&theta<12)
    thetaBucket = 14;
elseif(theta>=12&&theta<18)
    thetaBucket = 15;
    flag = -1;
elseif(theta>=18&&theta<24)
    thetaBucket = 16;
    flag = -1;
elseif(theta>=24&&theta<30)
    thetaBucket = 17;
    flag = -1;
elseif(theta>=30&&theta<60)
    thetaBucket = 18;
    flag = -1;
elseif(theta>=60&&theta<90)
    thetaBucket = 19;
    flag = -1;
elseif(theta>=90&&theta<120)
    thetaBucket = 20;
    flag = -1;
elseif(theta>=120&&theta<150)
    thetaBucket = 21;
    flag = -1;
elseif(theta>=150&&theta<=180)
    thetaBucket = 22;
    flag = -1;
end
if (x < -2.4 || x > 2.4)
    flag = -1;
    xBucket = 1;
end
if (x<-0.8&&x>=-2.4)
	xBucket = 1;
elseif (x<=0.8&&x>=-0.8)
	xBucket = 2;
elseif (x<=2.4&&x>0.8)
	xBucket = 3;
end

if (xDot<-0.5)
	xDotBucket = 1;
elseif (xDot>=-0.5&&xDot<=0.5)
	xDotBucket = 2;
else
	xDotBucket = 3;
end

if (thetaDot<-50)
	thetaDotBucket = 1;
elseif (thetaDot>=-50&&thetaDot<=50)
	thetaDotBucket = 2;
else
	thetaDotBucket = 3;
end

box = sub2ind([22,3,3,3],thetaBucket,thetaDotBucket,xBucket,xDotBucket);


classdef BinaryDescriptorMatcher < handle
    %BINARYDESCRIPTORMATCHER  BinaryDescriptor matcher class
    %
    % Furnishes all functionalities for querying a dataset provided by user or
    % internal to class (that user must, anyway, populate) on the model of
    % Descriptor Matchers.
    %
    % Once descriptors have been extracted from an image (both they represent
    % lines and points), it becomes interesting to be able to match a
    % descriptor with another one extracted from a different image and
    % representing the same line or point, seen from a differente perspective
    % or on a different scale. In reaching such goal, the main headache is
    % designing an efficient search algorithm to associate a query descriptor
    % to one extracted from a dataset. In the following, a matching modality
    % based on *Multi-Index Hashing (MiHashing)* will be described.
    %
    % ### Multi-Index Hashing
    %
    % The theory described in this section is based on [MIH]. Given a dataset
    % populated with binary codes, each code is indexed `m` times into `m`
    % different hash tables, according to `m` substrings it has been divided
    % into. Thus, given a query code, all the entries close to it at least in
    % one substring are returned by search as *neighbor candidates*. Returned
    % entries are then checked for validity by verifying that their full codes
    % are not distant (in Hamming space) more than `r` bits from query code.
    % In details, each binary code `h` composed of `b` bits is divided into
    % `m` disjoint substrings `h^(1), ..., h^(m)`, each with length
    % `floor(b/m)` or `ceil(b/m)` bits. Formally, when two codes `h` and `g`
    % differ by at the most `r` bits, in at the least one of their `m`
    % substrings they differ by at the most `floor(r/m)` bits. In particular,
    % when `||h-g||_H <= r` (where `||.||_H` is the Hamming norm), there must
    % exist a substring `k` (with `1 <= k <= m`) such that:
    %
    %     || h^(k) - g^(k) ||_H <= floor(r/m)
    %
    % That means that if Hamming distance between each of the `m` substring is
    % strictly greater than `floor(r/m)`, then `||h-g||_H` must be larger that
    % `r` and that is a contradiction. If the codes in dataset are divided
    % into `m` substrings, then `m` tables will be built. Given a query `q`
    % with substrings `{q^(i)}_[i=1..m]`, `i`-th hash table is searched for
    % entries distant at the most `floor(r/m)` from `q^(i)` and a set of
    % candidates `N_i(q)` is obtained. The union of sets `N(q) = U_i {N_i(q)}`
    % is a superset of the `r`-neighbors of `q`. Then, last step of algorithm
    % is computing the Hamming distance between `q` and each element in `N(q)`,
    % deleting the codes that are distant more that `r` from `q`.
    %
    % ## References
    % [MIH]:
    % > Mohammad Norouzi, Ali Punjani, and David J Fleet. "Fast search in
    % > hamming space with Multi-Index Hashing". In Computer Vision and
    % > Pattern Recognition (CVPR), 2012 IEEE Conference on, pages 3108-3115.
    % > IEEE, 2012.
    %
    % See also: cv.BinaryDescriptor, cv.DescriptorMatcher, cv.drawLineMatches
    %

    properties (SetAccess = private)
        % Object ID
        id
    end

    methods
        function this = BinaryDescriptorMatcher()
            %BINARYDESCRIPTORMATCHER  Constructor
            %
            %     matcher = cv.BinaryDescriptorMatcher()
            %
            % The BinaryDescriptorMatcher constructed is able to store and
            % manage 256-bits long entries.
            %
            % See also: cv.BinaryDescriptorMatcher
            %
            this.id = BinaryDescriptorMatcher_(0, 'new');
        end

        function delete(this)
            %DELETE  Destructor
            %
            %     matcher.delete()
            %
            % See also: cv.BinaryDescriptorMatcher
            %
            if isempty(this.id), return; end
            BinaryDescriptorMatcher_(this.id, 'delete');
        end
    end

    %% Algorithm
    methods
        function clear(this)
            %CLEAR  Clear dataset and internal data
            %
            %     matcher.clear()
            %
            % See also: cv.BinaryDescriptorMatcher.empty
            %
            BinaryDescriptorMatcher_(this.id, 'clear');
        end

        function status = empty(this)
            %EMPTY  Returns true if there are no train descriptors in the collection
            %
            %     status = matcher.empty()
            %
            % ## Output
            % * __status__ boolean status
            %
            % See also: cv.BinaryDescriptorMatcher.clear
            %
            status = BinaryDescriptorMatcher_(this.id, 'empty');
        end

        function save(this, filename)
            %SAVE  Saves the algorithm parameters to a file
            %
            %     matcher.save(filename)
            %
            % ## Input
            % * __filename__ Name of the file to save to.
            %
            % This method stores the algorithm parameters in the specified
            % XML or YAML file.
            %
            % See also: cv.BinaryDescriptorMatcher.load
            %
            BinaryDescriptorMatcher_(this.id, 'save', filename);
        end

        function load(this, fname_or_str, varargin)
            %LOAD  Loads algorithm from a file or a string
            %
            %     matcher.load(fname)
            %     matcher.load(str, 'FromString',true)
            %     matcher.load(..., 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __fname__ Name of the file to read.
            % * __str__ String containing the serialized model you want to
            %   load.
            %
            % ## Options
            % * __ObjName__ The optional name of the node to read (if empty,
            %   the first top-level node will be used). default empty
            % * __FromString__ Logical flag to indicate whether the input is a
            %   filename or a string containing the serialized model.
            %   default false
            %
            % This method reads algorithm parameters from the specified XML or
            % YAML file (either from disk or serialized string). The previous
            % algorithm state is discarded.
            %
            % See also: cv.BinaryDescriptorMatcher.save
            %
            BinaryDescriptorMatcher_(this.id, 'load', fname_or_str, varargin{:});
        end

        function name = getDefaultName(this)
            %GETDEFAULTNAME  Returns the algorithm string identifier
            %
            %     name = matcher.getDefaultName()
            %
            % ## Output
            % * __name__ This string is used as top level XML/YML node tag
            %   when the object is saved to a file or string.
            %
            % See also: cv.BinaryDescriptorMatcher.save, cv.BinaryDescriptorMatcher.load
            %
            name = BinaryDescriptorMatcher_(this.id, 'getDefaultName');
        end
    end

    %% BinaryDescriptorMatcher
    methods
        function add(this, descriptors)
            %ADD  Store locally new descriptors to be inserted in dataset, without updating dataset
            %
            %     matcher.add(descriptors)
            %
            % ## Input
            % * __descriptors__ cell array of matrices containing descriptors
            %   to be inserted into dataset. Each matrix `descriptors{i}`
            %   should contain descriptors relative to lines extracted from
            %   i-th image.
            %
            % See also: cv.BinaryDescriptorMatcher.clear
            %
            BinaryDescriptorMatcher_(this.id, 'add', descriptors);
        end

        function train(this)
            %TRAIN  Update dataset by inserting into it all descriptors that were stored locally by add function
            %
            %     matcher.train()
            %
            % NOTE: Every time this function is invoked, current dataset is
            % deleted and locally stored descriptors are inserted into
            % dataset. The locally stored copy of just inserted descriptors is
            % then removed.
            %
            % See also: cv.BinaryDescriptorMatcher.add
            %
            BinaryDescriptorMatcher_(this.id, 'train');
        end

        function matches = match(this, queryDescriptors, varargin)
            %MATCH  For every input query descriptor, retrieve the best matching one from a dataset provided from user or from the one internal to class
            %
            %     matches = matcher.match(queryDescriptors, trainDescriptors)
            %     matches = matcher.match(queryDescriptors)
            %     [...] = matcher.match(..., 'OptionName', optionValue, ...)
            %
            % ## Input
            % * __queryDescriptors__ query descriptors.
            % * __trainDescriptors__ dataset of descriptors furnished by user.
            %
            % ## Output
            % * __matches__ vector to host retrieved matches.
            %
            % ## Options
            % * __Mask__ mask to select which input descriptors must be
            %   matched to one in dataset. In the second variant, a vector of
            %   masks (the i-th mask in vector indicates whether each input
            %   query can be matched with descriptors in dataset relative to
            %   i-th image). Not set by default.
            %
            % For every input descriptor, find the best matching one:
            %
            % - in the first variant, for a pair of images
            % - in the second variant, from one image to a set
            %
            % See also: cv.BinaryDescriptorMatcher.knnMatch,
            %  cv.BinaryDescriptorMatcher.radiusMatch
            %
            matches = BinaryDescriptorMatcher_(this.id, 'match', queryDescriptors, varargin{:});
        end

        function matches = knnMatch(this, queryDescriptors, varargin)
            %KNNMATCH  For every input query descriptor, retrieve the best k matching ones from a dataset provided from user or from the one internal to class
            %
            %     matches = matcher.knnMatch(queryDescriptors, trainDescriptors, k)
            %     matches = matcher.knnMatch(queryDescriptors, k)
            %     [...] = matcher.knnMatch(..., 'OptionName', optionValue, ...)
            %
            % ## Input
            % * __queryDescriptors__ query descriptors.
            % * __trainDescriptors__ dataset of descriptors furnished by user.
            % * __k__ number of the closest descriptors to be returned for
            %   every input query.
            %
            % ## Output
            % * __matches__ vector to host retrieved matches.
            %
            % ## Options
            % * __Mask__ mask to select which input descriptors must be
            %   matched to ones in dataset. A vector of masks in the second
            %   variant (the i-th mask in vector indicates whether each input
            %   query can be matched with descriptors in dataset relative to
            %   i-th image). Not set by default.
            % * __CompactResult__ flag to obtain a compact result (if true, a
            %   vector that doesn't contain any matches for a given query is
            %   not inserted in final result). default false
            %
            % For every input descriptor, find the best k matching descriptors:
            %
            % - in the first variant, for a pair of images
            % - in the second variant, from one image to a set
            %
            % See also: cv.BinaryDescriptorMatcher.match,
            %  cv.BinaryDescriptorMatcher.radiusMatch
            %
            matches = BinaryDescriptorMatcher_(this.id, 'knnMatch', ...
                queryDescriptors, varargin{:});
        end

        function matches = radiusMatch(this, queryDescriptors, varargin)
            %RADIUSMATCH  For every input query descriptor, retrieve, from a dataset provided from user or from the one internal to class, all the descriptors that are not further than maxDist from input query
            %
            %     matches = matcher.radiusMatch(queryDescriptors, trainDescriptors, maxDistance)
            %     matches = matcher.radiusMatch(queryDescriptors, maxDistance)
            %     [...] = matcher.radiusMatch(..., 'OptionName', optionValue, ...)
            %
            % ## Input
            % * __queryDescriptors__ query descriptors.
            % * __trainDescriptors__ dataset of descriptors furnished by user.
            % * __maxDistance__ search radius.
            %
            % ## Output
            % * __matches__ vector to host retrieved matches.
            %
            % ## Options
            % * __Mask__ mask to select which input descriptors must be
            %   matched to ones in dataset. A vector of masks in the second
            %   variant (the i-th mask in vector indicates whether each input
            %   query can be matched with descriptors in dataset relative to
            %   i-th image). Not set by default.
            % * __CompactResult__ flag to obtain a compact result (if true, a
            %   vector that doesn't contain any matches for a given query is
            %   not inserted in final result). default false
            %
            % For every input desciptor, find all the ones falling in a
            % certaing matching radius:
            %
            % - in the first variant, for a pair of images
            % - in the second variant, from one image to a set
            %
            % See also: cv.BinaryDescriptorMatcher.match,
            %  cv.BinaryDescriptorMatcher.knnMatch
            %
            matches = BinaryDescriptorMatcher_(this.id, 'radiusMatch',...
                queryDescriptors, varargin{:});
        end
    end

end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
%
% problem 8- periodic signal
% in one period x[n]=[0.5,1,1]

 x=[0.5 1 1];

 xp=repmat(x,1,40);

 stem(10:length(xp)+9,xp);


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 [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')


% 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 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


%%   FDI Wind Turbines
% This script containing parameters for Bench Mark Model By Peter Fogh
% Odgaard, kk-electronic a/s, Viby J Denmark -Date 7.11.2008-
% with little modification by Nassim Laouti Date 13.11.2010
% Latest modification Date 16.02.2012

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
c3=10;c4=1000;P_r=4.8e6;
% Simple time and time
Ts=1/100;Time=Ts:Ts:4400;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Wind data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
D_v_wind=[Time', 9+4*sin(0.01*Time)'];

load winddata
D_v_wind=[windtime windspeed];

%% Wind Model
seed1 = 256;
seed2 = 894;
turbulence_seeds=[seed1 seed2];
R=57.5;
H=87;
k =4.7;
a=2.2;
alpha = 0.1;
D_rotor=2*R;
r = D_rotor/2;
m = 1+alpha*(alpha-1)*R^2/(8*H^2);
Length_scale = 600; %[m]
L=Length_scale;
Turbulence_intensity = 12;
turb_int=Turbulence_intensity;
T_sample=0.05;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Pitch and Blade Model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
omega_n=11.11;xi=0.6;rho=1.225;
R=57.5;r0 = 1.5;

% cq table
load AeroDynamics

% Fault models
% Fault 6
xi2=0.45;omega_n2=5.73;
% Fault 7
xi3=0.9;omega_n3=3.42;

%transfers to ss models
[Apb,Bpb,Cpb,Dpb]=tf2ss([omega_n^2],[1 2*xi*omega_n omega_n^2]);
[Apb1,Bpb1,Cpb1,Dpb1]=tf2ss([omega_n2^2],[1 2*xi2*omega_n2 omega_n2^2]);
[Apb2,Bpb2,Cpb2,Dpb2]=tf2ss([omega_n3^2],[1 2*xi3*omega_n3 omega_n3^2]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Drive Train Model
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
B_dt=775.49;
B_r=7.11;
B_g=45.6;
N_g=95;
K_dt=2.7e9
eta_dt=0.97;
J_g=390
J_r=55e6

Addt=[-(B_dt+B_r)/J_r B_dt/N_g/J_r -K_dt/J_r; eta_dt*B_dt/N_g/J_g -(eta_dt*B_dt/N_g^2+B_g)/J_g eta_dt*K_dt/N_g/J_g; 1 -1/N_g 0];
Bddt=[1/J_r 0; 0 -1/J_g;0 0];
Cddt=[1 0 0;0 1 0];
Dddt=[0 0;0 0];

% Fault models
% Fault 9
eta_dt2=.91

Addt2=[-(B_dt+B_r)/J_r B_dt/N_g/J_r -K_dt/J_r; eta_dt2*B_dt/N_g/J_g -(eta_dt2*B_dt/N_g^2+B_g)/J_g eta_dt2*K_dt/N_g/J_g; 1 -1/N_g 0];
Bddt2=[1/J_r 0; 0 -1/J_g;0 0];
Cddt2=[1 0 0;0 1 0];
Dddt2=[0 0;0 0];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Generator & Converter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
alpha_gc=1/20e-3;
eta_gc=0.98;

%Fault models
%fault 8
Constant_tau_gc=100;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Controller
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
K_opt=.5* rho* pi*R^2*eta_dt*R^3*0.4554/(N_g*8)^3

K_i=1;
K_p=4;
Omega_nom=162;
Omega_delta=5;
P_r=4.8e6;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Sensors
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
m_vw=1.5;
sigma_vm=0.5;
m_omega_r=0;
sigma_omega_r=0.004*2*pi;
m_omega_g=0;
sigma_omega_g=0.05;
m_tau_g=0;
sigma_tau_g=90;
m_P_g=0;
sigma_P_g=1e3;
m_Beta=0;
sigma_Beta=0.2;

%% Fault models (choose the scenario of fault parameters and magnitude)

% %1st fault scenario
% Constant_Beta_1_m1=5;Gain_Beta_2_m2=1.2;Constant_Beta_3_m1=10;Constant_Omega_r_m1=1.4;Gain_Omega_r_m2=1.1;Gain_Omega_g_m2=0.9;
% xi2=0.45;omega_n2=5.73;xi3=0.9;omega_n3=3.42;Constant_tau_gc=100;eta_dt2=.92;Constant_Omega_g_m1=140;

% %2nd fault scenario
% Constant_Beta_1_m1=6;Gain_Beta_2_m2=1.5;Constant_Beta_3_m1=8;Constant_Omega_r_m1=0.2;Gain_Omega_r_m2=1.2;Gain_Omega_g_m2=0.8;
% xi2=0.45;omega_n2=5.73;xi3=0.9;omega_n3=3.42;Constant_tau_gc=100;eta_dt2=.71;Constant_Omega_g_m1=80;

%3rd fault scenario
Constant_Beta_1_m1=4;Gain_Beta_2_m2=1.8;Constant_Beta_3_m1=12;Constant_Omega_r_m1=1.2;Gain_Omega_r_m2=0.7;Gain_Omega_g_m2=1.7;
xi2=0.45;omega_n2=5.73;xi3=0.9;omega_n3=3.42;Constant_tau_gc=700;eta_dt2=0.3;Constant_Omega_g_m1=100;

%4th fault scenario
% Constant_Beta_1_m1=2;Gain_Beta_2_m2=3;Constant_Beta_3_m1=15;Constant_Omega_r_m1=1.7;Gain_Omega_r_m2=1.7;Gain_Omega_g_m2=0.7;
% xi2=0.45;omega_n2=5.73;xi3=0.9;omega_n3=3.42;Constant_tau_gc=-500;eta_dt2=.6;Constant_Omega_g_m1=130;

% %5th fault scenario
% Constant_Beta_1_m1=-5;Gain_Beta_2_m2=10;Constant_Beta_3_m1=3;Constant_Omega_r_m1=2.5;Gain_Omega_r_m2=0.9;Gain_Omega_g_m2=0.2;
% xi2=0.45;omega_n2=5.73;xi3=0.9;omega_n3=3.42;Constant_tau_gc=1500;eta_dt2=.25;Constant_Omega_g_m1=120;

%6th fault scenario
% Constant_Beta_1_m1=1;Gain_Beta_2_m2=2;Constant_Beta_3_m1=6;Constant_Omega_r_m1=0.5;Gain_Omega_r_m2=0.2;Gain_Omega_g_m2=2.8;
% xi2=0.45;omega_n2=5.73;xi3=0.9;omega_n3=3.42;Constant_tau_gc=2500;eta_dt2=0.1;Constant_Omega_g_m1=150;

% % %7th fault scenario
% Constant_Beta_1_m1=24;Gain_Beta_2_m2=5;Constant_Beta_3_m1=20;Constant_Omega_r_m1=3.5;Gain_Omega_r_m2=3;Gain_Omega_g_m2=5;
% xi2=0.25;omega_n2=8.73;xi3=0.95;omega_n3=1.42;Constant_tau_gc=900;eta_dt2=0.4;Constant_Omega_g_m1=50;

%8th fault scenario
% Constant_Beta_1_m1=-3;Gain_Beta_2_m2=5;Constant_Beta_3_m1=7;Constant_Omega_r_m1=2;Gain_Omega_r_m2=0.5;Gain_Omega_g_m2=1.5;
% xi2=0.45;omega_n2=5.73;xi3=0.9;omega_n3=3.42;Constant_tau_gc=-1000;eta_dt2=0.22;Constant_Omega_g_m1=100;

% %9th fault scenario
% Constant_Beta_1_m1=2;Gain_Beta_2_m2=3;Constant_Beta_3_m1=7;Constant_Omega_r_m1=2;Gain_Omega_r_m2=0.5;Gain_Omega_g_m2=1.5;
% xi2=0.45;omega_n2=5.73;xi3=0.9;omega_n3=3.42;Constant_tau_gc=800;eta_dt2=0.42;Constant_Omega_g_m1=110;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[Apb1,Bpb1,Cpb1,Dpb1]=tf2ss([omega_n2^2],[1 2*xi2*omega_n2 omega_n2^2]);
[Apb2,Bpb2,Cpb2,Dpb2]=tf2ss([omega_n3^2],[1 2*xi3*omega_n3 omega_n3^2]);
%
Addt2=[-(B_dt+B_r)/J_r B_dt/N_g/J_r -K_dt/J_r; eta_dt2*B_dt/N_g/J_g -(eta_dt2*B_dt/N_g^2+B_g)/J_g eta_dt2*K_dt/N_g/J_g; 1 -1/N_g 0];
Bddt2=[1/J_r 0; 0 -1/J_g;0 0];
Cddt2=[1 0 0;0 1 0];
Dddt2=[0 0;0 0];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Fault signals (choose the scenario of fault appearance)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %1st fault scenario
% D_Fault1=[Time' 1-[zeros(1,2000/Ts) ones(1,100/Ts) zeros(1,2300/Ts)]'];
% D_Fault2=[Time' 1-[zeros(1,2300/Ts) ones(1,100/Ts) zeros(1,2000/Ts)]'];
% D_Fault3=[Time' 1-[zeros(1,2600/Ts) ones(1,100/Ts) zeros(1,1700/Ts)]'];
% D_Fault4=[Time' 1-[zeros(1,1500/Ts) ones(1,100/Ts) zeros(1,2800/Ts)]'];
% D_Fault5=[Time' 1-[zeros(1,1000/Ts) ones(1,100/Ts) zeros(1,3300/Ts)]'];
% D_Fault6=[Time' 1-[zeros(1,2900/Ts) ones(1,100/Ts) zeros(1,1400/Ts)]'];
% D_Fault7=[Time' 1-[zeros(1,3400/Ts) (0:1/3000:2999/3000) ones(1,40/Ts) (2999/3000:-1/3000:0) zeros(1,900/Ts)]'];
% D_Fault8=[Time' 1-[zeros(1,3800/Ts) ones(1,100/Ts) zeros(1,500/Ts)]'];
% D_Fault9=[Time' 1-[zeros(1,4000/Ts) ones(1,200/Ts) zeros(1,200/Ts)]'];
% D_Fault10=[Time' 1-[zeros(1,3600/Ts) ones(1,100/Ts) zeros(1,700/Ts) ]'];

%%2nd fault scenario
% D_Fault1=[Time' 1-[zeros(1,1000/Ts) ones(1,100/Ts) zeros(1,3300/Ts)]'];
% D_Fault2=[Time' 1-[zeros(1,3900/Ts) ones(1,100/Ts) zeros(1,400/Ts)]'];
% D_Fault3=[Time' 1-[zeros(1,1600/Ts) ones(1,100/Ts) zeros(1,2700/Ts)]'];
% D_Fault4=[Time' 1-[zeros(1,500/Ts) ones(1,100/Ts) zeros(1,3800/Ts)]'];
% D_Fault5=[Time' 1-[zeros(1,4000/Ts) ones(1,100/Ts) zeros(1,300/Ts)]'];
% D_Fault6=[Time' 1-[zeros(1,2900/Ts) ones(1,100/Ts) zeros(1,1400/Ts)]'];
% D_Fault7=[Time' 1-[zeros(1,3400/Ts) (0:1/3000:2999/3000) ones(1,40/Ts) (2999/3000:-1/3000:0) zeros(1,900/Ts)]'];
% D_Fault8=[Time' 1-[zeros(1,1800/Ts) ones(1,100/Ts) zeros(1,2500/Ts)]'];
% D_Fault9=[Time' 1-[zeros(1,3000/Ts) ones(1,200/Ts) zeros(1,1200/Ts)]'];
% D_Fault10=[Time' 1-[zeros(1,2600/Ts) ones(1,100/Ts) zeros(1,1700/Ts) ]'];

%3nd fault scenario
D_Fault1=[Time' 1-[zeros(1,100/Ts) ones(1,100/Ts) zeros(1,4200/Ts)]'];
D_Fault2=[Time' 1-[zeros(1,2500/Ts) ones(1,100/Ts) zeros(1,1800/Ts)]'];
D_Fault3=[Time' 1-[zeros(1,900/Ts) ones(1,100/Ts) zeros(1,3400/Ts)]'];
D_Fault4=[Time' 1-[zeros(1,1200/Ts) ones(1,100/Ts) zeros(1,3100/Ts)]'];
D_Fault5=[Time' 1-[zeros(1,1700/Ts) ones(1,100/Ts) zeros(1,2600/Ts)]'];
D_Fault6=[Time' 1-[zeros(1,3200/Ts) ones(1,100/Ts) zeros(1,1100/Ts)]'];
D_Fault7=[Time' 1-[zeros(1,3400/Ts) (0:1/3000:2999/3000) ones(1,40/Ts) (2999/3000:-1/3000:0) zeros(1,900/Ts)]'];
D_Fault8=[Time' 1-[zeros(1,4200/Ts) ones(1,100/Ts) zeros(1,100/Ts)]'];
D_Fault9=[Time' 1-[zeros(1,2000/Ts) ones(1,200/Ts) zeros(1,2200/Ts)]'];
D_Fault10=[Time' 1-[zeros(1,3900/Ts) ones(1,100/Ts) zeros(1,400/Ts)]'];

% %4th fault scenario
% D_Fault1=[Time' 1-[zeros(1,250/Ts) ones(1,100/Ts) zeros(1,4050/Ts)]'];
% D_Fault2=[Time' 1-[zeros(1,3200/Ts) ones(1,100/Ts) zeros(1,1100/Ts)]'];
% D_Fault3=[Time' 1-[zeros(1,1000/Ts) ones(1,100/Ts) zeros(1,3300/Ts)]'];
% D_Fault4=[Time' 1-[zeros(1,800/Ts) ones(1,100/Ts) zeros(1,3500/Ts)]'];
% D_Fault5=[Time' 1-[zeros(1,2200/Ts) ones(1,100/Ts) zeros(1,2100/Ts)]'];
% D_Fault6=[Time' 1-[zeros(1,2800/Ts) ones(1,100/Ts) zeros(1,1500/Ts)]'];
% D_Fault7=[Time' 1-[zeros(1,3400/Ts) (0:1/3000:2999/3000) ones(1,40/Ts) (2999/3000:-1/3000:0) zeros(1,900/Ts)]'];
% D_Fault8=[Time' 1-[zeros(1,1200/Ts) ones(1,100/Ts) zeros(1,3100/Ts)]'];
% D_Fault9=[Time' 1-[zeros(1,3900/Ts) ones(1,200/Ts) zeros(1,300/Ts)]'];
% D_Fault10=[Time' 1-[zeros(1,4200/Ts) ones(1,100/Ts) zeros(1,100/Ts)]'];

% %5th fault scenario
% D_Fault1=[Time' 1-[zeros(1,3000/Ts) ones(1,100/Ts) zeros(1,1300/Ts)]'];
% D_Fault2=[Time' 1-[zeros(1,1300/Ts) ones(1,100/Ts) zeros(1,3000/Ts)]'];
% D_Fault3=[Time' 1-[zeros(1,500/Ts) ones(1,100/Ts) zeros(1,3800/Ts)]'];
% D_Fault4=[Time' 1-[zeros(1,200/Ts) ones(1,100/Ts) zeros(1,4100/Ts)]'];
% D_Fault5=[Time' 1-[zeros(1,2000/Ts) ones(1,100/Ts) zeros(1,2300/Ts)]'];
% D_Fault6=[Time' 1-[zeros(1,3600/Ts) ones(1,100/Ts) zeros(1,700/Ts)]'];
% D_Fault7=[Time' 1-[zeros(1,3400/Ts) (0:1/3000:2999/3000) ones(1,40/Ts) (2999/3000:-1/3000:0) zeros(1,900/Ts)]'];
% D_Fault8=[Time' 1-[zeros(1,2400/Ts) ones(1,100/Ts) zeros(1,1900/Ts)]'];
% D_Fault9=[Time' 1-[zeros(1,2700/Ts) ones(1,200/Ts) zeros(1,1500/Ts)]'];
% D_Fault10=[Time' 1-[zeros(1,3300/Ts) ones(1,100/Ts) zeros(1,1000/Ts)]'];

% % %6th fault scenario
% D_Fault1=[Time' 1-[zeros(1,3800/Ts) ones(1,100/Ts) zeros(1,500/Ts)]'];
% D_Fault2=[Time' 1-[zeros(1,2600/Ts) ones(1,100/Ts) zeros(1,1700/Ts)]'];
% D_Fault3=[Time' 1-[zeros(1,4000/Ts) ones(1,100/Ts) zeros(1,300/Ts)]'];
% D_Fault4=[Time' 1-[zeros(1,2900/Ts) ones(1,100/Ts) zeros(1,1400/Ts)]'];
% D_Fault5=[Time' 1-[zeros(1,600/Ts) ones(1,100/Ts) zeros(1,3700/Ts)]'];
% D_Fault6=[Time' 1-[zeros(1,1500/Ts) ones(1,100/Ts) zeros(1,2800/Ts)]'];
% D_Fault7=[Time' 1-[zeros(1,1800/Ts) (0:1/3000:2999/3000) ones(1,40/Ts) (2999/3000:-1/3000:0) zeros(1,2500/Ts)]'];
% D_Fault8=[Time' 1-[zeros(1,100/Ts) ones(1,100/Ts) zeros(1,4200/Ts)]'];
% D_Fault9=[Time' 1-[zeros(1,2000/Ts) ones(1,200/Ts) zeros(1,2200/Ts)]'];
% D_Fault10=[Time' 1-[zeros(1,3500/Ts) ones(1,100/Ts) zeros(1,800/Ts)]'];
%
% %7th fault scenario
% D_Fault1=[Time' 1-[zeros(1,4200/Ts) ones(1,100/Ts) zeros(1,100/Ts)]'];
% D_Fault2=[Time' 1-[zeros(1,4000/Ts) ones(1,100/Ts) zeros(1,300/Ts)]'];
% D_Fault3=[Time' 1-[zeros(1,3700/Ts) ones(1,100/Ts) zeros(1,600/Ts)]'];
% D_Fault4=[Time' 1-[zeros(1,3500/Ts) ones(1,100/Ts) zeros(1,800/Ts)]'];
% D_Fault5=[Time' 1-[zeros(1,3000/Ts) ones(1,100/Ts) zeros(1,1300/Ts)]'];
% D_Fault6=[Time' 1-[zeros(1,2700/Ts) ones(1,100/Ts) zeros(1,1600/Ts)]'];
% D_Fault7=[Time' 1-[zeros(1,2500/Ts) (0:1/3000:2999/3000) ones(1,40/Ts) (2999/3000:-1/3000:0) zeros(1,1800/Ts)]'];
% D_Fault8=[Time' 1-[zeros(1,2100/Ts) ones(1,100/Ts) zeros(1,2200/Ts)]'];
% D_Fault9=[Time' 1-[zeros(1,1400/Ts) ones(1,200/Ts) zeros(1,2800/Ts)]'];
% D_Fault10=[Time' 1-[zeros(1,1000/Ts) ones(1,100/Ts) zeros(1,3300/Ts)]'];

% % 8th fault scenario
% D_Fault1=[Time' 1-[zeros(1,100/Ts) ones(1,100/Ts) zeros(1,4200/Ts)]'];
% D_Fault2=[Time' 1-[zeros(1,500/Ts) ones(1,100/Ts) zeros(1,3800/Ts)]'];
% D_Fault3=[Time' 1-[zeros(1,900/Ts) ones(1,100/Ts) zeros(1,3400/Ts)]'];
% D_Fault4=[Time' 1-[zeros(1,1200/Ts) ones(1,100/Ts) zeros(1,3100/Ts)]'];
% D_Fault5=[Time' 1-[zeros(1,1700/Ts) ones(1,100/Ts) zeros(1,2600/Ts)]'];
% D_Fault6=[Time' 1-[zeros(1,2900/Ts) ones(1,100/Ts) zeros(1,1400/Ts)]'];
% D_Fault7=[Time' 1-[zeros(1,3400/Ts) (0:1/3000:2999/3000) ones(1,40/Ts) (2999/3000:-1/3000:0) zeros(1,900/Ts)]'];
% D_Fault8=[Time' 1-[zeros(1,4200/Ts) ones(1,100/Ts) zeros(1,100/Ts)]'];
% D_Fault9=[Time' 1-[zeros(1,300/Ts) ones(1,200/Ts) zeros(1,3900/Ts)]'];
% D_Fault10=[Time' 1-[zeros(1,2200/Ts) ones(1,100/Ts) zeros(1,2100/Ts)]'];

% 9th fault scenario

% D_Fault2=[Time' 1-[zeros(1,100/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,2330/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,1830/Ts) ]'];
% Offset_Beta_2_m2=[Time' [zeros(1,100/Ts) 0.5*ones(1,10/Ts) zeros(1,5/Ts) 1*ones(1,10/Ts) zeros(1,5/Ts) 1.5*ones(1,10/Ts) zeros(1,5/Ts) 2*ones(1,10/Ts) zeros(1,5/Ts) 3*ones(1,10/Ts) zeros(1,2330/Ts) 0.5*ones(1,10/Ts) zeros(1,5/Ts) 1*ones(1,10/Ts) zeros(1,5/Ts) 1.5*ones(1,10/Ts) zeros(1,5/Ts) 2*ones(1,10/Ts) zeros(1,5/Ts) 3*ones(1,10/Ts) zeros(1,1830/Ts) ]'];
%
% D_Fault5=[Time' 1-[zeros(1,500/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,2330/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,5/Ts) ones(1,10/Ts) zeros(1,1430/Ts) ]'];
% Offset_Omega_r_m2=[Time' [zeros(1,500/Ts) 0.1*ones(1,10/Ts) zeros(1,5/Ts) 0.2*ones(1,10/Ts) zeros(1,5/Ts) 0.5*ones(1,10/Ts) zeros(1,5/Ts) 1*ones(1,10/Ts) zeros(1,5/Ts) 2*ones(1,10/Ts) zeros(1,2330/Ts) 0.1*ones(1,10/Ts) zeros(1,5/Ts) 0.2*ones(1,10/Ts) zeros(1,5/Ts) 0.5*ones(1,10/Ts) zeros(1,5/Ts) 1*ones(1,10/Ts) zeros(1,5/Ts) 2*ones(1,10/Ts) zeros(1,1430/Ts) ]'];
% Offset_Omega_g_m2=[Time' [zeros(1,500/Ts) 2.5*ones(1,10/Ts) zeros(1,5/Ts) 4*ones(1,10/Ts) zeros(1,5/Ts) 5*ones(1,10/Ts) zeros(1,5/Ts) 6*ones(1,10/Ts) zeros(1,5/Ts) 8*ones(1,10/Ts) zeros(1,2330/Ts) 2.5*ones(1,10/Ts) zeros(1,5/Ts) 4*ones(1,10/Ts) zeros(1,5/Ts) 5*ones(1,10/Ts) zeros(1,5/Ts) 6*ones(1,10/Ts) zeros(1,5/Ts) 8*ones(1,10/Ts) zeros(1,1430/Ts) ]'];

% D_Fault1=[Time' 1-[zeros(1,300/Ts) ones(1,100/Ts) zeros(1,4000/Ts)]'];
% D_Fault3=[Time' 1-[zeros(1,700/Ts) ones(1,100/Ts) zeros(1,3600/Ts)]'];
% D_Fault4=[Time' 1-[zeros(1,1000/Ts) ones(1,100/Ts) zeros(1,3300/Ts)]'];
% D_Fault6=[Time' 1-[zeros(1,1900/Ts) ones(1,100/Ts) zeros(1,2400/Ts)]'];
% D_Fault7=[Time' 1-[zeros(1,1400/Ts) (0:1/3000:2999/3000) ones(1,40/Ts) (2999/3000:-1/3000:0) zeros(1,2900/Ts)]'];
% D_Fault8=[Time' 1-[zeros(1,4200/Ts) ones(1,100/Ts) zeros(1,100/Ts)]'];
% D_Fault9=[Time' 1-[zeros(1,3300/Ts) ones(1,200/Ts) zeros(1,900/Ts)]'];
% D_Fault10=[Time' 1-[zeros(1,2200/Ts) ones(1,100/Ts) zeros(1,2100/Ts)]'];

% %without Fault
% D_Fault1=[Time' 1-[zeros(1,4400/Ts)]'];
% D_Fault2=[Time' 1-[zeros(1,4400/Ts)]'];
% D_Fault3=[Time' 1-[zeros(1,4400/Ts)]'];
% D_Fault4=[Time' 1-[zeros(1,4400/Ts)]'];
% D_Fault5=[Time' 1-[zeros(1,4400/Ts)]'];
% D_Fault6=[Time' 1-[zeros(1,4400/Ts)]'];
% D_Fault7=[Time' 1-[zeros(1,4400/Ts)]'];
% D_Fault8=[Time' 1-[zeros(1,4400/Ts)]'];
% D_Fault9=[Time' 1-[zeros(1,4400/Ts)]'];
%  D_Fault10=[Time' 1-[zeros(1,4400/Ts)]'];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Noise Seeds
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

min_seed = 0;
max_seed = 999;
% Generate random seeds for 13 'Random Number' Generators blocks
seed = min_seed + (max_seed-min_seed).*rand(13, 1);
% Round up to integer
seed = ceil(seed);

%% Simulation
open_system('FDI_measures.mdl');

tic
sim('FDI_measures.mdl');
toc


 function F_enhance= RMSHEg_gray( F )
% clear all;
% close all;
% clc;
x0=F;
%x0=imread('om.bmp');
%x0=imread('11.bmp');
 %x0=imread('12.bmp');
%y0=rgb2ycbcr(x0);
luma=x0;%(:,:,1);
[m,n]=size(luma);
% cb=y0(:,:,2);
% cr=y0(:,:,3);
k=mean(mean(luma));
XM=k;
r=round(k);
l=length(luma(luma<=r));
         listindex=find(luma<=r);
 k1=reshape(luma(listindex),l,1);%obtained below fist mean values put it into an arry
 k2=mean(k1);%find mean of mean in first half
 k3=round(k2);%round to nearest integer to find pdf and cdf till first quarter
  sum=0;
 l1=length(luma(luma<=k3));%find all  values in luma falls below fist mean
 listindex1=find(luma<=k3);%all  values in luma falls below fist mean given in their locations
 k4=reshape(luma(listindex1),l1,1);%arrange all fist mean values in a vector
     xpdf=hist(k4,[0:k3]);%pdf from 0:r
      xpdf=xpdf/l1;%normalized pdf to get nk/n,l=sum of xpdf,total no of pixels form 0 to r.
     % plot(xpdf);
%      plot(imhist(luma));
%       xlabel('gray levels up to mean');
%       ylabel('pdf up to mean');
%       title('histogram for half an image up to mean');
      sk=xpdf*triu(ones(k3+1));
%       figure(2);
%       plot(sk);
%       xlabel('gray levels upto 1st mean');
%       ylabel('cdf upto mean');
%       title('cdf for half  of an image up to ist mean');
      alpha=0.6;
      for l2=0:k3
              list1=find(k4==l2);%find value in an vector i.e converted from matrix
              %list(list1)=alpha*sk(l2+1)*(k3+1)+(1-alpha)*(k3+1);
              list(list1)=sk(l2+1)*(k3+1);%map dont disturb to get bhe as
              %it is 13/3/2011
              ert(l2+1)=sk(l2+1)*(k3+1);
      end
          p=zeros(m,n);
      p(listindex1)= list;
%      figure(3);
%      imshow(p);
%      xlabel('gray levels up to first mean');
%       ylabel('luma component equilized image up to first mean');
%      title('processed luma image up to first mean');
     k=mean(mean(luma));
r=round(k);
l=length(luma(luma<=r));
        listindex=find(luma<=r);
 k1=reshape(luma(listindex),l,1);%obtained below fist mean values put it into an arry
 k2=mean(k1);%find mean of mean in first half
 k3=round(k2);%round to nearest integer to find pdf and cdf till first quarter
%   sum=0;
   b=k3;
%  l2=length(luma(luma>k3));%find all  values in luma falls below fist mean
  listindex2=find((luma>k3)&(luma<=r));%all  values in luma falls below fist mean given in their locations
  l2=length(listindex2);
  k5=reshape(luma(listindex2),l2,1);%arrange all fist mean values in a vector
      x2pdf=hist(k5,[k3+1:r]);%pdf from 0:r
       x2pdf=x2pdf/l2;%normalized pdf to get nk/n,l=sum of xpdf,total no of pixels form 0 to r.
%        figure(4);
%        plot(x2pdf);
%        xlabel('gray levels 2nd mean');
%        ylabel('pdf of 2nd mean');
%        title('histogram for 2nd mean');
       sk2=x2pdf*triu(ones(r-k3));
%        figure(5);
%        plot(sk2);
%        xlabel('gray levels of 2nd mean');
%        ylabel('cdf upto mean');
%        title('cdf for half  of an image of 2nd mean');
       k2u=1;
       for l3=k3+1:r
               list2=find(k5==l3);%find value in an vector i.e converted from matrix
               %list1(list2)=alpha*(sk2(k2u)*(r-k3)+(k3+1))+(1-alpha)*(k3+1);
               list1(list2)=(k3+1)+(sk2(k2u))*(r-k3);%map dont disturb to
               %get BHE 13/3/2011
               ert(l3)=(k3+1)+(sk2(k2u))*(r-k3);
               k2u=k2u+1;
       end
           p1=zeros(m,n);
       p1(listindex2)= list1;
%       figure(6);
%       imshow(p1);
%      xlabel('gray levels up to first 2nd mean');
%       ylabel('luma component equilized image 2nd mean');
%      title('processed luma image up to 2nd mean');
 %lupper30=length(luma(luma>r);
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 lupper30=length(luma(luma>r));
%for i=0:r
    %if(luma(luma<=r))
        listindexupper3=find(luma>r);
   % end
%end
 k1upper=reshape(luma(listindexupper3),lupper30,1);
 mean3=mean(k1upper);
  r3=round(mean3);
 %length30=length((luma<=r3)&(luma>r));
  listindexupper30=find((luma>r)&(luma<=r3));
  length30=length(listindexupper30);
  k30upper=reshape(luma(listindexupper30),length30,1);

  %length30=length((luma<=r3)&(luma>r));
%  sum=0;
      xpdfupper30=hist(k30upper,[r+1:r3]);%pdf from r+1:r3
      xpdfupper30=xpdfupper30/length30;%normalized pdf to get nk/n,l=sum of xpdf,total no of pixels form r+1 to 255.
%       figure(7);
%      plot(xpdfupper30);
%      xlabel('gray levels   3rd mean');
%      ylabel('pdf of 3rd mean');
%      title('histogram for upper half an image  3rd mean');
      skupper30=xpdfupper30*triu(ones(r3-r));
%       figure(8);
%       plot(skupper30);
%       xlabel('gray levels after mean');
%       ylabel('cdf after mean');
%       title('cdf for upper half  of an image after mean');
      k3u=1;
      for k3upper=(r+1):r3
              list1upper30=find(k30upper==k3upper);%find value in an vector i.e converted from matrix
              listnew(list1upper30)=(r+1)+skupper30(k3u)*(r3-r);%map dont
              %disturg to get original heq 14/3/2011
              %listnew(list1upper30)=alpha*(skupper30(k3u)*(r3-r)+(r+1))+(1-alpha)*(r+1);
              ert(k3upper)=(r+1)+skupper30(k3u)*(r3-r);
         k3u=k3u+1;
     end

          p2=zeros(m,n);

     p2(listindexupper30)= listnew;
%     figure(9);
%      imshow(p2);
     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
     listindexupper40=find(luma>r3);
     lupper40=length(listindexupper40);
   % end
%end
 k1upper40=reshape(luma(listindexupper40),lupper40,1);
 %sum=0;
 %for i=0:r
     xpdfupper40=hist(k1upper40,[r3+1:255]);%pdf from r+1:255
     xpdfupper40=xpdfupper40/lupper40;%normalized pdf to get nk/n,l=sum of xpdf,total no of pixels form r+1 to 255.
%      figure(10);
%      plot(xpdfupper40);
%      xlabel('gray levels after 4mean');
%      ylabel('pdf after 4mean');
%      title('histogram for upper half an image after 4mean');
     skupper40=xpdfupper40*triu(ones(255-r3));
%      figure(11);
%      plot(skupper40);
%      xlabel('gray levels after 4mean');
%      ylabel('cdf after 4mean');
%      title('cdf for upper half  of an image after 4mean');
     k4u=1;
     for k4upper=(r3+1):255
         %if(xpdfupper(k2upper)>r)
             list1upper40=find(k1upper40==k4upper);%find value in an vector i.e converted from matrix
             %for k2u=1:58
             %listnew4(list1upper40)=alpha*(198+skupper40(k4u)*(255-r3))+(1-alpha)*(r3+1);
             listnew4(list1upper40)=(r3+1)+skupper40(k4u)*(255-r3);%map
             %dont disturb to get original bbhe 14/3/2011
         %end
         ert(k4upper)=(r3+1)+skupper40(k4u)*(255-r3);
         k4u=k4u+1;
     end

%      for i=0:l-1
          p3=zeros(m,n);
%          if (p(listindex))
%              p(:)=list;
%          end
%      end

     p3(listindexupper40)= listnew4;
%     figure(12);
%     imshow(p3);
%     xlabel('gray levels after 4mean');
%      ylabel('luma component equilized image after 4mean');
%      title('processed luma image after 4mean');
     om=p+p1+p2+p3;
      F_enhance=om;
 end
     %ommmmm=p1+p;
%     figure(13);
% %     imshow(ommmmm);
%      colormap('gray');
%       xlabel('gray level');
%      ylabel('combined lower and upper half luma component equilized image');
%      title('combined luma image');
%     figure(8);
%     imshow(uint8(om));
%     for j1=0:255
%         count=0;
%         for i1=0:m*n-1
%             if om(i1+1)==j1
%                 count=count+1;
%             end
%         end
%             prob(j1+1)=count/m*n;
%     end
%     figure(16);
%     plot(prob);
%
%    for j2=0:255
%         count1=0;
%         for i2=0:m*n-1
%             if luma(i2+1)==j2
%                 count1=count1+1;
%             end
%         end
%             prob2(j2+1)=count1/m*n;
%     end
%     figure(17);
%     plot(prob2);
%     xlabel('gray levels after mean');
%      ylabel('luma component equilized image after mean');
%      title('processed luma image after mean');
%     ommmmm=p1+p;
%     figure(7);
%      colormap('gray');
%       xlabel('gray level');
%      ylabel('combined lower and upper half luma component equilized image');
%      title('combined luma image');
%     image(ommmmm);
% % % %      cat1=cat(3,om,cb,cr);
% % % %     figure(14);
% % % %     imshow(cat1);
% % % %     xlabel('gray level(ycbcr)');
% % % %      ylabel('combined lower and upper half luma,cromablue,croma red component equilized image');
% % % %      title('luma croma b and r color processed image');
% % % %     catconversion=ycbcr2rgb(cat1);
% % % %     figure(15);
% % % %     imshow(catconversion);
%     xlabel('gray level(rgb)');
%      ylabel('combined lower and upper half  RGB component equilized image');
%      title('converted from ycbcr2rgb color(RGB) processed image');
%      YM=mean(mean(om));
%      AMBE=abs(XM-YM);
%      disp('Absolute mean brightness error=');
%      disp(AMBE);
%      MSE = MeanSquareError(luma, om);
%     % figure(16);
% disp('Mean Square Error = ');
% disp(MSE);
% PSNR = PeakSignaltoNoiseRatio(luma, om);
% %figure(17);
% disp('Peak Signal to Noise Ratio = ');
% disp(PSNR);
% E=entropy(uint8(om));
% disp('Entropy=');
% disp(E);
%      figure(16);
%      plot(imhist(om));


function [fluxROOM, solutionROOM, totalFluxDiff] = linearROOM(model, fluxWT, rxnKO, varargin)
% Performs a LP version of the ROOM (Regulatory on/off minimization of
% metabolic flux changes) approach
%
% USAGE:
%
%    [fluxROOM, solutionROOM, totalFluxDiff] = linearROOM(model, WTflux, rxnKO, delta, epsilon, printLevel)
%
% INPUTS:
%    model:            Metabolic model
%    fluxWT:           Numeric array with flux distribution of wild type
%    rxnKO:            List of perturbations performed to the model
%                      (reactions that are eliminated)
%
% OPTIONAL INPUTS:
%    delta:            Multiplicative tol for flux change (Default = 0.03)
%    epsilon:          Additive tolerance for flux change (Default = 0.001)
%    printLevel:       Verbose output (Default = 1)
%
% OUTPUTS:
%    fluxROOM:         Flux distribution after ROOM calculation
%    solutionROOM:     Solution structure
%    totalFluxDiff:    Euclidean distance of ROOM objective, i.e.
%                      :math:`\sum (v_{wt}-v_{del})^2`
%
% Solve the following problem:
%
% .. math::
%      min ~&~ \sum y_{i} \\
%          ~&~ S_{del}v_{del} = 0 \\
%          ~&~ lb_{del} \leq v_{del} \leq ub_{del} \\
%          ~&~ for i=1:nRxns\\
%          ~&~ v_{i} - y_{i}(v_{max,i}-w_{wt,i}^u) \leq w_{wt,i}^u \\
%          ~&~ v_{i} - y_{i}(v_{min,i}-w_{wt,i}^l) \geq w_{wt,i}^l \\
%          ~&~ 0 \leq y_{i} \leq 1 \\
%          ~&~ w_{wt,i}^u = w_{wt,i} + \delta |w_{wt,i}| + \epsilon \\
%          ~&~ w_{wt,i}^l = w_{wt,i} - \delta |w_{wt,i}| - \epsilon \\
%
% NOTE::
%
%    The code here has been based on:
%    Shlomi, T., Berkman, O., & Ruppin, E. (2005). Regulatory on/off
%    minimization of metabolic flux changes after genetic perturbations.
%    Proceedings of the National Academy of Sciences, 102(21), 7695-7700
%
% .. Authors:
%       - Luis V. Valcarcel, 23/01/2019, University of Navarra, CIMA & TECNUN School of Engineering.

p = inputParser;
% check required arguments
addRequired(p, 'model');
addRequired(p, 'WTflux', @(x)isnumeric(x)&&isvector(x));
addRequired(p, 'rxnKO', @(x)iscell(x));
% Check optional arguments
addParameter(p, 'delta', 0.03, @(x)isnumeric(x)&&isscalar(x));
addParameter(p, 'epsilon', 0.001, @(x)isnumeric(x)&&isscalar(x));
addParameter(p, 'printLevel', 1, @(x)isnumeric(x)&&isscalar(x));
% extract variables from parser
parse(p, model, fluxWT, rxnKO, varargin{:});
delta = p.Results.delta;
epsilon = p.Results.epsilon;
printLevel = p.Results.printLevel;

% LP solution tolerance
global CBT_LP_PARAMS
if (exist('CBT_LP_PARAMS', 'var'))
    if isfield(CBT_LP_PARAMS, 'objTol')
        tol = CBT_LP_PARAMS.objTol;
    else
        tol = 1e-6;
    end
else
    tol = 1e-6;
end

% Check the inputs
fluxWT = reshape(fluxWT,[],1); % reshape flux vector
assert(numel(model.rxns)==numel(fluxWT), 'This flux distribution has different number of reactions than the model')
assert(norm(model.S * fluxWT)< 10*tol, 'This flux distribution cannot exist in this model')
assert(all(ismember(rxnKO, model.rxns)), 'Some reactions are not in the model')

% Eliminate almost-zero fluxes
fluxWT(abs(fluxWT)<tol) = 0;

% generate auxiliary variables
WT_upperTol = fluxWT + delta*abs(fluxWT) + epsilon;
WT_lowerTol = fluxWT - delta*abs(fluxWT) - epsilon;

%auxiliary variables
[nMets, nRxns] = size(model.S);

% Generate model
ROOMmodel = model;
new_variables = strcat('binary_',model.rxns);
% add novel variables
ROOMmodel = addCOBRAVariables(ROOMmodel,new_variables,'lb',0,'ub',1);
% add novel constrains
for i=1:nRxns
    ROOMmodel = addCOBRAConstraints(ROOMmodel, {model.rxns{i}, strcat('binary_',model.rxns{i})}, WT_upperTol(i),...
        'c', [+1, -(model.ub(i)-WT_upperTol(i))], 'dsense', 'L');
end
for i=1:nRxns
    ROOMmodel = addCOBRAConstraints(ROOMmodel, {model.rxns{i}, strcat('binary_',model.rxns{i})}, WT_lowerTol(i),...
        'c', [+1, -(model.lb(i)-WT_lowerTol(i))], 'dsense', 'G');
end
% Block reactions in model
ROOMmodel = changeRxnBounds(ROOMmodel, rxnKO, 0, 'b');
% change objective function
ROOMmodel.c(:) = 0;
ROOMmodel.evarc(cellfun(@length,regexp(ROOMmodel.evars,'^binary_'))==1) = 1;
ROOMmodel.osenseStr = 'min';

solutionROOM = optimizeCbModel(ROOMmodel);

if solutionROOM.stat == 1
    fluxROOM = solutionROOM.v(1:nRxns);
    totalFluxDiff = norm(fluxROOM-fluxWT);
else
    fluxROOM = nan(nRxns,1);
    totalFluxDiff = nan;
end

end


classdef spinoperator
%SPINOPERATOR   Abstract class for representing the spatial part of differential
%operators for time-dependent PDEs.
%   SPINOPERATOR is a class for representing the spartial part S of a
%   time-dependent PDE of the form u_t = S(u) = Lu + N(u), where L is a linear
%   operator and N is a nonlinear operator. SPINOP (in 1D), SPINOP2 (in 2D),
%   SPINOP3 (in 3D) and SPINOPSPHERE (on the sphere) are full implementations.
%
% See also SPINOP, SPINOP2, SPINOP3, SPINOPSPHERE.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS PROPERTIES:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    properties ( Access = public )
        domain              % Spatial domain (1x2 DOUBLE in 1D, 1x4 in 2D and on
                            % the sphere, 1x6 in 3D)
        init                % Initial condition (CHEBFUN in 1D, CHEBFUN2 in 2D,
                            % CHEBFUN3 in 3D, SPHEREFUN on the SPHERE,
                            % CHEBMATRIX for systems)
        lin                 % Linear part of the operator (FUNCTION HANDLE)
        nonlin              % Nonlinear part of the operator (FUNCTION HANDLE)
        tspan               % Vector of strictly increasing time
                            % samples (DOUBLE)
    end

    % DEPENDENT PROPERTIES:
    properties ( Access = public, Dependent = true )
        numVars             % Number of unknown functions (1x1 INT)
    end

    % DEPENDENT AND HIDDEN PROPERTIES:
    properties ( Access = public, Hidden = true, Dependent = true )
        nonlinearPartCoeffs % Differential part of the operator in coefficient
                            % space (FUNCTION HANDLE)
        nonlinearPartVals   % Nondifferential part of the operator in value
                            % space (FUNCTION HANDLE)
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% METHODS FOR DEPENDENT PROPERTIES:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods

        % METHOD for numVars:
        function nVars = get.numVars(S)

            nVars = nargin(S.lin);

        end

        % METHOD for nonlinearPartCoeffs:
        function Nc = get.nonlinearPartCoeffs(S)

            % Extract the nonlinear part:
            N = S.nonlin;

            % If N is empty, we are done:
            if ( isempty(N) == 1 )
                Nc = @(u) 0*u;
                return
            end

            % Else, get the variables of the workspace:
            func = functions(N);
            wrk = func.workspace{1};
            names = fieldnames(wrk);
            if ( isempty(names) == 0 )
                lengthNames = size(names, 1);
                for iter = 1:lengthNames
                    eval(sprintf('%s = wrk.(names{iter});', names{iter}));
                end
            end

            % Convert FUNCN to a string STRN:
            strN = func2str(N);

            % Get the number of variables NVARS:
            nVars = nargin(N);

            % For scalar equations in 1D, we support nonlinearities of the form
            % diff(f(u),m) with m>=0:
            if ( isa(S, 'spinop') == 1 && nVars == 1 )

                % Compute the differentiation order to get NC=diff(u,m):
                diffOrderTwoOrGreater = regexp(strN, ',\d*', 'match');
                diffOrderOne = regexp(strN, 'diff(', 'match');
                if ( isempty(diffOrderTwoOrGreater) == 0 )
                    diffOrder = diffOrderTwoOrGreater{1}(2:end);
                elseif ( isempty(diffOrderOne) == 0 )
                    diffOrder = '1';
                else
                    diffOrder = '0';
                end
                Nc = ['@(u) diff(u,', diffOrder, ')'];
                Nc = eval(Nc);

            % For scalar equations in 2D/3D and on the sphere, and for systems
            % of equations in 1D/2D/3D and on the sphere, we only support
            % nonlinearities of the form f_i(u_1,...,u_n), i.e., with no
            % differentiation, so Nc=1:
            else

                % Nc=1:
                Nc = 1;

            end

        end

        % METHOD for nonlinearPartVals:
        function Nv = get.nonlinearPartVals(S)

            % Extract the nonlinear part:
            N = S.nonlin;

            % If N is empty, we are done:
            if ( isempty(N) == 1 )
                Nv = @(u) 0*u;
                return
            end

            % Else, get the variables of the workspace:
            func = functions(N);
            wrk = func.workspace{1};
            names = fieldnames(wrk);
            if ( isempty(names) == 0 )
                lengthNames = size(names, 1);
                for iter = 1:lengthNames
                    eval(sprintf('%s = wrk.(names{iter});', names{iter}));
                end
            end

            % Convert FUNCN to a string STRN:
            strN = func2str(N);

            % Get the number of variables NVARS:
            nVars = nargin(N);

            % For scalar equations in 1D, we support nonlinearities of the form
            % diff(f(u),m) with m>=0:
            if ( isa(S, 'spinop') == 1 && nVars == 1 )

                % Get rid of the differentiation part in STRN to get NV=f(u):
                oldString = {'diff', ',\d*)'};
                newString = {'', ')'};
                Nv = regexprep(strN, oldString, newString);
                Nv = eval(Nv);

            % For scalar equations in 2D/3D and on the sphere, and for systems
            % of equations in 1D/2D/3D and on the sphere, we only support
            % nonlinearities of the form f_i(u_1,...,u_n), i.e., with no
            % differentiation, so Nv=N:
            else

                % Nv=N:
                strNv = func2str(N);

                % We're going to relabel the variables, e.g., @(u,v) u + v is
                % going to be relabelled @(u) u(1:length(u)/2) + ...
                % u(length(u)/2+1:end). First, get the names of the variables:
                openParenthesis = strfind(strNv, '(');
                openParenthesis = openParenthesis(1);
                closeParenthesis = strfind(strNv, ')');
                closeParenthesis = closeParenthesis(1);
                variablesNames = strNv(openParenthesis+1:closeParenthesis-1);
                variablesNames = regexp(variablesNames,  ',', 'split');

                % Second, relabel the variables:
                strNv = strNv(closeParenthesis+1:end);
                for iter = 1:nVars
                    idx1 = [num2str(rats((iter-1)/nVars)), '*', 'length(u)', '+1'];
                    idx2 = [num2str(rats(iter/nVars)), '*', 'length(u)'];
                    strNvNew = strrep(strNv, variablesNames{iter}, ...
                        ['u(', idx1, ':', idx2, ',:,:)']);
                    strNv = strNvNew;
                end
                Nv = ['@(u)', strNvNew];
                Nv = eval(Nv);

            end
        end

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% ABSTRACT AND NON-STATIC METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Abstract = true, Static = false )

        % Discretize a SPINOPERATOR:
        [L, Nc] = discretize(S, N)

        % Returns indexes for dealiasing procedure (2/3-rule):
        idx = getDealiasingIndexes(S, N, nVars)

        % Returns the type of CHEBFUN on which the SPINOPERATOR acts:
        out = getChebfunType(S)

        % Returns the transform coeffs -> values:
        F = getCoeffs2ValsTransform(S)

        % Returns the spatial dimension:
        dim = getDimension(S)

        % Returns a grid correspoding to a SPINOPRERATOR object:
        grid = getGrid(S, N, dom)

        % Returns the adequate SPINPREFERENCE object:
        pref = getPreference(S)

        % Returns the transform values -> coeffs:
        F = getVals2CoeffsTransform(S)

        % Returns 1 if the linear part of the SPINOPERATOR is diagonal,
        % 0 otherwise:
        out = isDiag(S)

        % Initialize a movie when solving a PDE specified by a SPINOPERATOR:
        [p, opts] = initializeMovie(S, dt, pref, v, compGrid, plotGrid)

        % Update the movie when solving a PDE specified by a SPINOPERATOR:
        opts = updateMovie(S, dt, p, options, t, v, compGrid, plotGrid)

        % Reshape the data that will be used for constructing the solution at
        % the end of the time-stepping: (For example, for SPINOPSPEHRE, it
        % extracts half of the data, since the data has been doubled-up with
        % the DFS method.)
        data = reshapeData(S, data, nVars)

        % Add the (repeated) endpoints to a periodic grid:
        grid = reshapeGrid(S, grid)

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CONCRETE AND STATIC METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Abstract = false, Static = true )

        % Solve a PDE defined by a SPINOPERATOR:
        [uout, tout, computingTime] = solvepde(varargin)

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CONCRETE AND NON-STATIC METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Abstract = false, Static = false )

        % Create a contour around each eigenvalue of the linear part of a
        % SPINOPERATOR:
        LR = computeLR(S, dt, L, M)

    end

end


function [ ap, kpvt, rcond, z ] = sspco ( ap, n )

%*****************************************************************************80
%
%% SSPCO factors a real symmetric matrix stored in packed form.
%
%  Discussion:
%
%    SSPCO uses elimination with symmetric pivoting and estimates
%    the condition of the matrix.
%
%    If RCOND is not needed, SSPFA is slightly faster.
%
%    To solve A*X = B, follow SSPCO by SSPSL.
%
%    To compute inverse(A)*C, follow SSPCO by SSPSL.
%
%    To compute inverse(A), follow SSPCO by SSPDI.
%
%    To compute determinant(A), follow SSPCO by SSPDI.
%
%    To compute inertia(A), follow SSPCO by SSPDI.
%
%  Packed storage:
%
%    The following program segment will pack the upper triangle of a
%    symmetric matrix.
%
%      k = 0
%      do j = 1, n
%        do i = 1, j
%          k = k + 1
%          ap(k) = a(i,j)
%        end
%      end
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real AP(N*(N+1)/2), the packed form of a symmetric matrix A.  The
%    columns of the upper triangle are stored sequentially in a one-dimensional array.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real AP(N*(N+1)/2), a block diagonal matrix and the multipliers
%    which were used to obtain it, stored in packed form.  The
%    factorization can be written A = U*D*U' where U is a product of
%    permutation and unit upper triangular matrices, U' is the transpose
%    of U, and D is block diagonal with 1 by 1 and 2 by 2 blocks.
%
%    Output, integer KPVT(N), the pivot indices.
%
%    Output, real RCOND, an estimate of the reciprocal condition
%    of A.  For the system A*X = B, relative perturbations in A and B of size
%    EPSILON may cause relative perturbations in X of size EPSILON/RCOND.
%    If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate underflows.
%
%    Output, real Z(N) a work vector whose contents are usually
%    unimportant.  If A is close to a singular matrix, then Z is an
%    approximate null vector in the sense that
%      norm(A*Z) = RCOND * norm(A) * norm(Z).
%

%
%  Find norm of A using only upper half.
%
  j1 = 1;
  for j = 1 : n
    z(j) = sasum ( j, ap(j1:j1+j-1), 1 );
    ij = j1;
    j1 = j1 + j;
    for i = 1 : j-1
      z(i) = z(i) + abs ( ap(ij) );
      ij = ij + 1;
    end
  end

  anorm = max ( z(1:n) );
%
%  Factor.
%
  [ ap, kpvt, info ] = sspfa ( ap, n );
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A*Z = Y and A*Y = E.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where U*D*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve U * D * W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  k = n;
  ik = floor ( ( n * ( n - 1 ) ) / 2 );

  while ( k ~= 0 )

    kk = ik + k;
    ikm1 = ik - ( k - 1 );

    if ( kpvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    kp = abs ( kpvt(k) );
    kps = k + 1 - ks;

    if ( kp ~= kps )
      t = z(kps);
      z(kps) = z(kp);
      z(kp) = t;
    end

    if ( z(k) ~= 0.0 )
      ek = abs ( ek ) * r4_sign ( z(k) );
    end

    z(k) = z(k) + ek;
    z(1:k-ks) = saxpy ( k-ks, z(k), ap(ik+1:ik+k-ks), 1, z(1:k-ks), 1 );

    if ( ks ~= 1 )
      if ( z(k-1) ~= 0.0 )
        ek = abs ( ek ) * r4_sign ( z(k-1) );
      end
      z(k-1) = z(k-1) + ek;
      z(1:k-ks) = saxpy ( k-ks, z(k-1), ap(ikm1+1:ikm1+k-ks), 1, z(1:k-ks), 1 );
    end

    if ( ks ~= 2 )

      if ( abs ( ap(kk) ) < abs ( z(k) ) )
        s = abs ( ap(kk) ) / abs ( z(k) );
        z(1:n) = s * z(1:n);
        ek = s * ek;
      end

      if ( ap(kk) ~= 0.0 )
        z(k) = z(k) / ap(kk);
      else
        z(k) = 1.0;
      end

    else

      km1k = ik + k - 1;
      km1km1 = ikm1 + k - 1;
      ak = ap(kk) / ap(km1k);
      akm1 = ap(km1km1) / ap(km1k);
      bk = z(k) / ap(km1k);
      bkm1 = z(k-1) / ap(km1k);
      denom = ak * akm1 - 1.0;
      z(k) = ( akm1 * bk - bkm1 ) / denom;
      z(k-1) = ( ak * bkm1 - bk ) / denom;

    end

    k = k - ks;
    ik = ik - k;
    if ( ks == 2 )
      ik = ik - ( k + 1 );
    end

  end

  z(1:n) = z(1:n) / sasum ( n, z(1:n), 1 );
%
%  Solve U' * Y = W.
%
  k = 1;
  ik = 0;

  while ( k <= n )

    if ( kpvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    if ( k ~= 1 )

      z(k) = z(k) + sdot ( k-1, ap(ik+1:ik+k-1), 1, z(1:k-1), 1 );
      ikp1 = ik + k;

      if ( ks == 2 )
        z(k+1) = z(k+1) + sdot ( k-1, ap(ikp1+1:ikp1+k-1), 1, z(1:k-1), 1 );
      end

      kp = abs ( kpvt(k) );

      if ( kp ~= k )
        t = z(k);
        z(k) = z(kp);
        z(kp) = t;
      end

    end

    ik = ik + k;
    if ( ks == 2 )
      ik = ik + ( k + 1 );
    end
    k = k + ks;

  end

  s = 1.0 / sasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = 1.0;
%
%  Solve U * D * V = Y.
%
  k = n;

  ik = floor ( n * ( n - 1 ) / 2 );

  while ( 0 < k )

    kk = ik + k;
    ikm1 = ik - ( k - 1 );

    if ( kpvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    if ( k ~= ks )

      kp = abs ( kpvt(k) );
      kps = k + 1 - ks;

      if ( kp ~= kps )
        t = z(kps);
        z(kps) = z(kp);
        z(kp) = t;
      end

      z(1:k-ks) = saxpy ( k-ks, z(k), ap(ik+1:ik+k-ks), 1, z(1:k-ks), 1 );

      if ( ks == 2 )
        z(1:k-ks) = saxpy ( k-ks, z(k-1), ap(ikm1+1:ikm1+k-ks), 1, z(1:k-ks), 1 );
      end

    end

    if ( ks ~= 2 )

      if ( abs ( ap(kk) ) < abs ( z(k) ) )
        s = abs ( ap(kk) ) / abs ( z(k) );
        z(1:n) = s * z(1:n);
        ynorm = s * ynorm;
      end

      if ( ap(kk) ~= 0.0 )
        z(k) = z(k) / ap(kk);
      else
        z(k) = 1.0;
      end

    else

      km1k = ik + k - 1;
      km1km1 = ikm1 + k - 1;
      ak = ap(kk) / ap(km1k);
      akm1 = ap(km1km1) / ap(km1k);
      bk = z(k) / ap(km1k);
      bkm1 = z(k-1) / ap(km1k);
      denom = ak * akm1 - 1.0;
      z(k) = ( akm1 * bk - bkm1 ) / denom;
      z(k-1) = ( ak * bkm1 - bk ) / denom;

    end

    k = k - ks;
    ik = ik - k;
    if ( ks == 2 )
      ik = ik - ( k + 1 );
    end

  end

  s = 1.0 / sasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;
%
%  Solve U' * Z = V.
%
  k = 1;
  ik = 0;

  while ( k <= n )

    if ( kpvt(k) < 0 )
      ks = 2;
    else
      ks = 1;
    end

    if ( k ~= 1 )

      z(k) = z(k) + sdot ( k-1, ap(ik+1:ik+k-1), 1, z(1:k-1), 1 );
      ikp1 = ik + k;

      if ( ks == 2 )
        z(k+1) = z(k+1) + sdot ( k-1, ap(ikp1+1:ikp1+k-1), 1, z(1:k-1), 1 );
      end

      kp = abs ( kpvt(k) );

      if ( kp ~= k )
        t = z(k);
        z(k) = z(kp);
        z(kp) = t;
      end

    end

    ik = ik + k;
    if ( ks == 2 )
      ik = ik + ( k + 1 );
    end
    k = k + ks;

  end
%
%  Make ZNORM = 1.0.
%
  s = 1.0 / sasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end