function out = DN_pleft(y,th)
% DN_pleft  Distance from the mean at which a given proportion of data are
%                   more distant.
%
% Measures the maximum distance from the mean at which a given fixed proportion,
% p, of the time-series data points are further.
% Normalizes by the standard deviation of the time series
% (could generalize to separate positive and negative deviations in future)
% Uses the quantile function from Matlab's Statistics Toolbox
%
%---INPUTS:
% y, the input data vector
% th, the proportion of data further than p from the mean
%           (output p, normalized by standard deviation)

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

if nargin < 2 || isempty(th)
    th = 0.1; % default
end

p = quantile(abs(y-mean(y)),1-th);

% A proportion, th, of the data lie further than p from the mean
out = p/std(y);

end


function op = linop_vec( sz )
%LINOP_VEC Matrix to vector reshape operator
%OP = LINOP_VEC( SZ )
%    Constructs a TFOCS-compatible linear operator that reduces a matrix
%    variable to a vector version using column-major order.
%    This is equivalent to X(:)
%    The transpose operator will reshape a vector into a matrix.
%
%    The input SZ should of the form [M,N] where [M,N] describe the
%    size of the matrix variable.  The ouput vector will be of
%    length M*N.  If SZ is a single entry, then M = N is assumed.
%    For advanced usage with multidimensional arrays,
%    use the more general linop_reshape function instead.
%
%   To do the reverse operation (from vector to matrix),
%   use this function together with linop_adjoint.
%
%   See also linop_reshape

if numel(sz) > 2, error('must supply a 2-entry vector'); end
if numel(sz) == 1, sz = [sz(1),sz(1)]; end

% Switch conventions of the size variable:
sz = { [sz(1),sz(2)], [sz(1)*sz(2),1] };

op = @(x,mode)linop_handles_vec( sz, x, mode );

function y = linop_handles_vec(sz, x, mode )
switch mode,
    case 0, y = sz;
    case 1, y = x(:);
    case 2,
        MN = sz{1}; M = MN(1); N = MN(2);
        y = reshape( x, M, N );

end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function [rhsHx, rhsHy, rhsEz] = MaxwellCurvedRHS2D(cinfo, Hx,Hy,Ez)

% function [rhsHx, rhsHy, rhsEz] = MaxwellCurvedRHS2D(cinfo, Hx,Hy,Ez)
% Purpose  : Evaluate RHS flux in 2D Maxwell TM form

Globals2D;

[rhsHx,rhsHy,rhsEz] = MaxwellRHS2D(Hx, Hy, Ez);

% correct residuals at each curved element
Ncinfo = length(cinfo);
for n=1:Ncinfo

  % for each curved element computed L2 derivatives via cubature
  cur = cinfo(n); k1 = cur.elmt; cDx = cur.Dx; cDy = cur.Dy;

  rhsHx(:,k1) = -cDy*Ez(:,k1);
  rhsHy(:,k1) =  cDx*Ez(:,k1);
  rhsEz(:,k1) =  cDx*Hy(:,k1) - cDy*Hx(:,k1);

  % for each face of each curved element use Gauss quadrature based lifts
  for f1=1:Nfaces
    k2 = EToE(k1,f1);
    gnx = cur.gnx(:,f1); gny = cur.gny(:,f1);
    gVM = cur.gVM(:,:,f1); gVP = cur.gVP(:,:,f1);
    glift = cur.glift(:,:,f1);

    % compute difference of solution traces at Gauss nodes
    gdHx = gVM*Hx(:,k1) - gVP*Hx(:,k2);
    gdHy = gVM*Hy(:,k1) - gVP*Hy(:,k2);
    gdEz = gVM*Ez(:,k1) - gVP*Ez(:,k2);

    % correct jump at Gauss nodes on domain boundary faces
    if(k1==k2)
      gdHx = 0*gdHx; gdHy = 0*gdHy; gdEz = 2*gVM*Ez(:,k1);
    end

    % perform upwinding
    gndotdH =  gnx.*gdHx+gny.*gdHy;
    fluxHx =  gny.*gdEz + gndotdH.*gnx-gdHx;
    fluxHy = -gnx.*gdEz + gndotdH.*gny-gdHy;
    fluxEz = -gnx.*gdHy + gny.*gdHx   -gdEz;

    % lift flux terms using Gauss based lift operator
    rhsHx(:,k1) = rhsHx(:,k1) + glift*fluxHx/2;
    rhsHy(:,k1) = rhsHy(:,k1) + glift*fluxHy/2;
    rhsEz(:,k1) = rhsEz(:,k1) + glift*fluxEz/2;
  end
end
return;


% transform radians to degrees

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function deg = rad2deg(rad)

deg=rad*180/pi;


% Copyright (C) 1993-2014, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

%%begin
% In the field of robotics there are many possible ways of representing
% positions and orientations, but the homogeneous transformation is well
% matched to MATLABs powerful tools for matrix manipulation.
%
% Homogeneous transformations describe the relationships between Cartesian
% coordinate frames in terms of translation and orientation.

%  A pure translation of 0.5m in the X direction is represented by

transl(0.5, 0.0, 0.0)

% a rotation of 90degrees about the Y axis by

troty(pi/2)

% and a rotation of -90degrees about the Z axis by

trotz(-pi/2)

%  these may be concatenated by multiplication

t = transl(0.5, 0.0, 0.0) * troty(pi/2) * trotz(-pi/2)

%
% If this transformation represented the origin of a new coordinate frame with respect
% to the world frame origin (0, 0, 0), that new origin would be given by

t * [0 0 0 1]'

% the orientation of the new coordinate frame may be expressed in terms of
% Euler angles

tr2eul(t)

% or roll/pitch/yaw angles

tr2rpy(t)

% It is important to note that tranform multiplication is in general not
% commutative as shown by the following example

trotx(pi/2) * trotz(-pi/8)
trotz(-pi/8) * trotx(pi/2)


function m = medianw( x, w, dim )
% Fast weighted median.
%
% Computes the weighted median of a set of samples.
%  http://en.wikipedia.org/wiki/Weighted_median
% "A weighted median of a sample is the 50% weighted percentile."
% For matrices computes median along each column (or dimension dim).
% If all weights are equal to 1 gives identical results to median.
%
% USAGE
%  m = medianw( x, w, [dim] )
%
% INPUTS
%  x      - vector or array of samples
%  w      - vector or array of weights
%  dim    - dimension along which to compute median
%
% OUTPUTS
%  m      - weighted median value of x
%
% EXAMPLE - simple toy example
%  x=[1 2 3]; w=[1 1 5]; medianw(x,w)
%
% EXAMPLE - comparison to median
%  n=randi(100); m=randi(100);
%  x=rand(n,m); w=ones(n,m);
%  m1=median(x); m2=medianw(x,w);
%  assert(isequal(m1,m2))
%
% See also median
%
% Piotr's Computer Vision Matlab Toolbox      Version 3.24
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

if(nargin<3), dim=find(size(x)~=1,1); end
d=dim; nd=ndims(x); n=numel(x);

if( n==1 || size(x,d)==1 )
  m=x;
elseif( length(x)==n )
  [x,o]=sort(x); w=w(o); w=cumsum(w);
  w=w/w(end); [~,j]=min(w<=.5);
  if(j==1 || w(j-1)~=.5), m=x(j);
  else m=(x(j-1)+x(j))/2; end
else
  if(d>1), p=[d 1:d-1 d+1:nd]; x=permute(x,p); w=permute(w,p); end
  [x,o]=sort(x); w=w(o); w=cumsum(w); is={':'}; is=is(ones(1,nd-1));
  w=bsxfun(@rdivide,w,w(end,is{:})); [~,j]=min(w<=.5);
  s=size(x); s=reshape(((1:n/s(1))-1)*s(1),size(j));
  j0=max(1,j-1); j0=j0+s; j=j+s;
  same=w(j0)~=.5; j0(same)=j(same); m=(x(j0)+x(j))/2;
  if(d>1), p=[2:d 1 d+1:nd]; m=permute(m,p); end
end

end


function [x,y,X,Y] = simplex(func, x, kernel, opts, varargin)
%
% SIMPLEX - multidimensional unconstrained non-linear optimsiation
%
%    X = SIMPLEX(FUNC,X) finds a local minumum of a function, via a function
%    handle FUNC, starting from an initial point X.  The local minimum is
%    located via the Nelder-Mead simplex algorithm [1], which does not require
%    any gradient information.
%
%    [X,Y] = SIMPLEX(FUNC,X) also returns the value of the function, Y, at
%    the local minimum, X.
%
%    X = SIMPLEX(FUNC,X,OPTS) allows the optimisation parameters to be
%    specified via a structure, OPTS, with members
%
%       opts.Chi         - Parameter governing expansion steps
%       opts.Delta       - Parameter governing size of initial simplex.
%       opts.Gamma       - Parameter governing contraction steps.
%       opts.Rho         - Parameter governing reflection steps.
%       opts.Sigma       - Parameter governing shrinkage steps.
%       opts.MaxIter     - Maximum number of optimisation steps.
%       opts.MaxFunEvals - Maximum number of function evaluations.
%       opts.TolFun      - Stopping criterion based on the relative change in
%                          value of the function in each step.
%       opts.TolX        - Stopping criterion based on the change in the
%                          minimiser in each step.
%
%    OPTS = SIMPLEX() returns a structure containing the default optimisation
%    parameters, with the following values:
%
%       opts.Chi         = 2
%       opts.Delta       = 0.01
%       opts.Gamma       = 0.5
%       opts.Rho         = 1
%       opts.Sigma       = 0.5
%       opts.MaxIter     = 200
%       opts.MaxFunEvals = 1000
%       opts.TolFun      = 1e-6
%       opts.TolX        = 1e-6
%
%    X = SIMPLEX(FUNC,X,OPTS, P1, P2, ...) allows addinal parameters to be
%    passed to the function to be minimised.
%
%    [X,Y,XX,YY] = SIMPLEX(FUNC, X) also returns in XX all of the values of
%    X evaluated during the optimisation process and in YY the corresponding
%    values of the function.
%
%    References:
%
%       [1] J. A. Nelder and R. Mead, "A simplex method for function
%           minimization", Computer Journal, 7:308-313, 1965.
%
% Description : Simple implementation of the Nelder-Mead simplex optimisation
%               algorithm [1].  Similar to the fminsearch routine from the
%               MATLAB optimisation toolbox.
%
% References  : [1] J. A. Nelder and R. Mead, "A simplex method for function
%                   minimization", Computer Journal, 7:308-313, 1965.
%
% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

% use default optimisation parameters if none given

if nargin < 4

    opts.Chi         = 2;
    opts.Delta       = 1.2;%0.01;
    opts.Gamma       = 0.5;
    opts.Rho         = 1;
    opts.Sigma       = 0.5;
    opts.MaxIter     = 15;%200;
    opts.MaxFunEvals = 25;
    opts.TolFun      = 1e-6;
    opts.TolX        = 1e-6;

end

% return structure containing default optimisation parameters

if nargin == 0

    x = opts;

    return

end

% get initial parameters

x = x(:);
n = length(x);
x = repmat(x', n+1, 1);
y = zeros(n+1, 1);

% form initial simplex

for i=1:n

    x(i,i) = x(i,i) + opts.Delta;
    y(i)   = func(x(i,:), varargin{:});

end

y(n+1) = func(x(n+1,:), varargin{:});
X      = x;
Y      = y;
count  = n+1;

[~, idx] = min(y);
xx        = x(idx,:);

if strcmp(kernel,'RBF_kernel') || strcmp(kernel,'wav_kernel')|| strcmp(kernel,'RBF4_kernel')
    k = 1;
    format = '  % 4d        % 4d     %e     %.4f        %.4f      %s \n';
    fprintf(1, '\n Iteration   Func-count    min f(x)    log(gamma)    log(sig2)    Procedure\n\n');
    fprintf(1, format, 1, count, min(y),xx(1),xx(2),'initial');
elseif strcmp(kernel,'lin_kernel')
    k = 2;
    format = '  % 4d        % 4d     %e     %.4f           %s \n';
    fprintf(1, '\n Iteration   Func-count    min f(x)    log(gamma)      Procedure\n\n');
    fprintf(1, format, 1, count, min(y),xx(1),'initial');
elseif strcmp(kernel,'poly_kernel')
    k = 3;
    format = '  % 4d        % 4d     %e     %.4f         %.4f         %s   \n';
    fprintf(1, '\n Iteration   Func-count    min f(x)    log(gamma)      log(t)        Procedure\n\n');
    fprintf(1, format, 1, count, min(y),xx(1),xx(2),'initial');
end

% iterative improvement

for i=2:opts.MaxIter

    % order

    [y,idx] = sort(y);
    x       = x(idx,:);

    % reflect

    centroid = mean(x(1:end-1,:));
    x_r      = centroid + opts.Rho*(centroid - x(end,:));
    y_r      = func(x_r, varargin{:});
    count    = count + 1;
    X        = [X ; x_r];
    Y        = [Y ; y_r];

    if y_r >= y(1) && y_r < y(end-1)

        % accept reflection point

        x(end,:) = x_r;
        y(end)   = y_r;
        [~, idx] = min(y);
        xx = x(idx,:);
        if k==1 || k==3
            fprintf(1, format, i, count, min(y),xx(1),xx(2),'reflect');
        elseif k==2
            fprintf(1, format, i, count, min(y),xx(1),'reflect');
        end

    else

        if y_r < y(1)

            % expand

            x_e   = centroid + opts.Chi*(x_r - centroid);
            y_e   = func(x_e, varargin{:});
            count = count + 1;
            X     = [X ; x_e];
            Y     = [Y ; y_e];

            if y_e < y_r

                % accept expansion point

                x(end,:) = x_e;
                y(end)   = y_e;
                [~, idx] = min(y);
                xx = x(idx,:);
                if k==1 || k==3
                   fprintf(1, format, i, count, min(y),xx(1),xx(2), 'expand');
                elseif k==2
                   fprintf(1, format, i, count, min(y),xx(1), 'expand');
                end

            else

                % accept reflection point

                x(end,:) = x_r;
                y(end)   = y_r;
                [~, idx] = min(y);
                xx = x(idx,:);
                if k==1 || k==3
                   fprintf(1, format, i, count, min(y),xx(1),xx(2), 'reflect');
                elseif k==2
                   fprintf(1, format, i, count, min(y),xx(1), 'reflect');
                end

            end

        else

            % contract

            shrink = 0;

            if y(end-1) <= y_r && y_r < y(end)

                % contract outside

                x_c   = centroid + opts.Gamma*(x_r - centroid);
                y_c   = func(x_c, varargin{:});
                count = count + 1;
                X     = [X ; x_c];
                Y     = [Y ; y_c];

                if y_c <= y_r

                    % accept contraction point

                    x(end,:) = x_c;
                    y(end)   = y_c;
                    [~, idx] = min(y);
                    xx = x(idx,:);
                    if k==1 || k==3
                        fprintf(1, format, i, count, min(y),xx(1),xx(2),'contract outside');
                    elseif k==2
                        fprintf(1, format, i, count, min(y),xx(1),'contract outside');
                    end

                else

                    shrink = 1;

                end

            else

                % contract inside

                x_c   = centroid + opts.Gamma*(centroid - x(end,:));
                y_c   = func(x_c, varargin{:});
                count = count + 1;
                X     = [X ; x_c];
                Y     = [Y ; y_c];

                if y_c <= y(end)

                    % accept contraction point

                    x(end,:) = x_c;
                    y(end)   = y_c;
                    [~, idx] = min(y);
                    xx = x(idx,:);
                    if k==1 || k==3
                        fprintf(1, format, i, count, min(y),xx(1),xx(2),'contract inside');
                    elseif k==2
                        fprintf(1, format, i, count, min(y),xx(1),'contract inside');
                    end

                else

                    shrink = 1;

                end

            end

            if shrink

                % shrink

                for j=2:n+1

                    x(j,:) = x(1,:) + opts.Sigma*(x(j,:) - x(1,:));
                    y(j)   = func(x(j,:), varargin{:});
                    count  = count + 1;
                    X      = [X ; x(j,:)];
                    Y      = [Y ; y(j)];

                end
                [~, idx] = min(y);
                xx = x(idx,:);
                if k==1 || k==3
                    fprintf(1, format, i, count, min(y),xx(1),xx(2),'shrink');
                elseif k==2
                    fprintf(1, format, i, count, min(y),xx(1),'shrink');
                end

            end

        end

    end

    % evaluate stopping criterion

    if max(abs(min(x) - max(x))) < opts.TolX

        fprintf(1, 'optimisation terminated sucessfully (TolX criterion)\n');

        break;

    end

    if abs(max(y) - min(y))/max(abs(y))  < opts.TolFun

        fprintf(1, 'optimisation terminated sucessfully (TolFun criterion)\n\n');

        break;

    end
    if count  > opts.MaxFunEvals

        fprintf(1, 'optimisation terminated sucessfully (MaxFunEvals criterion)\n\n');

        break;

    end

end

if i == opts.MaxIter

    fprintf(1, 'Warning : maximim number of iterations exceeded\n\n');

end

% update model structure

[y, idx] = min(y);
x        = x(idx,:);

% bye bye...


function [AF,BF]=wpfbtbounds(wt,varargin)
%WPFBTBOUNDS Frame bounds of WPFBT
%   Usage: fcond=wpfbtbounds(wt,L);
%          [A,B]=wpfbtbounds(wt,L);
%          [...]=wpfbtbounds(wt);
%
%   `wpfbtbounds(wt,L)` calculates the ratio $B/A$ of the frame bounds
%   of the wavelet packet filterbank specified by *wt* for a system of length
%   *L*. The ratio is a measure of the stability of the system.
%
%   `wpfbtbounds(wt)` does the same, except *L* is chosen to be the next
%   compatible length bigger than the longest filter from the identical
%   filterbank.
%
%   `[A,B]=wpfbtbounds(...)` returns the lower and upper frame bounds
%   explicitly.
%
%   See |wfbt| for explanation of parameter *wt*.
%
%   Additionally, the function accepts the following flags:
%
%   `'intsqrt'`(default),`'intnoscale'`, `'intscale'`
%       The filters in the filterbank tree are scaled to reflect the
%       behavior of |wpfbt| and |iwpfbt| with the same flags.
%
%   `'scaling_notset'`(default),`'noscale'`,`'scale'`,`'sqrt'`
%     Support for scaling flags as described in |uwpfbt|. By default,
%     the bounds are caltulated for |wpfbt|, passing any of the non-default
%     flags results in bounds for |uwpfbt|.
%
%   See also: wpfbt, filterbankbounds

% AUTHOR: Zdenek Prusa

complainif_notenoughargs(nargin,1,'WPFBTBOUNDS');

definput.keyvals.L = [];
definput.flags.interscaling = {'intsqrt', 'intscale', 'intnoscale'};
definput.import = {'uwfbtcommon'};
definput.importdefaults = {'scaling_notset'};
[flags,~,L]=ltfatarghelper({'L'},definput,varargin);

wt = wfbtinit({'strict',wt},'nat');

if ~isempty(L) && flags.do_scaling_notset
   if L~=wfbtlength(L,wt)
       error(['%s: Specified length L is incompatible with the length of ' ...
              'the time shifts.'],upper(mfilename));
   end;
end

for ii=1:numel(wt.nodes)
   a = wt.nodes{ii}.a;
   assert(all(a==a(1)),sprintf(['%s: One of the basic wavelet ',...
                                'filterbanks is not uniform.'],...
                                upper(mfilename)));
end

% Do the equivalent filterbank using multirate identity property
[gmultid,amultid] = wpfbt2filterbank(wt,flags.interscaling,flags.scaling);

if isempty(L)
   L = wfbtlength(max(cellfun(@(gEl) numel(gEl.h),gmultid)),wt);
end

% Do the equivalent uniform filterbank
if any(amultid~=amultid(1))
   [gu,au] = nonu2ufilterbank(gmultid,amultid);
else
   [gu,au] = deal(gmultid,amultid);
end

if nargout<2
   AF = filterbankbounds(gu,au,L);
elseif nargout == 2
   [AF, BF] = filterbankbounds(gu,au,L);
end


function M=convertPF(P,Pp,isProj)
% Canonical case. Converts from P to F
% I should remove that function
%
% Vincent's Structure From Motion Toolbox      Version 1.1
% Copyright (C) 2008-2011 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the GPL [see external/gpl.txt]

F=Pp; Pp=P;

if isProj
  if isempty(F)
    % Reference: HZ2, p246, Table 9.1
    ep=Pp(:,4);
    M = skew(ep)*Pp(:,1:3);
    return
  else
    % Reference: HZ2, p256, Result 9.14
    [U,S,V] = svd(F); %#ok<NASGU>
    ep = U(:,end);
    M = [ skew(ep)*F ep ];
    return
  end
else    % F has form 14.1, HZ2, p345 :  [ 0 0 a; 0 0 b; c d e ]
  if isempty(F)
    % Reference: HZ2, p348, table 14.1
    M=[ 0 0 Pp(2,3); 0 0 -Pp(1,3); Pp(1,3)*Pp(2,1)-Pp(1,1)*Pp(2,3) ...
      Pp(1,3)*Pp(2,2)-Pp(1,2)*Pp(2,3) Pp(1,3)*Pp(2,4)-Pp(2,3)*P(1,4) ];
    return
  else
    % Reference: HZ2, p348, table 14.1
    M=zeros(3,4); M(3,4)=1;
    M(1,3)=-F(2,3);
    M(2,3)=F(1,3);
    M(1,1)=-(F(3,1)/M(1,3)*M(2,3)/M(1,3)-1)/(1+(M(2,3)/M(1,3))^2);
    M(2,1)=(F(3,1)+M(1,1)*M(2,3))/M(1,3);

    M(1,2)=-((F(3,2)/M(1,3)-1)*M(2,3)/M(1,3))/(1+(M(2,3)/M(1,3))^2);
    M(2,2)=(F(3,2)+M(1,2)*M(2,3))/M(1,3);
    M(1,4)=1; M(2,4)=(F(3,3)+M(2,3)*M(1,4))/M(1,3);
    return
  end
end
error('Bad input');


function Spath = Simulate_SLV_func( N_sim, M, T, S_0, r, q, model, params)
% Simulates Paths of Stochastic Local Volatility Models (basic Euler Scheme for most cases)
% N_sim = # paths
% M = #time steps on [0,T], ie dt =T/M
% Note: returns paths of dimension (N_sim,M+1), since they include S_0
%
% SVLModel:                         (parameters)
%        1 = Heston:                alpha, v0, rho, theta, eta
%        2 = SABR:                  alpha, v0, rho, beta
%        3 = Shifted SABR:          alpha, v0, rho, beta, shift
%        4 = Quadratic SLV:         alpha, v0, rho, theta, eta, a, b, c
%        5 = TanHyp-Heston:         alpha, v0, rho, theta, eta, beta
%        6 = Heston-SABR:           alpha, v0, rho, theta, eta, beta
%
%==============================
% Initialize Common Params/Vectors
%==============================
alpha  = params.alpha;   % (vol of vol)
v0     = params.v0;  % initial vol/var
rho    = params.rho; % covar bewteen asset and vol innovations

Sigmav = alpha; %NOTE: alpha is same as Sigmav (vol of vol)

if model == 1 || model == 4 || model == 5 || model == 6
    theta = params.theta;
    eta   = params.eta;
end

Spath = zeros(N_sim,M+1);
Spath(:,1) = S_0;

dt = T/M;
sqdt = sqrt(dt);
sqdtrho1 = sqdt*rho;
sqdtrho2 = sqdt*sqrt(1-rho^2);

Zeta = r - q;
%==============================
% Simulate Based on Specific StochVol Model
%==============================

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if model == 1 %HESTON MODEL

    expEta = exp(-eta*dt);
    driftv = theta*(1 - expEta);  %analytical drift for variance process
    vOld   = v0*ones(N_sim,1); %used to store variance process
    sqvOld = sqrt(v0)*ones(N_sim,1);

    for m = 1:M
        W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
        Spath(:,m+1) = Spath(:,m).*exp((Zeta - .5*vOld)*dt + sqdt*sqvOld.*W1);  %log scheme
        %Spath(:,m+1) = Spath(:,m)*( (1+r)*dt + sqrt(vOld)*(sqrho1*W1 + sqrho2*W2));  %level scheme
        vNew = driftv + vOld*expEta + Sigmav*sqvOld.*(sqdtrho1*W1 + sqdtrho2*W2);
        %vNew = (sqvOld +.5*Sigmav*(sqdtrho1*W1 + sqdtrho2*W2)).^2 + eta*(theta - vOld)*dt -.25*Sigmav*dt; %Milstein
        vOld = abs(vNew); %Always stays positive, the "reflection" scheme
        %vOld = max(0,vNew); %least biased scheme
        sqvOld = sqrt(vOld);
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif model == 2  % SABR
    beta  = params.beta;
    cons1 = -.5*(alpha)^2*dt;
    vOld  = v0*ones(N_sim,1); %used to store variance process
    for m = 1:M
        W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
        Spath(:,m+1) = max(0, Spath(:,m) + Spath(:,m).^beta.* vOld .*sqdt.*W1);  %level scheme
        vOld = vOld.*exp(cons1 + alpha*(sqdtrho1*W1 + sqdtrho2*W2));
        %vOld = vOld + alpha*vOld.*(sqdtrho1*W1 + sqdtrho2*W2);
    end

%     %%% LOG SCHEME
%     cons2 = 2*(beta - 1);
%     for m = 1:M
%         W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
%         Spath(:,m+1) = Spath(:,m).*exp(-.5*vOld.^2.*Spath(:,m).^cons2*dt + Spath(:,m).^(beta-1).* vOld .*sqdt.*W1);  %level scheme
%         vOld = vOld.*exp(cons1 + alpha*(sqdtrho1*W1 + sqdtrho2*W2));
%     end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif model == 3  % SHIFTED SABR
    beta  = params.beta;
    shift = params.shift;
    cons1 = -.5*(alpha)^2*dt;
    vOld  = v0*ones(N_sim,1); %used to store variance process
    for m = 1:M
        W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
        %%% Note: because of shift, -shift <= S_t
        Spath(:,m+1) = max(-shift, Spath(:,m) + (Spath(:,m) + shift).^beta.* vOld .*sqdt.*W1);  %level scheme
        vOld = vOld.*exp(cons1 + alpha*(sqdtrho1*W1 + sqdtrho2*W2));
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif model == 4  % QUADRATIC SLV
    a = params.a; b = params.b; c = params.c;

    expEta = exp(-eta*dt);
    driftv = theta*(1 - expEta);  %analytical drift for variance process
    vOld   = v0*ones(N_sim,1); %used to store variance process
    sqvOld = sqrt(v0)*ones(N_sim,1);

    drift = 1+ Zeta*dt;
    for m = 1:M
        W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions

        Spath(:,m+1) = max(0,Spath(:,m)*drift + sqdt*sqvOld.*W1.*(Spath(:,m).*(a*Spath(:,m) +b) + c));

        %Spath(:,m+1) = Spath(:,m)*( (1+r)*dt + sqrt(vOld)*(sqrho1*W1 + sqrho2*W2));  %level scheme
        vNew = driftv + vOld*expEta + Sigmav*sqvOld.*(sqdtrho1*W1 + sqdtrho2*W2);
        %vNew = (sqvOld +.5*Sigmav*(sqdtrho1*W1 + sqdtrho2*W2)).^2 + eta*(theta - vOld)*dt -.25*Sigmav*dt; %Milstein
        %vOld = abs(vNew); %Always stays positive, the "reflection" scheme
        vOld = max(0,vNew); %least biased scheme
        sqvOld = sqrt(vOld);
    end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif model == 5  % TAN-HYP-HESTON
    beta = params.beta;

    expEta = exp(-eta*dt);
    driftv = theta*(1 - expEta);  %analytical drift for variance process
    vOld   = v0*ones(N_sim,1); %used to store variance process
    sqvOld = sqrt(v0)*ones(N_sim,1);

    drift = 1+ Zeta*dt;
    for m = 1:M
        W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions

        Spath(:,m+1) = max(0,Spath(:,m)*drift + sqdt*sqvOld.*W1.*tanh(beta*Spath(:,m)));
        vNew = driftv + vOld*expEta + Sigmav*sqvOld.*(sqdtrho1*W1 + sqdtrho2*W2);
        %vNew = (sqvOld +.5*Sigmav*(sqdtrho1*W1 + sqdtrho2*W2)).^2 + eta*(theta - vOld)*dt -.25*Sigmav*dt; %Milstein
        %vOld = abs(vNew); %Always stays positive, the "reflection" scheme
        vOld = max(0,vNew); %least biased scheme
        sqvOld = sqrt(vOld);
    end

elseif model == 6   % HESTON-SABR
    drift = 1+ Zeta*dt;
    expEta = exp(-eta*dt);
    driftv = theta*(1 - expEta);  %analytical drift for variance process
    vOld   = v0*ones(N_sim,1); %used to store variance process
    sqvOld = sqrt(v0)*ones(N_sim,1);

    beta  = params.beta;

    for m = 1:M
        W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions

        %%%% LEVEL scheme
        Spath(:,m+1) = max(0, Spath(:,m)*drift + Spath(:,m).^beta.* sqvOld .*sqdt.*W1);

%         %%%% LOG scheme
%         Spath(:,m+1) = Spath(:,m).*exp(-.5*vOld.^2.*Spath(:,m).^cons2*dt + Spath(:,m).^(beta-1).* sqvOld .*sqdt.*W1);

        vNew = driftv + vOld*expEta + Sigmav*sqvOld.*(sqdtrho1*W1 + sqdtrho2*W2);
        vOld = abs(vNew); %Always stays positive, the "reflection" scheme
        %vOld = max(0,vNew); %least biased scheme
        sqvOld = sqrt(vOld);
    end

%     beta  = params.beta;
%     cons1 = -.5*(alpha)^2*dt;
%     vOld  = v0*ones(N_sim,1); %used to store variance process
%     for m = 1:M
%         W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
%         Spath(:,m+1) = max(0, Spath(:,m) + Spath(:,m).^beta.* vOld .*sqdt.*W1);  %level scheme
%         vOld = vOld.*exp(cons1 + alpha*(sqdtrho1*W1 + sqdtrho2*W2));
%         %vOld = vOld + alpha*vOld.*(sqdtrho1*W1 + sqdtrho2*W2);
%     end
%
%     expEta = exp(-eta*dt);
%     driftv = theta*(1 - expEta);  %analytical drift for variance process
%     vOld   = v0*ones(N_sim,1); %used to store variance process
%     sqvOld = sqrt(v0)*ones(N_sim,1);
%
%     for m = 1:M
%         W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
%         Spath(:,m+1) = Spath(:,m).*exp((Zeta - .5*vOld)*dt + sqdt*sqvOld.*W1);  %log scheme
%         %Spath(:,m+1) = Spath(:,m)*( (1+r)*dt + sqrt(vOld)*(sqrho1*W1 + sqrho2*W2));  %level scheme
%         vNew = driftv + vOld*expEta + Sigmav*sqvOld.*(sqdtrho1*W1 + sqdtrho2*W2);
%         %vNew = (sqvOld +.5*Sigmav*(sqdtrho1*W1 + sqdtrho2*W2)).^2 + eta*(theta - vOld)*dt -.25*Sigmav*dt; %Milstein
%         vOld = abs(vNew); %Always stays positive, the "reflection" scheme
%         %vOld = max(0,vNew); %least biased scheme
%         sqvOld = sqrt(vOld);
%     end

end

% %%%% REFORMULATED VERSION
% etahat = eta*theta; thetahat = (eta + Sigmav^2)/etahat; Sigmavhat = -Sigmav; v0hat = 1/v0;
% vOld = v0hat*ones(N_sim,1); %used to store variance process
% sqvOld = sqrt(v0hat)*ones(N_sim,1);
%
% expkt = exp(-etahat*dt);
% driftv = thetahat*(1 - expkt);  %analytical drift for variance process
% sqtSv = sqdt*Sigmavhat;  %analytical drift for variance process
%
% for m = 1:M
%     W1 = randn(N_sim,1); W2 = randn(N_sim,1);  %Generate two Brownian motions
%
%     Spath(:,m+1) = Spath(:,m).*exp((r - .5*vOld)*dt + (sqrho1*W1 + sqrho2*W2)./sqvOld);  %log scheme
%     %Spath(:,m+1) = Spath(:,m)*( (1+r)*dt + sqrt(vOld)*(sqrho1*W1 + sqrho2*W2));  %level scheme
%
%     vNew = driftv + vOld*expkt + sqtSv*sqvOld.*W1;
%
%     vOld = abs(vNew); %Always stays positive, the "reflection" scheme
%     sqvOld = sqrt(vOld);
% end


function M = getmassmat3(elem2dof,volume,Dlambda,elemType,K)
%% GETMASSMAT Get mass matrix of the finite element space
% M = getmassmat(Dlambda,elem2dof,elemType,K) get mass matrix of the finite element
% space specified by elemType.
%
% The elemType can be:
%
% - "RT0": The lowest order Raviart-Thomas element
% - "ND1': The lowest order Nedelec element
%

if ~exist('K','var'), K = []; end
NT = size(elem2dof,1);

%% ND1: the lowest order edge element
if strcmp(elemType,'ND1')
    NE = double(max(elem2dof(:)));
    DiDj = zeros(NT,4,4);
    for i = 1:4
        for j = i:4
            DiDj(:,i,j) = dot(Dlambda(:,:,i),Dlambda(:,:,j),2);
            DiDj(:,j,i) = DiDj(:,i,j);
        end
    end
    locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
    ii = zeros(21*NT,1); jj = zeros(21*NT,1); sM = zeros(21*NT,1);
    index = 0;
    for i = 1:6
        for j = i:6
            ii(index+1:index+NT) = double(elem2dof(:,i));
            jj(index+1:index+NT) = double(elem2dof(:,j));
            % mass matrix
            % locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
            i1 = locEdge(i,1); i2 = locEdge(i,2);
            j1 = locEdge(j,1); j2 = locEdge(j,2);
            Mij = 1/20*volume.*( (1+(i1==j1))*DiDj(:,i2,j2) ...
                               - (1+(i1==j2))*DiDj(:,i2,j1) ...
                               - (1+(i2==j1))*DiDj(:,i1,j2) ...
                               + (1+(i2==j2))*DiDj(:,i1,j1));
            sM(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end
    diagIdx = (ii == jj);   upperIdx = ~diagIdx;
    M = sparse(ii(diagIdx),jj(diagIdx),sM(diagIdx),NE,NE);
    MU = sparse(ii(upperIdx),jj(upperIdx),sM(upperIdx),NE,NE);
    M = M + MU + MU';
end

%% RT0: the lowest order face element
if strcmp(elemType,'RT0')
    NF = double(max(elem2dof(:)));
    localFace = [2 3 4; 1 3 4; 1 2 4; 1 2 3]; % normal vector is not oriented
    M = sparse(NF,NF);
    for i = 1:4
        for j = i:4
            % local to global index map
            ii = double(elem2dof(:,i));
            jj = double(elem2dof(:,j));
            i1 = localFace(i,1); i2 = localFace(i,2); i3 = localFace(i,3);
            j1 = localFace(j,1); j2 = localFace(j,2); j3 = localFace(j,3);
            % computation of mass matrix --- (phi_i, phi_j)
            Mij = 1/20*volume*4.*( ...
                  (1+(i1==j1))*dot(mycross(Dlambda(:,:,i2),Dlambda(:,:,i3),2), ...
                                   mycross(Dlambda(:,:,j2),Dlambda(:,:,j3),2),2)...
                 +(1+(i1==j2))*dot(mycross(Dlambda(:,:,i2),Dlambda(:,:,i3),2), ...
                                   mycross(Dlambda(:,:,j3),Dlambda(:,:,j1),2),2)...
                 +(1+(i1==j3))*dot(mycross(Dlambda(:,:,i2),Dlambda(:,:,i3),2), ...
                                   mycross(Dlambda(:,:,j1),Dlambda(:,:,j2),2),2)...
                 +(1+(i2==j1))*dot(mycross(Dlambda(:,:,i3),Dlambda(:,:,i1),2), ...
                                   mycross(Dlambda(:,:,j2),Dlambda(:,:,j3),2),2)...
                 +(1+(i2==j2))*dot(mycross(Dlambda(:,:,i3),Dlambda(:,:,i1),2), ...
                                   mycross(Dlambda(:,:,j3),Dlambda(:,:,j1),2),2)...
                 +(1+(i2==j3))*dot(mycross(Dlambda(:,:,i3),Dlambda(:,:,i1),2), ...
                                   mycross(Dlambda(:,:,j1),Dlambda(:,:,j2),2),2)...
                 +(1+(i3==j1))*dot(mycross(Dlambda(:,:,i1),Dlambda(:,:,i2),2), ...
                                   mycross(Dlambda(:,:,j2),Dlambda(:,:,j3),2),2)...
                 +(1+(i3==j2))*dot(mycross(Dlambda(:,:,i1),Dlambda(:,:,i2),2), ...
                                   mycross(Dlambda(:,:,j3),Dlambda(:,:,j1),2),2)...
                 +(1+(i3==j3))*dot(mycross(Dlambda(:,:,i1),Dlambda(:,:,i2),2), ...
                                   mycross(Dlambda(:,:,j1),Dlambda(:,:,j2),2),2));
            if ~isempty(K)
                Mij = Mij./K;
            end
            if (j==i)
                M = M + sparse(ii,jj,Mij,NF,NF);
            else
                M = M + sparse([ii;jj],[jj;ii],[Mij; Mij],NF,NF);
            end
        end
    end
end


function varargout = double_pendulum2(varargin)

% the function simulate double pendulum, using 4th-order Runge-Kutta algorithm for the diffrential equations.
% the diffrential equation are very similar to eq. (12) and (13) here:
%                           http://home1.fvcc.edu/~dhicketh/DiffEqns/spring09projects/LauraStickel/Double%20Pendulum.pdf
% where omega=d(theta)/dt
% Moshe Lindner & Orit Peleg, August 2010 (C)

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @double_pendulum_OpeningFcn, ...
                   'gui_OutputFcn',  @double_pendulum_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before double_pendulum is made visible.
function double_pendulum_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to double_pendulum (see VARARGIN)

% Choose default command line output for double_pendulum
handles.output = hObject;
handles.len1=.5;
handles.len2=.5;
handles.mass1=.5;
handles.mass2=.5;
handles.t1=0;
handles.t2=0;
handles.w1=0;
handles.w2=0;
handles.G=9.8;
pend_plot(handles);

% Update handles structure
guidata(hObject, handles);

% UIWAIT makes double_pendulum wait for user response (see UIRESUME)
% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.
function varargout = double_pendulum_OutputFcn(hObject, eventdata, handles)
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;

% --- Executes on button press in start_stop.
function start_stop_Callback(hObject, eventdata, handles)
% hObject    handle to start_stop (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(hObject,'string','Stop');
set(handles.omega1,'enable','off');
set(handles.theta1,'enable','off');
set(handles.omega2,'enable','off');
set(handles.theta2,'enable','off');
set(handles.length_rat,'enable','off');
set(handles.mass_rat,'enable','off');
set(handles.reset,'enable','off');
set(handles.gravity,'enable','off');
i=1;
while get(hObject,'value')==1
    if (get(handles.traj,'value'))==1
        x_1=handles.len1*sin(handles.t1);
        y_1=-handles.len1*cos(handles.t1) ;
        x_2=handles.len2*sin(handles.t2) + x_1;
        y_2=-handles.len2*cos(handles.t2) + y_1;
        handles.trajectory(1,i)=x_2;
        handles.trajectory(2,i)=y_2;
        i=i+1;
        if i>400
            i=1;
        end
    elseif i~=1
        handles.trajectory=[];
        i=1;
    end
    [handles.t1,handles.t2,handles.w1,handles.w2]=db_pendulum(handles.t1,handles.t2,handles.w1,handles.w2,handles.mass1,handles.mass2,handles.len1,handles.len1,handles.G,.01);
    pend_plot(handles);
    set(handles.omega1,'string',num2str(handles.w1));
    set(handles.theta1,'string',num2str(handles.t1));
    set(handles.omega2,'string',num2str(handles.w2));
    set(handles.theta2,'string',num2str(handles.t2));
    guidata(hObject, handles);
end
set(hObject,'string','Start');
set(handles.omega1,'enable','on');
set(handles.theta1,'enable','on');
set(handles.omega2,'enable','on');
set(handles.theta2,'enable','on');
set(handles.length_rat,'enable','on');
set(handles.mass_rat,'enable','on');
set(handles.reset,'enable','on');
set(handles.gravity,'enable','on');
% --- Executes on slider movement.
function length_rat_Callback(hObject, eventdata, handles)
% hObject    handle to length_rat (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.len1=get(hObject,'value');
handles.len2=1-handles.len1;
pend_plot(handles);
guidata(hObject, handles);
% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

% --- Executes during object creation, after setting all properties.
function length_rat_CreateFcn(hObject, eventdata, handles)
% hObject    handle to length_rat (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end

% --- Executes on slider movement.
function mass_rat_Callback(hObject, eventdata, handles)
% hObject    handle to mass_rat (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.mass1=get(hObject,'value');
handles.mass2=1-handles.mass1;
pend_plot(handles);
guidata(hObject, handles);
% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

% --- Executes during object creation, after setting all properties.
function mass_rat_CreateFcn(hObject, eventdata, handles)
% hObject    handle to mass_rat (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end

function omega1_Callback(hObject, eventdata, handles)
% hObject    handle to omega1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.w1=str2num(get(hObject,'String'));
energy(handles)
guidata(hObject, handles);
% Hints: get(hObject,'String') returns contents of omega1 as text
%        str2double(get(hObject,'String')) returns contents of omega1 as a double

% --- Executes during object creation, after setting all properties.
function omega1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to omega1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function theta1_Callback(hObject, eventdata, handles)
% hObject    handle to theta1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.t1=str2num(get(hObject,'String'));
pend_plot(handles);
guidata(hObject, handles);
% Hints: get(hObject,'String') returns contents of theta1 as text
%        str2double(get(hObject,'String')) returns contents of theta1 as a double

% --- Executes during object creation, after setting all properties.
function theta1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to text_1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function omega2_Callback(hObject, eventdata, handles)
% hObject    handle to omega2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.w1=str2num(get(hObject,'String'));
energy(handles)
guidata(hObject, handles);
% Hints: get(hObject,'String') returns contents of omega2 as text
%        str2double(get(hObject,'String')) returns contents of omega2 as a
%        double

% --- Executes during object creation, after setting all properties.
function omega2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to omega2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function theta2_Callback(hObject, eventdata, handles)
% hObject    handle to theta2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.t2=str2num(get(hObject,'String'));
pend_plot(handles);
guidata(hObject, handles);
% Hints: get(hObject,'String') returns contents of theta2 as text
%        str2double(get(hObject,'String')) returns contents of theta2 as a double

% --- Executes during object creation, after setting all properties.
function theta2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to theta2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

% --- Executes on button press in traj.
function traj_Callback(hObject, eventdata, handles)
% hObject    handle to traj (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.trajectory=[];
guidata(hObject, handles);
% Hint: get(hObject,'Value') returns toggle state of traj

% --- Executes on button press in reset.
function reset_Callback(hObject, eventdata, handles)
% hObject    handle to reset (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.len1=.5;
handles.len2=.5;
handles.mass1=.5;
handles.mass2=.5;
handles.t1=0;
handles.t2=0;
handles.w1=0;
handles.w2=0;
handles.G=9.8;
set(handles.gr,'string','9.8')
set(handles.gravity,'value',9.8)
set(handles.omega1,'string','0')
set(handles.omega2,'string','0')
set(handles.theta1,'string','0')
set(handles.theta2,'string','0')
set(handles.mass_rat,'value',0.5)
set(handles.length_rat,'value',0.5)
pend_plot(handles);
guidata(hObject, handles);

function pend_plot(data);
[xx yy zz]=cylinder(0.1,20);
    x_1=data.len1*sin(data.t1);
    y_1=-data.len1*cos(data.t1) ;
    x_2=data.len2*sin(data.t2) + x_1;
    y_2=-data.len2*cos(data.t2) + y_1;
hold off
plot([0 x_1 x_2],[0 y_1 y_2],'k','linewidth',2);
hold on
if get(data.traj,'value') ==1
    plot(data.trajectory(1,:),data.trajectory(2,:),'r.','markersize',3)
end
fill(x_1+xx(1,:).*(data.mass1)^.3,y_1+yy(1,:).*(data.mass1)^.3,'b')
fill(x_2+xx(1,:).*(data.mass2)^.3,y_2+yy(1,:).*(data.mass2)^.3,'b')
set(gca,'xtick',[],'ytick',[])
axis(1.2*[-1 1 -1 1]);
box on;
drawnow
energy(data)

function [theta_1 theta_2 omega_1 omega_2]=db_pendulum(theta1, theta2, w1, w2, mass1, mass2, d1, d2, G, dt)
l1 = dt * (l(w1));
m1 = dt * (m(w2));
f1 = dt * (f(theta1, theta2, w1, w2, mass1, mass2, d1, d2, G));
g1 = dt * (g(theta1, theta2, w1, w2, mass1, mass2, d1, d2, G));

l2 = dt * (l(w1+(f1/2.)));
m2 = dt * (m(w2+(g1/2.)));
f2 = dt * (f(theta1+(l1/2.),theta2+(m1/2.),w1+(f1/2.),w2+(g1/2.), mass1, mass2, d1, d2, G));
g2 = dt * (g(theta1+(l1/2.),theta2+(m1/2.),w1+(f1/2.),w2+(g1/2.), mass1, mass2, d1, d2, G));

l3 = dt * (l(w1+(f2/2.)));
m3 = dt * (m(w2+(g2/2.)));
f3 = dt * (f(theta1+(l2/2.),theta2+(m2/2.),w1+(f2/2.),w2+(g2/2.), mass1, mass2, d1, d2, G));
g3 = dt * (g(theta1+(l2/2.),theta2+(m2/2.),w1+(f2/2.),w2+(g2/2.), mass1, mass2, d1, d2, G));

l4 = dt * (l(w1+f3));
m4 = dt * (m(w2+g3));
f4 = dt * (f(theta1+(l3),theta2+(m3),w1+(f3),w2+(g3), mass1, mass2, d1, d2, G));
g4 = dt * (g(theta1+(l3),theta2+(m3),w1+(f3),w2+(g3), mass1, mass2, d1, d2, G));
theta_1 = theta1 + (l1+(2.*l2)+(2.*l3)+l4)/6.;
theta_2 = theta2 + (m1+(2.*m2)+(2.*m3)+m4)/6.;
omega_1 = w1 + (f1+(2.*f2)+(2.*f3)+f4)/6.;
omega_2 = w2 + (g1+(2.*g2)+(2.*g3)+g4)/6.;

function f1=f(theta1, theta2, w1, w2, mass1, mass2, d1, d2, G)
f1= (  (( -d2*mass2*w2*w2*sin(theta1- theta2)/(d1*(mass1+mass2)) )  - ...
    (  mass2*w1*w1*cos(theta1-theta2)*sin(theta1- theta2)/(mass1+mass2) )  +...
    (  mass2*G*cos(theta1- theta2)*sin(theta2)/(d1*(mass1+mass2))  )  -...
    (  G*sin(theta1)/d1)...
    ) /  ( 1. - (mass2*cos(theta1- theta2)*cos(theta1- theta2)/(mass1+mass2)))  );
function g1=g(theta1, theta2 ,w1, w2, mass1, mass2, d1, d2, G)
g1=(( (-G*sin(theta2)/d2) + (mass2*w2*w2*cos(theta1-theta2)*sin(theta1-theta2)/(mass1+mass2)) +...
    (G*cos(theta1-theta2)*sin(theta1)/d2) + (d1*w1*w1*sin(theta1-theta2)/d2)  )  /  (1. - (mass2*cos(theta1-theta2)*cos(theta1-theta2)/(mass1+mass2))) );
function L1=l(w1)
L1=w1;
function M1=m(w2)
M1= w2;

% --- Executes on slider movement.
function gravity_Callback(hObject, eventdata, handles)
% hObject    handle to gravity (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.G=get(hObject,'value');
set(handles.gr,'string',num2str(handles.G));
guidata(hObject, handles);

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

% --- Executes during object creation, after setting all properties.
function gravity_CreateFcn(hObject, eventdata, handles)
% hObject    handle to gravity (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end

function energy(data)
  Epot1=-data.mass1*data.G*data.len1*(cos(data.t1)-1.000) ;
  Epot2=-data.mass2*data.G*((cos(data.t1)-1.000)*data.len1+(cos(data.t2)-1.000)*data.len2);
  Ekin1=0.5*data.mass1*(data.w1*data.len1).^2;
  Ekin2=0.5*data.mass2*(   (data.w2*data.len2*cos(data.t2) +data.w1*data.len1*cos(data.t1)).^2 + (data.w2*data.len2*sin(data.t2) +data.w1*data.len1*sin(data.t1)).^2 );
  Etot=Ekin1+Ekin2+Epot1+Epot2;

  set(data.U1,'string',num2str(Epot1));
  set(data.U2,'string',num2str(Epot2));
  set(data.T1,'string',num2str(Ekin1));
  set(data.T2,'string',num2str(Ekin2));
  set(data.totE,'string',num2str(Etot));


% Figure 5.40 Feedback Control of Dynamic Systems, 5e
%                   Franklin, Powell, Emami
%
% attitude hold auto pilot for Dakota
% root locus with integral control

clf
Wsp=6.3;  % short period
Zsp=.4;
Wph=.24;  % phugoid mode
Zph=.06;
Kg=160;    % gain so that 1" of control input produces 5 deg

NUMsp=[1 Zsp*Wsp];
NUMph=[1 3*Wph];
DENsp=[1 2*Zsp*Wsp Wsp^2];
DENph=[1 2*Zph*Wph Wph^2];

numG=Kg*conv(NUMsp,NUMph);
denG=conv(DENsp,DENph);
zeroG=roots(numG);

% lead compensation

numD=[1 3];
denD=[1 20];

num=conv(numG,numD);
den=conv(denG,denD);
zeroD=roots(numD);

K=1.5;
[r2]=rlocus(num,den,K);

% now add integral control

polesI=[r2';0];  % add integral control pole
denI=poly(polesI);

axis('square')
subplot(121)
axis([-30 0 -15 15])
plot(polesI,'x'),grid
hold on
plot(zeroD,0,'o')
plot(zeroG(1),0,'o',zeroG(2),0,'o')

ki0=.01:.05:.51;
ki1=1:.5:10;
ki2=12:5:102;
ki3=120:20:1000;
ki=[ki0 ki1 ki2 ki3 5000];
[R]=rlocus(K*num,denI,ki);
plot(R,'-')
axis([-25 2 -15 15]);
title ('Fig. 5.40 Root Locus vs. K_I for autopilot')
xlabel('Re(s)')
ylabel('Im(s)')

KI=.15;
[R1]=rlocus(K*num,denI,KI);
damp(R1')
plot(R1,'*')
hold off

% now zoom in to origin

subplot(122)
plot(polesI,'x'),grid
axis([-4 0 -2 2])
hold on
xlabel('Re(s)')
ylabel('Im(s)')
plot(zeroD,0,'o')
plot(zeroG(1),0,'o',zeroG(2),0,'o')
plot(R,'-')
plot(R1,'*')

hold off
axis('normal')

subplot(111)


%% Transient diffusion equation
%% PDE and boundary conditions
% The transient diffusion equation reads
%
% $$\alpha\frac{\partial c}{\partial t}+\nabla.\left(-D\nabla c\right)=0,$$
%
% where $c$ is the independent variable (concentration, temperature, etc)
% , $D$ is the diffusion coefficient, and $\alpha$ is a constant.
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc

%% Define the domain and create a mesh structure
L = 50;  % domain length
Nx = 10; % number of cells
m = createMesh3D(Nx,Nx+3,Nx+6, L/2,2*pi,L);
%% Create the boundary condition structure
BC = createBC(m); % all Neumann boundary condition structure
% BC.left.a(:) = 0; BC.left.b(:)=1; BC.left.c(:)=0; % left boundary
BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=0; % right boundary
% BC.top.a(:) = 0; BC.top.b(:)=1; BC.top.c(:)=3; % top boundary
% BC.bottom.a(:) = 0; BC.bottom.b(:)=1; BC.bottom.c(:)=0; % bottom boundary
BC.top.periodic=1;
BC.back.a(:) = 0; BC.back.b(:)=1; BC.back.c(:)=3; % back boundary
BC.front.a(:) = 0; BC.front.b(:)=1; BC.front.c(:)=0; % front boundary
%% define the transfer coeffs
D_val = 1;
D = createCellVariable(m, D_val);
alfa = createCellVariable(m, 1);
u = createFaceVariable(m, [0,0,0.5]);
%% define initial values
c_init = 1;
c_old = createCellVariable(m, c_init,BC); % initial values
c = c_old; % assign the old value of the cells to the current values
%% loop
Dave = harmonicMean(D);
Mdiff = diffusionTerm(Dave);
[Mbc, RHSbc] = boundaryCondition(BC);
FL = fluxLimiter('Superbee');
Mconv = convectionTvdTerm(u, c, FL);
dt = 1; % time step
final_t = 50;
for t=dt:dt:final_t
    [M_trans, RHS_trans] = transientTerm(c_old, dt, alfa);
    M = M_trans-Mdiff+Mbc+Mconv;
    RHS = RHS_trans+RHSbc;
    c = solvePDE(m,M, RHS);
    c_old = c;
    figure(1);visualizeCells(c);drawnow;
end
%% visualization
 figure(1);visualizeCells(c);


function [v,d,z]=soundspeed(t,p,m,g)
%SOUNDSPEED gives the speed of sound, density of air and acostuc impedance as a function of temp & pressure [V,D,Z]=(T,P,M,G)
%
%  Inputs:  T        air temperature in Celsius  [20 deg C]
%           P        air pressure [1 atm]
%           M        average molecular weight of air [0.0289644 kg/mol]
%           G        adiabatic constant for air [1.4]
%
% Outputs:  V        is the speed of sound in m/s
%           D        density of air in kg/m^3
%           Z        characteristic impedance of air Pa.s/m

% Notes: (1) Sound pressure is often measured in dB (SPL) relative to 20uPa [20*log10(p/p0)]
%            Sound pressure is inversely proportional to distance.
%        (2) Sound intensity is often measured indB relative to pW/m^2,[10*log10(J*10^12)]
%            Intensity is inversely proportional to distance squared.
%        (3) Intensity * impedance = pressure^2, so with the default values, 1 pW/m^2 = 20.33 uPa
%            So: X dB (SPL) = X-93.98 dB (Pa) = X-0.14 dB (pW/m^2) =  X-120.14 dB (W/m^2)
%        (4) The default air pressure (which does not affect sound speed) in various units is:
%            1 atm = 101325 Pa = 1.01325 bar = 1.0332 at = 760 torr = 14.696 psi

%	   Copyright (C) Mike Brookes 2006
%      Version: $Id: soundspeed.m,v 1.5 2009/02/27 07:44:54 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<4
    g=1.4;
    if nargin<3
        m=0.0289644;      % gm/mol
        if nargin<2
            p=1;
            if nargin<1
                t=20;
            end
        end
    end
end
p=p*101325; % convert pressure: atm to pascal
k=t+273.15;  % absolute temperature
r=8.3144;  % J/(mol K) universal gas constant
d=p*m/(r*k);
v=sqrt(g*r*k/m);
z=v*d;


function [C_pos,C_neg,Ctot_pos,Ctot_neg] = clustering_coef_wu_sign(W,coef_type)
%CLUSTERING_COEF_WU_SIGN     Multiple generalizations of the clustering coefficient
%
%   [C_pos,C_neg,Ctot_pos,Ctot_neg] = clustering_coef_wu_sign(W,coef_type);
%
%   The weighted clustering coefficient is the average weight or intensity
%   of all triangles associated with each node.
%
%   Inputs:
%       W,
%           Weighted undirected connection matrix
%
%       corr_type,
%           Desired type of clustering coefficient.
%           Options:
%           1,  (default) Onnela et al. formula, used in original
%               clustering_coef_wu.m. Computed separately for positive &
%               negative weights.
%           2,  Zhang & Horvath formula, similar to Onnela formula except
%               denominator of Onnela formula relies on binarizing the
%               network whereas this denominator is based on weight value,
%               which reduces the sensitivity of this measure to the
%               weights directly connected to the node of interest.
%               Computed separately for positive & negative weights.
%           3,  Constantini & Perugini's generalization of the Zhang &
%               Horvath formula. This formula takes both positive &
%               negative weights into account simultaneously, & is
%               particularly sensitive to non-redundnacy in path
%               information based on sign (i.e., when two weights are
%               positive & one negative, or all three are negative, both of
%               which indicate that the weight of the third path is not
%               redundant information). Produces only one value.
%
%
%   Outputs:
%       C_pos/C_neg,
%           Clustering coefficient vector for positive/negative weights.
%           For the third option, only one vector is outputted (as C_pos).
%       Ctot_pos/Ctot_neg,
%           Mean clustering coefficient for positive and negative weights.
%
%   References:
%       Onnela et al. (2005) Phys Rev E 71:065103
%       Zhang & Horvath (2005) Stat Appl Genet Mol Biol 41:1544-6115
%       Costantini & Perugini (2014) PLOS ONE 9:e88669
%
%
%   Contributor: Jeff Spielberg, Boston University, 2014-2015
%                (script based on clustering_coef_wu.m)

%
%   Modification History:
%   May 2014: Added computation of pos & neg weights separately &
%             computation of mean coefficient (Jeff Spielberg)
%   May 2015: Added computation of Zhang & Horvath and Constantini &
%             Perugini formulas (Jeff Spielberg)

if ~exist('coef_type','var')
    coef_type = 1;
end

n            = length(W);                   %number of nodes
W(1:n+1:end) = 0;

switch coef_type
    case 1
        W_pos                = W.*(W>0);
        K_pos                = sum(W_pos~=0,2);
        cyc3_pos             = diag((W_pos.^(1/3))^3);
        K_pos(cyc3_pos == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_pos                = cyc3_pos./(K_pos.*(K_pos-1));         %clustering coefficient
        Ctot_pos             = mean(C_pos);

        W_neg                = -W.*(W<0);
        K_neg                = sum(W_neg~=0,2);
        cyc3_neg             = diag((W_neg.^(1/3))^3);
        K_neg(cyc3_neg == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_neg                = cyc3_neg./(K_neg.*(K_neg-1));         %clustering coefficient
        Ctot_neg             = mean(C_neg);
    case 2
        W_pos    = W.*(W>0);
        cyc3_pos = zeros(n,1);
        cyc2_pos = zeros(n,1);
        for i = 1:n
            for j = 1:n
                for q = 1:n
                    cyc3_pos(i) = cyc3_pos(i)+(W_pos(j,i)*W_pos(i,q)*W_pos(j,q));
                    if j~=q
                        cyc2_pos(i) = cyc2_pos(i)+(W_pos(j,i)*W_pos(i,q));
                    end
                end
            end
        end
        cyc2_pos(cyc3_pos == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_pos                   = cyc3_pos./cyc2_pos;         %clustering coefficient
        Ctot_pos                = mean(C_pos);

        W_neg    = -W.*(W<0);
        cyc3_neg = zeros(n,1);
        cyc2_neg = zeros(n,1);
        for i = 1:n
            for j = 1:n
                for q = 1:n
                    cyc3_neg(i) = cyc3_neg(i)+(W_neg(j,i)*W_neg(i,q)*W_neg(j,q));
                    if j~=q
                        cyc2_neg(i) = cyc2_neg(i)+(W_neg(j,i)*W_neg(i,q));
                    end
                end
            end
        end
        cyc2_neg(cyc3_neg == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_neg                   = cyc3_neg./cyc2_neg;         %clustering coefficient
        Ctot_neg                = mean(C_neg);
    case 3
        cyc3         = zeros(n,1);
        cyc2         = zeros(n,1);

        for i = 1:n
            for j = 1:n
                for q = 1:n
                    cyc3(i) = cyc3(i)+(W(j,i)*W(i,q)*W(j,q));
                    if j~=q
                        cyc2(i) = cyc2(i)+(W(j,i)*W(i,q));
                    end
                end
            end
        end

        cyc2(cyc3 == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_pos           = cyc3./cyc2;         %clustering coefficient
        Ctot_pos        = mean(C_pos);
        C_neg           = nan(size(C_pos));
        Ctot_neg        = nan(size(Ctot_pos));
end


function varargout = PlotPaths(varargin)

% Begin initialization code
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @GUI_OpeningFcn, ...
                   'gui_OutputFcn',  @GUI_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code

% --- Executes just before GUI is made visible.
function GUI_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to GUI (see VARARGIN)

% Choose default command line output for GUI
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

% UIWAIT makes GUI wait for user response (see UIRESUME)
% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.
function varargout = GUI_OutputFcn(hObject, eventdata, handles)
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;

% --- Executes on button press in pushbutton1.
function pushbutton1_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
kappa=2;
theta=1;
sigma=.4;
X0=1;
n=1000;
m=7;
W=randn(n,m);
X=zeros(n,m);
X(1,:)=X0;

t=[0:1/n:1];

for j=1:m
    for i=1:n
        X(i+1,j)=X(i,j)+kappa*(theta-X(i,j))/n+sigma*W(i,j)/sqrt(n);
    end
end
plot(t,X)
xlabel('$t$','interpreter','latex','FontSize',14)
ylabel('$r_t$','interpreter','latex','FontSize',14)
title('$Some~Vasicek~Paths~:~~dr_{t}=\kappa(\theta-r_{t})dt+\sigma dW_{t}$','interpreter','latex','FontSize',14)
axis([0 1 0 2])

% --- Executes on button press in pushbutton2.
function pushbutton2_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
kappa=2;
theta=1;
sigma=.4;
X0=1;
n=1000;
m=7;
W=randn(n,m);
X=zeros(n,m);
X(1,:)=X0;
t=[0:1/n:1];
for j=1:m
    for i=1:n
        X(i+1,j)=X(i,j)+kappa*(theta-X(i,j))/n+sigma*sqrt(X(i,j))*W(i,j)/sqrt(n);
    end
end
plot(t,X)
xlabel('$t$','interpreter','latex','FontSize',14)
ylabel('$r_t$','interpreter','latex','FontSize',14)
title('$Some~Cox.Ingersoll.Ross~Paths~:~~dr_t=\kappa(\theta-r_t)dt+\zeta \sqrt{r_t} dW_t$','interpreter','latex','FontSize',14)
axis([0 1 0 2])

% --- Executes on button press in pushbutton3.
function pushbutton3_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

n=1000;
m=50;
X0=ones(1,m);
r=0.35;
sigma=.25;
X=[X0;1+r/n+sigma*sqrt(1/n)*randn(n,m)];
t=[0:1/n:1];
Xt=cumprod(X);
plot(t,Xt)
xlabel('$t$','interpreter','latex','FontSize',13)
ylabel('$X_t$','interpreter','latex','FontSize',13)
title('$Geometric ~Brownian ~Motion~:~~dX_t=r X_t dt+ \sigma X_t dW_t$','interpreter','latex','FontSize',13)
axis([0 1 0 3])

% --- Executes on button press in pushbutton4.
function pushbutton4_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

n=1000;
m=50;
X0=zeros(1,m);
r=0.05;
sigma=.2;
X=[X0;r/n+sigma*sqrt(1/n)*randn(n,m)];
Xt=cumsum(X);
t=[0:1/n:1];
plot(t,Xt)
xlabel('$t$','interpreter','latex','FontSize',13)
ylabel('$X_t$','interpreter','latex','FontSize',13)
title('$Arithmetic ~Brownian ~Motion~:~~dX_t=r dt+ \sigma dW_t$','interpreter','latex','FontSize',13)
axis([0 1 -1 1])

% --- Executes on button press in pushbutton5.
function pushbutton5_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

n=1000;
m=20;
X0=zeros(1,m);
sigma=.35;
X=[X0;sigma*sqrt(1/n)*randn(n,m)];
Xt=cumsum(X);
Y=[X0;repmat(Xt(n+1,:),n,1)/n];
Yt=cumsum(Y);
Z=Xt-Yt;
t=[0:1/n:1];
plot(t,Z)
xlabel('$t$','interpreter','latex','FontSize',13)
ylabel('$B_t$','interpreter','latex','FontSize',13)
title('$~Brownian ~Bridge~:~~B(t)=W(t)- \frac{t}{T} W(T)$','interpreter','latex','FontSize',13)
axis([0 1 -1 1])

% --- Executes on button press in pushbutton6.
function pushbutton6_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

n=1000;
m=5;
X0=zeros(1,m);
sigma=.3;
X=[X0;sigma*sqrt(1/n)*randn(n,m)];
Y=[X0;sigma*sqrt(1/n)*randn(n,m)];
Xt=cumsum(X);
Yt=cumsum(Y);
t=[0:1/n:1];
plot(Xt,Yt)
xlabel('$W_x(t)$','interpreter','latex','FontSize',13)
ylabel('$W_y(t)$','interpreter','latex','FontSize',13)
title('$2~Dimension ~Brownian ~Motion~:~~\vec{W}(t)=(W_x(t)~~W_y(t))$','interpreter','latex','FontSize',13)
axis([-1 1 -1 1])

% --- Executes on button press in pushbutton7.
function pushbutton7_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton7 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

n=1000;
m=5;
X0=zeros(1,m);
sigma=.4;
X=[X0;sigma*sqrt(1/n)*randn(n,m)];
Y=[X0;sigma*sqrt(1/n)*randn(n,m)];
Z=[X0;sigma*sqrt(1/n)*randn(n,m)];
Xt=cumsum(X);
Yt=cumsum(Y);
Zt=cumsum(Z);
t=[0:1/n:1];
plot3(Xt,Yt,Zt)
grid on
xlabel('$W_x(t)$','interpreter','latex','FontSize',13)
ylabel('$W_y(t)$','interpreter','latex','FontSize',13)
zlabel('$W_z(t)$','interpreter','latex','FontSize',13)
title('$3~Dimension ~Brownian ~Motion~:~~\vec{W}(t)=(W_x(t)~~W_y(t)~~W_z(t))$','interpreter','latex','FontSize',13)
axis([-1 1 -1 1 -1 1])

% Rodolphe Sitter


function Y = AtA_Toeplitz(X,Lambda);
% AtA_Toeplitz: Gram Operator of SeparateAngle
%  Usage:
%     Y = AtA(X,Lambda)
%  Inputs:
%    X      n*n matrix (x,y)
%    Lambda Fourier multipliers
%  Outputs:
%    Y      n*n matrix (x,y)
%  Description:
%    Performs  A'A where A is the angular windowing using the
%    Toeplitz structure of A'A. AtA_Toeplitz uses FFTs.
%
% By Emmanuel Candes, 2003-2004

  n  = size(X,1); n2 = n/2;

  X1 = zeros(n); X2 = zeros(n);

  [ix,w] = DetailMeyerWindow([n2/4 n2/2],3);
  lx = reverse(-ix);

  F = ifft_mid0(X).*sqrt(n); % Take FT

  for r = 1:(n2/2),
    k = lx(r); t = n2 + k + 1;
    X1(:,t) = GUSFT_Toeplitz(F(:,t),Lambda(:,r,1));

    rsym = n2/2+1-r; t = n2 - k + 1;
    X1(:,t) = GUSFT_Toeplitz(F(:,t),Lambda(:,rsym,2));
  end

  X1 = fft_mid0(X1)/sqrt(n); % Invert FT

  F = ifft_mid0(X.').*sqrt(n); % Take FT

  for r = 1:(n2/2),
    k = lx(r); t = n2 + k + 1;
    X2(:,t) = GUSFT_Toeplitz(F(:,t),Lambda(:,r,1));

    rsym = n2/2+1-r;  t = n2 - k + 1;
    X2(:,t) = GUSFT_Toeplitz(F(:,t),Lambda(:,rsym,2));
  end

  X2 = fft_mid0(X2)/sqrt(n); % Invert FT

  Y = X1 + X2.';


function err = relaxT1FitFunc(x, m, alpha, tr, te)
%
% function err = relaxT1FitFunc(x, m, alpha, tr, te);
% inputs:
%       x = [r1, pd] where r1 is the inverse of the longitudinal relaxation
%       time (1/T1) and pd is a compsite term representing
%       M_0*G*exp(-TE/T_2star)
%       m: spgr data
%
%       alpha: flip angle (in degrees)
%       tr, te = tr and te
%
%   Output: the error for each measurement
%
%

r1 = x(1);
pd = x(2);

%pd = M_0*G*exp(-te/T_2star);
S_alpha = pd .* sin(alpha) .* (1-exp(-r1.*tr)) ./ (1-cos(alpha) .* exp(-r1.*tr));
err = S_alpha - m;

return;


function d = dot(g1,g2,varargin)
% inner product of quaternions g1 and g2
%
% Input
%  q1, q2 - @quaternion
%
% Output
%  d - double
%

d = g1.a .* g2.a + g1.b .* g2.b + g1.c .* g2.c + g1.d .* g2.d;


% =========================
%  Copula-Marginal Algorithm (CMA) to generate and manipulate flexible copulas, as described in
%  Meucci A., "New Breed of Copulas for Risk and Portfolio Management", Risk, September 2011
%
%  Most recent version of article and code available at http://www.symmys.com/node/335
%  =========================

clear; clc; close all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generate panic distribution
N=20;
J=50000;

p=ones(J,1)/J;

r_c=.3;
c2_c=(1-r_c)*eye(N)+r_c*ones(N,N);

b=0.02;
r=.99;
c2_p=(1-r)*eye(N)+r*ones(N,N);

s2=blkdiag(c2_c,c2_p);
Z = MvnRnd(zeros(2*N,1),s2,J);

X_c = Z(:,1:N);
X_p = Z(:,N+1:end);
D = (normcdf(X_p)<b);

X=(1-D).*X_c+D.*X_p;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% perturb probabilities via Fully Flexible Views
Aeq = ones(1,J);  % constrain probabilities to sum to one...
beq=1;
Aeq = [Aeq   % ...constrain the first moments...
    X'];
beq=[beq
    zeros(N,1)];
p_ = EntropyProg(p,[],[],Aeq ,beq); % ...compute posterior probabilities

[xdd,udd,U]=CMAseparation(X,p_);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% merge panic copula with normal marginals
y=[];
u=[];
for n=1:N
    sig=.20;
    yn = linspace(-4*sig,4*sig,100)';
    un=normcdf(yn,0,sig);

    y=[y yn];
    u=[u un];
end
Y=CMAcombination(y,u,U);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute portfolio risk
w=ones(N,1)/N; % portfolio weights
R_w=Y*w;

figure
[n,D]=pHist(R_w,p_,round(10*log(J))  );
h=bar(D,n,1);
[x_lim]=get(gca,'xlim');
set(gca,'ytick',[])
set(h,'FaceColor',.5*[1 1 1],'EdgeColor',.5*[1 1 1]);
grid on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% rotate panic copula (only 2Dim)
if N==2
    th=pi/2;
    R=[cos(th) sin(th)
        -sin(th) cos(th)];
    X_=X*R';
    [xdd,udd,U_]=CMAseparation(X_,p_);
end


function [ x, seed ] = r8_normal_ab ( a, b, seed )

%*****************************************************************************80
%
%% R8_NORMAL_AB returns a scaled pseudonormal R8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, the mean of the normal PDF.
%
%    Input, real B, the standard deviation of the normal PDF.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X, a sample of the standard normal PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  [ x, seed ] = r8_normal_01 ( seed );
  x = a + b * x;

  return
end


function obj = subobjective(weight, ind, idealpoint, method)
%SUBOBJECTIVE function evaluate a point's objective with a given method of
%decomposition.

%   Two method are implemented by far is Weighted-Sum and Tchebesheff.
%   weight: is the decomposition weight.(column wise vector).
%   ind: is the individual point(column wise vector).
%   idealpoint: the idealpoint for Tchebesheff decomposition.
%   method: is the decomposition method, the default is 'te' when is
%   omitted.
%
%   weight and ind can also be matrix. in which have two scenairos:
%   When weight is a matrix, then it's treated as a column wise set of
%   weights. in that case, if ind is a size 1 column vector, then the
%   subobjective is computed with every weight and the ind; if ind is also
%   a matrix of the same size as weight, then the subobjective is computed
%   in a column-to-column, with each column of weight computed against the
%   corresponding column of ind.
%   A row vector of subobjective is return in both case.

    if (nargin==2)
        obj = ws(weight, ind);
    elseif (nargin==3)
        obj = te(weight, ind, idealpoint);
    else
        if strcmp(method, 'ws')
            obj=ws(weight, ind);
        elseif strcmp(method, 'te')
            obj=te(weight, ind, idealpoint);
        else
            obj= te(weight, ind, idealpoint);
        end
    end
end

function obj = ws(weight, ind)
    if size(ind, 2) == 1
       obj = (weight'*ind)';
    else
       obj = sum(weight.*ind);
    end
end

function obj = te(weight, ind, idealpoint)
    s = size(weight, 2);
    indsize = size(ind,2);

    weight((weight == 0))=0.00001;

    if indsize==s
        part2 = abs(ind-idealpoint(:,ones(1, indsize)));
        obj = max(weight.*part2);
    elseif indsize ==1
        part2 = abs(ind-idealpoint);
        obj = max(weight.*part2(:,ones(1, s)));
    else
        error('individual size must be same as weight size, or equals 1');
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inverse dynamics for the 2dof planar robot
%
%   tau = inversedynamics_2dofplanar(robot, q, qd, qdd, gc, fext)
%
%   Where robot stores the kinematic and dynamic parameters for this robot.
%   q: joint positions.
%   qd: joint velocities.
%   qdd: joint accelerations.
%   fext: vector of external forces. Defined in the BASE reference system.
%   let gc=9.81 m/s^2 if the force is acting on the -Y0 direction, or gc =
%   -9.81 m/s^2 if acting on the opposite direction.
%
%   This function just executes the inverse dynamic model for this robot.
%   The equations to compute this dynamic model can be found in:
%   "ROBOT ANALYSIS. The mechanics of Serial and Parallel
%        manipulators". Lung Weng Tsai. John Wiley and Sons, inc. ISBN:
%        0-471-32593-7. page 405.
%
%
%   Author: Arturo Gil Aparicio arturo.gil@umh.es
%   Date: 08/03/2014
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2016, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function qdd = forwarddynamics_2dofplanar(robot, q, qd, tau, gc, fext)
a = eval(robot.DH.a);
a1=a(1);
a2=a(2);

m1=robot.dynamics.masses(1);
m2=robot.dynamics.masses(2);

%Express it as a general dynamic function:
%M*qdd + V + G = Q, where Q is a vector of generalized forces or moments
%M is a 2x2 manipulator inertia matrix
M = [(1/3)*m1*a1^2 + m2*(a1^2 + a1*a2*cos(q(2))+(1/3)*a2^2)  m2*((1/2)*a1*a2*cos(q(2))+(1/3)*a2^2);
     m2*((1/2)*a1*a2*cos(q(2))+(1/3)*a2^2)                      (1/3)*m2*a2^2];

V = [-m2*a1*a2*sin(q(2))*(qd(1)*qd(2)+(1/2)*qd(2)^2);
            (1/2)*m2*a1*a2*sin(q(2))*qd(1)^2        ];

G = [(1/2)*m1*gc*a1*cos(q(1))+ m2*gc*a1*cos(q(1)) + (1/2)*m2*gc*a2*cos(q(1)+q(2));
         (1/2)*m2*gc*a2*cos(q(1)+q(2))              ];

%assure fext is a column vector
fext=fext(:);

%External forces are propagated to every joint by using the manipulators
%Jacobian
J = manipulator_jacobian(robot, q);

% %Account for friction by summing in tau.
if robot.dynamics.friction
    for j=1:robot.DOF,
        tau(j) = tau(j) - friction(robot, qd, j);
    end
end

inv(M)
tau
V
G
J'*fext
%Finally use the general equation to compute the torques, considering the
%external forces.
qdd =  inv(M)*(tau - V - G - J'*fext);


function [logp, yhat, res] = tapas_softmax_2beta(r, infStates, ptrans)
% Calculates the log-probability of responses under the softmax model with different betas for
% rewards and punishments
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013-2016 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Predictions or posteriors?
predorpost = r.c_obs.predorpost;

% Transform betas to their native space
% be(1): rewards, be(2): punishments
be = exp(ptrans(1:2));

% Initialize returned log-probabilities, predictions,
% and residuals as NaNs so that NaN is returned for all
% irregualar trials
n = size(infStates,1);
logp = NaN(n,1);
yhat = NaN(n,1);
res  = NaN(n,1);

% Weed irregular trials out from inferred states, inputs, and responses
states = squeeze(infStates(:,1,:,1,predorpost));
states(r.irr,:) = [];
u = r.u(:,1);
u(r.irr) = [];
y = r.y(:,1);
y(r.irr) = [];

% Number of choices
nc = size(infStates,3);

% Partition functions
Z1 = sum(exp(be(1)*states),2);
Z1 = repmat(Z1,1,nc);

Z2 = sum(exp(be(2)*states),2);
Z2 = repmat(Z2,1,nc);

% Softmax probabilities
prob1 = exp(be(1)*states)./Z1;
prob2 = exp(be(2)*states)./Z2;

% Extract probabilities of chosen options
probc1 = prob1(sub2ind(size(prob1), 1:length(y), y'));
probc2 = prob2(sub2ind(size(prob2), 1:length(y), y'));

% Choose the correct column
probc = probc1'.*(u==1) +probc2'.*(u==0);

% Calculate log-probabilities for non-irregular trials
reg = ~ismember(1:n,r.irr);
logp(reg) = log(probc);
yhat(reg) = probc;
res(reg) = -log(probc);

return;


% MAIN  --  minimum acceleration trajectory
%
% For a simple pendulum:
%
% x = position
% v = velocity
% u = torque
%
% ddx = f(x,dx,u);     <-- dynamics
%
% cost = integral(  ddx^2  );     <-- cost function
%
% subject to:
%   x(0) = 0;
%   x(1) = pi;
%   dx(0) = 0;
%   dx(1) = pi;
%
% How to pose as a standard trajectory optimization problem?
%
% dx = v1;
% dv1 = f(x,v1,u1)
%
% v2 == v1;   % <-- Key line.
% dv2 = u2;
% cost = integral(  u2^2  );
%
%
% NOTES:
%
%   z = [x;v1;v2];
%   u = [u1;u2];
%
clc; clear;
addpath ../../..

%%%% Specify boundary conditions
t0 = 0;
tF = 5;

maxTorque = 1.0;
z0 = [0;0;0];
zF = [pi;0;0];

param.k = 1.0;  % gravity torque constant for pendulum model
param.b = 0.1;  % viscous damping constant

%%%% Pack up boundary conditions
problem.bounds.initialTime.low = t0;
problem.bounds.initialTime.upp = t0;

problem.bounds.finalTime.low = tF;
problem.bounds.finalTime.upp = tF;

problem.bounds.initialState.low = z0;
problem.bounds.initialState.upp = z0;

problem.bounds.finalState.low = zF;
problem.bounds.finalState.upp = zF;

problem.bounds.control.low = [-maxTorque; -inf];
problem.bounds.control.upp = [maxTorque; inf];

%%%% Initialize trajectory with a straight line
problem.guess.time = [t0,tF];
problem.guess.state = [z0, zF];
problem.guess.control = [zeros(2,1), zeros(2,1)];

%%%% Pack up function handles
problem.func.dynamics = @(t,z,u)(  dynamics(z,u,param)  );
problem.func.pathObj = @(t,z,u)(  pathObjective(u)  );
problem.func.pathCst = @(t,z,u)(  pathConstraint(z)  );

%%%% Choice of solver:
method = 'trapezoid';

switch method
    case 'chebyshev'
        problem.options.method = method;
        problem.options.chebyshev.nColPts = 25;
    case 'trapezoid'
        problem.options.method = method;
    case 'hermiteSimpson'
        problem.options.method = method;
        problem.options.hermiteSimpson.nSegment = 15;
        problem.options.nlpOpt.MaxFunEvals = 5e4;
    case 'gpops'
        problem.options.method = 'gpops';
    otherwise
        error('invalid method')
end

%%%% Solve
soln = optimTraj(problem);

%%%% Unpack the solution

tGrid = soln.grid.time;
xGrid = soln.grid.state(1, :);
v1Grid = soln.grid.state(2, :);
v2Grid = soln.grid.state(3, :);
u1Grid = soln.grid.control(1, :);
dv2Grid = soln.grid.control(2, :);

t = linspace(tGrid(1), tGrid(end), 100);
z = soln.interp.state(t);
u = soln.interp.control(t);
x = z(1,:);
v1 = z(2,:);
v2 = z(3,:);
u1 = u(1,:);
dv2 = u(2,:);

%%%% Plot the trajectory against time
figure(1); clf;

subplot(2,2,1); hold on;
plot(t,x)
plot(tGrid,xGrid,'ko','MarkerSize',8,'LineWidth',2);
title('angle')

subplot(2,2,2); hold on;
plot(t,v1)
plot(t,v2)
plot(tGrid,v1Grid,'ko','MarkerSize',8,'LineWidth',2);
plot(tGrid,v2Grid,'ko','MarkerSize',8,'LineWidth',2);
title('angular rate')
legend('v1','v2')

subplot(2,2,3); hold on;
plot(t([1,end]),[1,1]*maxTorque,'k--','LineWidth',1);
plot(t([1,end]),-[1,1]*maxTorque,'k--','LineWidth',1);
plot(t,u1)
plot(tGrid,u1Grid,'ko','MarkerSize',8,'LineWidth',2);
title('torque')

subplot(2,2,4); hold on;
plot(t,dv2)
plot(tGrid,dv2Grid,'ko','MarkerSize',8,'LineWidth',2);
title('angular acceleration')


%[2018]-"Butterfly optimization algorithm: a novel approach for global
%optimization"

% (9/12/2020)

function BOA = jButterflyOptimizationAlgorithm(feat,label,opts)
% Parameters
lb    = 0;
ub    = 1;
thres = 0.5;
c     = 0.01;   % modular modality
p     = 0.8;    % switch probability

if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'c'), c = opts.c; end
if isfield(opts,'p'), p = opts.p; end
if isfield(opts,'thres'), thres = opts.thres; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = zeros(N,dim);
for i = 1:N
	for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
	end
end
% Pre
Xnew = zeros(N,dim);
fitG = inf;
fit  = zeros(1,N);

curve = inf;
t = 1;
% Iterations
while t <= max_Iter
  % Fitness
  for i = 1:N
    fit(i) = fun(feat,label,(X(i,:) > thres),opts);
    % Global update
    if fit(i) < fitG
      fitG = fit(i);
      Xgb  = X(i,:);
    end
  end
  % Power component, increase from 0.1 to 0.3
  a = 0.1 + 0.2 * (t / max_Iter);
  for i = 1:N
    % Compute fragrance (1)
    f = c * (fit(i) ^ a);
    % Random number in [0,1]
    r = rand();
    if r < p
      r1 = rand();
      for d = 1:dim
        % Move toward best butterfly (2)
        Xnew(i,d) = X(i,d) + ((r1 ^ 2) * Xgb(d) - X(i,d)) * f;
      end
    else
      % Random select two butterfly
      R  = randperm(N);
      J  = R(1);
      K  = R(2);
      r2 = rand();
      for d = 1:dim
        % Move randomly (3)
        Xnew(i,d) = X(i,d) + ((r2 ^ 2) * X(J,d) - X(K,d)) * f;
      end
    end
    % Boundary
    XB = Xnew(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    Xnew(i,:) = XB;
  end
  % Replace
  X = Xnew;
  % Save
  curve(t) = fitG;
  fprintf('\nIteration %d Best (BOA)= %f',t,curve(t))
  t = t + 1;
end
% Select features
Pos   = 1:dim;
Sf    = Pos((Xgb > thres) == 1);
sFeat = feat(:,Sf);
% Store results
BOA.sf = Sf;
BOA.ff = sFeat;
BOA.nf = length(Sf);
BOA.c  = curve;
BOA.f  = feat;
BOA.l  = label;
end


%%*********************************************************************
%% This is an example of using the interface
%% to solve the following theta problem of a graph
%% with adjacency matrix G:
%% max <E,X>
%% s.t. trace(X)==1 ; X positive semidefinite
%%      X(i,j) == 0 for (i,j) in Edge_set
%% SDPNAL+:
%% Copyright (c) 2017 by
%% Yancheng Yuan, Kim-Chuan Toh, Defeng Sun and Xinyuan Zhao
%%*********************************************************************

 load theta6.mat
 [IE,JE] = find(triu(G,1));
 n = length(G);
 %%
 model = ccp_model('Example_theta');
     X = var_sdp(n, n);
     model.add_variable(X);
     model.maximize(sum(X));
     model.add_affine_constraint(trace(X) == 1);
     options=2;
     if (options==1)
        model.add_affine_constraint(X(IE,JE) == 0);
        model.add_affine_constraint(X >= 0);
     else %% another way to specify the constraints
        U = 1e20*(1-spones(G+G'));
        model.add_affine_constraint(0 <= X <= U);
     end
 model.solve;
%%*********************************************************************


%tstoolbox/mex/baker
%   Generate time-series from the iterated Baker map .
%
%   Syntax:
%
%     * x = baker(length, [eta l1 l2 x0 y0])
%
%   Input arguments:
%
%     * length - number of samples to generate
%     * [eta l1 l2 x0 y0] - vector of parameters and initial conditions
%
%   Output arguments:
%
%     * x - time series
%
%   Example:
%
%x = baker(2000, [0.6 0.25 0.4 rand(1,1) rand(1,1)]);
%plot(x(1:end-1,2), x(2:end,2), '.')
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt


function [ a, irow, icol ] = i4mat_red ( m, n, a )

%*****************************************************************************80
%
%% I4MAT_RED divides out common factors in a row or column of an I4MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in the matrix.
%
%    Input, integer N, the number of columns in the matrix.
%
%    Input, integer A(M,N), the matrix to be reduced.
%
%    Output, integer A(M,N), the reduced matrix.  The greatest common factor
%    in any row or column is 1.
%
%    Output, integer IROW(M), the row factors that were divided out.
%
%    Output, integer ICOL(N), the column factors that were divided out.
%
  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_RED - Warning!\n' );
    fprintf ( 1, '  M must be greater than 0.\n' );
    fprintf ( 1, '  Input M = %d\n', m );
    error ( 'I4MAT_RED - Fatal error!' );
  end

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_RED - Warning!\n' );
    fprintf ( 1, '  N must be greater than 0.\n' );
    fprintf ( 1, '  Input N = %d\n', n );
    error ( 'I4MAT_RED - Fatal error!' );
  end
%
%  Remove factors common to a column.
%
  incx = 1;
  for j = 1 : n
    [ a(1:m,j), icol(j) ] = i4vec_red ( m, a(1:m,j), incx );
  end
%
%  Remove factors common to a row.
%
  incx = 1;
  for i = 1 : m
    [ a(i,1:n), irow(i) ] = i4vec_red ( n, a(i,1:n), incx );
  end

  return
end


function hermite_cubic_test02 ( )

%*****************************************************************************80
%
%% HERMITE_CUBIC_TEST02 tests HERMITE_CUBIC_VALUE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HERMITE_CUBIC_TEST02:\n' );
  fprintf ( 1, '  HERMITE_CUBIC_VALUE evaluates a Hermite cubic polynomial.\n' );
  fprintf ( 1, '  Try out data from a cubic function:\n' );
  fprintf ( 1, '  on [0,10] and [-1.0,1.0] and [0.5,0.75]\n' );

  for x_interval = 1 : 3

    if ( x_interval == 1 )
      x1 = 0.0;
      x2 = 10.0;
    elseif ( x_interval == 2 )
      x1 = -1.0;
      x2 = +1.0;
    elseif ( x_interval == 3 )
      x1 = 0.5;
      x2 = 0.75;
    end

    [ f1, d1, s1, t1 ] = cubic_value ( x1 );
    [ f2, d2, s2, t2 ] = cubic_value ( x2 );

    fprintf ( 1, '\n' );
    fprintf ( 1, '    J      X           F           D           S           T\n' );

    for j = - 3 : 12
      x = ( ( 10 - j ) * x1   ...
          +        j   * x2 ) ...
            / 10.0;

      [ f, d, s, t ] = hermite_cubic_value ( x1, f1, d1, x2, f2, d2, 1, x );
      [ fc, dc, sc, tc ] = cubic_value ( x );

      fprintf ( 1, '\n' );
      if ( j == 0 )
        fprintf ( 1, '*Data  %10f  %10f  %10f\n', x1, f1, d1 );
      end
      fprintf ( 1, 'Exact  %10f  %10f  %10f  %10f  %10f\n',    x,  fc, dc, sc, tc );
      fprintf ( 1, '  %3d  %10f  %10f  %10f  %10f  %10f\n', j, x,  f,  d,  s,  t );
      if ( j == 10 )
        fprintf ( 1, '*Data  %10f  %10f  %10f\n', x2, f2, d2 );
      end

    end

  end

  return
end


function [energy, jumpPenalty, dataError] = energyL2Potts( u, f, gamma, A, isotropic )
%energyL2Potts Computes the energy of the L2 Potts functional

if exist('A', 'var') && not(isempty(A))
    Au = A * u;
    dataError = sum((Au(:) - f(:)).^2);
else
    dataError = sum((u(:) - f(:)).^2);
end

if isvector(u)
    % vectors
    jumpPenalty = sum(diff(u(:)) ~= 0);
else
    % matrices
    nJumpComp = 0; % jumps in compass directions
    nJumpDiag = 0; % jumps in diagonal directions

    % count jumps
    for i = 1:size(u, 1)
        for j = 1:size(u, 2)-1
            if ~all((u(i,j,:) == u(i,j+1,:)))
                nJumpComp = nJumpComp + 1;
            end
        end
    end
    for i = 1:size(u, 1)-1
        for j = 1:size(u, 2)
            if ~all((u(i,j,:) == u(i+1,j,:)))
                nJumpComp = nJumpComp + 1;
            end
        end
    end
    for i = 1:size(u, 1)-1
        for j = 1:size(u, 2)-1
            if ~all((u(i,j,:) == u(i+1,j+1,:)))
                nJumpDiag = nJumpDiag + 1;
            end
        end
    end
    for i = 1:size(u, 1)-1
        for j = 2:size(u, 2)
            if ~all((u(i,j,:) == u(i+1,j-1,:)))
                nJumpDiag = nJumpDiag + 1;
            end
        end
    end

    % set weights (isotropic by default)
    if ~exist('isotropic', 'var') || isotropic
        omega1 = sqrt(2) - 1;
        omega2 = 1 - sqrt(2)/2;
    else
        omega1 = 1;
        omega2 = 0;
    end

    % compute energy
    jumpPenalty = (omega1 * nJumpComp + omega2* nJumpDiag);

end

energy = gamma * jumpPenalty + dataError;

end


function M = euclideansubspacefactory(E, proj, dim)
% Returns a manifold struct to optimize over a subspace of a linear manifold
%
% The factory produces a manifold description M for a linear subspace S of
% a linear space E.
%
% Points and tangent vectors on M are represented in memory in exactly the
% same way as they are for E. Furthermore, E is considered the embedding
% space of S. In particular, the Euclidean gradient of a function is
% understood as the gradient of that function in E, whereas the gradient of
% that function in S is here called the Riemannian gradient. Similar
% considerations hold for the Hessian.
%
% The linear space E is described by a structure obtained from a factory.
% For example, E can be the space of vectors, matrices or nD-arrays of real
% or complex numbers, or a subspace of those. E is equipped with a (real)
% inner product, making it a (real) Euclidean space.
%
% The subspace S is described by an orthogonal projector proj from E to E,
% whose image (i.e., span, range, ...) is S. Orthogonality is judged with
% respect to the inner product on E, and S inherits that inner product,
% making it a Euclidean space itself, and a Riemannian submanifold of E.
%
% The dimension of the subspace S must ideally also be specified.
%
% Inputs:
%   E is the factory for a linear space; for example:
%       E = euclideanfactory(n)
%       E = euclideanfactory(n, m)
%       E = euclideanfactory([n1, n2, n3])
%       E = euclideancomplexfactory(n) % and other dimensions too
%       ...
%
%   proj is a function handle that takes as input a point u of E and
%       returns another point of E: the orthogonal projection of u to S.
%       Orthogonality is understood with respect to the inner product on E,
%       that is, E.inner(x, u, v) = <u, v>. Like any orthogonal projector,
%       it must be linear, and it must satisfy:
%           <u, Pv> = <Pu, v>    and    PPw = Pw
%       for all points u, v, w in E.
%       To check these properties on random vectors, call M.checkproj();
%
%   dim is the dimension of the subspace S (an integer). As always in
%       Manopt, we consider linear spaces over the real numbers, hence,
%       this is the dimension of S as a real linear space. For example, the
%       dimension of R^n is n, and the dimension of C^n is 2n.
%
% Output:
%   M is a factory for optimization over the subspace S.
%
%
% Example:
%
% Among complex vector of length n, consider the subspace S of such vectors
% that can be the discrete Fourier transform of a real vector of length n
% (that is, the subspace generated by feeding all real vectors of length n
% to fft). The factory M below describes that subspace.
%
%   n = 17;
%   E = euclideancomplexfactory(n);
%   proj = @(u) (u + conj(u([1 ; (n:-1:2)'])))/2;
%   M = euclideansubspacefactory(E, proj, n);
%   M.checkproj(); % for debugging
%
%
% Note 1: this factory is designed to work with linear subspaces only: it
% does not work for affine subspaces. Explicitly: S must contain the origin
% of E. If you need support for affine subspaces, let us know on the Manopt
% forum and we can help (or share your improved code :)).
%
% Note 2: the linear space E can itself be a linear subspace of another.
% For example, we can have:
%       E = symmetricfactory(n, k)
%       E = skewfactory(n, k)
%       E = euclideansubspacefactory(...) % recursive nesting
% For these use-cases, bear in mind that the embedding space of M is E,
% hence, the Euclidean gradient and Hessian are expected to be given in E,
% not in the 'bigger' linear space E possibly lives in.
%
% See also: euclideanfactory euclideancomplexfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, April 25, 2019.
% Contributors:
% Change log:

    M = E;

    M.name = @() ['Subspace of ', E.name()];

    M.proj = @(x, u) proj(u);

    M.egrad2rgrad = @(x, eg) proj(eg);

    M.ehess2rhess = @(x, eg, eh, d) proj(eh);

    M.tangent = @(x, u) proj(u);

    M.rand = @() proj(E.rand());

    M.randvec = @randvec;
    function v = randvec(x)
        v = proj(E.randvec(x));
        v = v / E.norm(x, v);
    end

    if exist('dim', 'var')
        M.dim = @() dim;
        M.typicaldist = @() sqrt(dim);
    else
        M = rmfield(M, 'dim');
        warning('manopt:subspacedim', ...
               ['Since the dimension of the subspace was not specified' ...
                ', M.dim() is not available in the returned factory.']);
    end

    M.checkproj = @() checkproj(E, proj);

end

% Tool to check that proj is indeed an orthogonal projector to a subspace
% of E, with respect to the inner product on the linear space E.
function checkproj(E, proj)
    x = E.rand();
    u = E.randvec(x);
    v = E.randvec(x);
    a = randn();
    b = randn();
    % Check that P is linear.
    Paubv = proj(E.lincomb(x, a, u, b, v));
    aPvbPv = E.lincomb(x, a, proj(u), b, proj(v));
    fprintf(['Is proj linear? This number should be zero up to ' ...
             'machine precision:\n\t%.16e\n'], ...
                E.norm(x, E.lincomb(x, 1, Paubv, -1, aPvbPv)));
    % Check that PPw = Pw.
    fprintf(['Is it a projector? This number should be zero up to ' ...
             'machine precision:\n\t%.16e\n'], ...
                E.norm(x, E.lincomb(x, 1, proj(u), -1, proj(proj(u)))));
    % Check that <u, Pv> = <Pu, v>.
    fprintf(['Is it self-adjoint? These two numbers should be equal ' ...
             'up to machine precision:\n\t%.16e\n\t%.16e\n'], ...
                E.inner(x, u, proj(v)), ...
                E.inner(x, proj(u), v));
end


% Reconstruction of image from Laplacian pyramid
%
% Arguments:
%   pyramid 'pyr', as generated by function 'laplacian_pyramid'
%
% tom.mertens@gmail.com, August 2007
%
%
% More information:
%   'The Laplacian Pyramid as a Compact Image Code'
%   Burt, P., and Adelson, E. H.,
%   IEEE Transactions on Communication, COM-31:532-540 (1983).
%

function R = reconstruct_laplacian_pyramid_(pyr)

r = size(pyr{1},1);
c = size(pyr{1},2);
nlev = length(pyr);

% start with low pass residual
R = pyr{nlev};
filter = pyramid_filter;
for l = nlev - 1 : -1 : 1
    % upsample, and add to current level
    odd = 2*size(R) - size(pyr{l});
    R = pyr{l} + upsample_(R,odd,filter);
end