function [x,fval,exitflag,output]=cplexmiqcp(H, f, Aineq, bineq, Aeq, beq, l, Q, r, sostype, sosind, soswt, lb, ub, ctype, x0, options)
%%
% Purpose
% Solve quadratically constrained linear or quadratic integer programming
% problems.
%
% Syntax
%    x = cplexmiqcp(H,f,Aineq,bineq)
%    x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq)
%    x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r)
%    x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt)
%    x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt,lb,
%        ub)
%    x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt,lb,
%        ub,ctype,x0)
%    x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt,lb,
%        ub,ctype,x0,options)
%    x = cplexmiqcp(problem)
%    [x,fval] = cplexmiqcp(...)
%    [x,fval,exitflag] = cplexmiqcp(...)
%    [x,fval,exitflag,output] = cplexmiqcp(...)
%    [x,fval,exitflag,output,lambda] = cplexmiqcp(...)
%
% Description
% Finds the minimum of a problem specified by
%    min      0.5*x'*H*x+f*x or f*x
%    st.      Aineq*x      <= bineq
%             Aeq*x         = beq
%             l*x + x'*Q*x <= r
%             lb <= x <= ub
%             x belongs to BICSN
%
% f, bineq, beq, l, r, lb and ub are column vectors.
% H, Aineq, Aeq, and Q are matrices.
% x is a BICSN vector -- that is, its individual entries are each required
% to be binary, general integer, continuous, semi-continuous or
% semi-integer.
%
% x = cplexmiqcp(H,f,Aineq,bineq) solves the mixed integer programming
% problem min 1/2*x'*H*x + f*x subject to Aineq*x <= bineq. If no
% quadratic objective term exists, set H=[].
%
% x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq) solves the preceding problem
% while additionally satisfying the equality constraints Aeq*x = beq. If no
% inequalities exist, set Aineq=[] and bineq=[].
%
% x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r) solves the preceding
% problem while additionally satisfying the quadratic inequality
% constraints l*x + x'*Q*x <= r. If no equalities exist, set Aeq=[] and beq=[].
%
% x = cplexmiqcp(f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt) solves
% the preceding problem with the additional requirement that the SOS
% constraints are satisfied. If no quadratic inequalities exist, set l=[],
% Q=[] and r=[].
%
% x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt,lb,ub)
% defines a set of lower and upper bounds on the design variables, x, so
% that the solution is in the range lb <= x <= ub. If no SOS constraints
% exist, set sostype=[],sosind=[] and soswt=[].
%
% x =
% cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt,lb,ub,ctype
% ) defines the types for each of the design variables. If no bounds exist,
% set lb=[] and ub=[].
%
% x =
% cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,sostype,sosind,soswt,lb,ub,x0)
% sets the starting point to x0. If all design variables are continuous,
% set ctype=[].
%
% x = cplexmiqcp(H,f,Aineq,bineq,Aeq,beq,l,Q,r,lb,ub,ctype,x0,options) minimizes
% with the optimization options specified in the structure options, which
% can be created using the function cplexoptimset If you do not wish to
% give an initial point, set x0=[].
%
% x = cplexmiqcp(problem) where problem is a structure.
%
% [x,fval] = cplexmiqcp(...) returns the value of the objective function at
% the solution x: fval = 0.5*x'*H*x + f*x.
%
% [x,fval,exitflag] = cplexmiqcp(...) returns a value exitflag that
% describes the exit condition of cplexmiqcp.
%
% [x,fval,exitflag,output] = cplexmiqcp(...) returns a structure output
% that contains information about the optimization.
%
% Input Arguments
% H         Double matrix for objective function
% f         Double column vector for objective function
% Aineq     Double matrix for linear inequality constraints
% bineq     Double column vector for linear inequality constraints
% Aeq       Double matrix for linear equality constraints
% beq       Double column vector for linear equality constraints
% l         Double column vector or matrix
%           Linear part of quadratic constraints
% Q         Double matrix or double matrix cell for quadratic
%           constraints
% r         Double or double row vector
%           Righthand side of quadratic inequality constraints
% sostype   String with possible char values  '1', '2'
% sosind    Double column vector or column vector cell of indices for
%           the SOSs to be added
% soswt     Double column vector or column vector cell of weights for
%           the SOSs to be added
% lb        Double column vector of lower bounds
% ub        Double column vector of upper bounds
% ctype     String with possible char values 'B','I','C','S','N'
%           ctype(j) to 'B', 'I','C', 'S', or 'N' to indicate
%           that x(j) should be binary, general integer,
%           continuous, semi-continuous or semi-integer
%           (respectively).
% x0        Double column vector for initial point for x
% options   Options structure created with cplexoptimset
%
% problem   Structure containing the following fields:
%           H         Double matrix for objective function
%           f         Double column vector for objective function
%           Aineq     Double matrix for linear inequality constraints
%           bineq     Double column vector for linear inequality constraints
%           Aeq       Double matrix for linear equality constraints
%           beq       Double column vector for linear equality constraints
%      	    qc 	      Struct vector
%  	        qc(i).a 	 Double column vector for linear part of the
%                        quadratic constraint
%  	        qc(i).rhs    Double for righthand side for quadratic constraint
%  	        qc(i).Q 	 Double matrix for quadratic part of the
%                        quadratic constraint
%        	sos 	  Struct vector representing the SOSs
%  	        sos(i).type  String with possible char values  '1', '2'
%       	sos(i).ind   Double column vector of indices for the SOSs
%                        to be added
%  	        sos(i).wt    Double column vector of weights for the SOSs
%                        to be added
%           lb        Double column vector of lower bounds
%           ub        Double column vector of upper bounds
%           ctype     String with possible char values 'B','I','C','S','N'
%                     ctype(j) to 'B', 'I','C', 'S', or 'N' to indicate
%                     that x(j) should be binary, general integer,
%                     continuous, semi-continuous or semi-integer
%                     (respectively).
%           x0        Double column vector for initial point for x
%           options   Options structure created with cplexoptimset
%
% Output Arguments
% x         Solution found by the optimization function. If exitflag > 0,
%           then x is a solution; otherwise, x is the value of the
%           optimization routine when it terminated prematurely.
% fval      Value of the objective function at the solution x
% exitflag  Integer identifying the reason the optimization algorithm
%           terminated
% output    Structure containing information about the optimization. The
%           fields of the structure are:
%           iterations         Number of iterations
%           algorithm          Optimization algorithm used
%           message            Exit message
%           time               Execution time of the algorithm
%           cplexstatus        Status code of the solution
%           cplexstatusstring  Status string of the solution
%
%
%  See also cplexoptimset
%

% ---------------------------------------------------------------------------
% File: cplexmiqcp.m
% ---------------------------------------------------------------------------
% Licensed Materials - Property of IBM
% 5725-A06 5725-A29 5724-Y48 5724-Y49 5724-Y54 5724-Y55
% Copyright IBM Corporation 2008, 2010. All Rights Reserved.
%
% US Government Users Restricted Rights - Use, duplication or
% disclosure restricted by GSA ADP Schedule Contract with
% IBM Corp.
% ---------------------------------------------------------------------------


function wulff()
    % wulff -- Program for plotting a Wulff net
    % to plot points, first calculate theta = pi*(90-azimuth)/180
    % then rho = tan(pi*(90-dip)/360), and finally the components
    % xp = rho*cos(theta) and yp = rho*cos(theta)
    % turned into function by Celso G Reyes 2017

    N = 50;
    cx = cos(0:pi/N:2*pi);                           % points on circle
    cy = sin(0:pi/N:2*pi);
    xh = [-1 1];                                     % horizontal axis
    yh = [0 0];
    xv = [0 0];                                      % vertical axis
    yv = [-1 1];
    axis([-1 1 -1 1]);
    axis('square');
    plot(xh,yh,'-k',xv,yv,'-k');                     %plot green axes
    axis off;
    set(gca,'NextPlot','add');
    plot(cx,cy,'-k');                                %plot white circle
    psi = 0:pi/N:pi;
    for i = 1:8                                      %plot great circles
        rdip = i*(pi/18);                             %at 10 deg intervals
        radip = atan(tan(rdip)*sin(psi));
        rproj = tan((pi/2 - radip)/2);
        x1 = rproj .* sin(psi);
        x2 = rproj .* (-sin(psi));
        y = rproj .* cos(psi);
        plot(x1,y,':k',x2,y,':k');
    end
    for i = 1:8                                     %plot small circles
        alpha = i*(pi/18);
        xlim = sin(alpha);
        % ylim = cos(alpha);
        x = -xlim:0.01:xlim;
        d = 1/cos(alpha);
        rd = d*sin(alpha);
        y0 = sqrt(rd*rd - (x .* x));
        y1 = d - y0;
        y2 = - d + y0;
        plot(x,y1,':k',x,y2,':k');
    end
    axis('square');
    set(gcf,'color','w');
end


%This computes the hand-eye calibration using the method described in
%"Hand-Eye Calibration Using Dual Quaternions" from
%Konstantinos Daniilidis
%
%Input:
%Hmarker2world      a 4x4xNumber_of_Views Matrix of the form
%                   Hmarker2world(:,:,i) = [Ri_3x3 ti_3x1;[ 0 0 0 1]]
%                   with
%                   i = number of the view,
%                   Ri_3x3 the rotation matrix
%                   ti_3x1 the translation vector.
%                   Defining the transformation of the robot hand / marker
%                   to the robot base / external tracking device
%Hgrid2cam          a 4x4xNumber_of_Views Matrix (like above)
%                   Defining the transformation of the grid to the camera
%
%Output:
%Hcam2marker_       The transformation from the camera to the marker /
%                   robot arm
%err                The residuals from the least square processes
%
%Christian Wengert
%Computer Vision Laboratory
%ETH Zurich
%Sternwartstrasse 7
%CH-8092 Zurich
%www.vision.ee.ethz.ch/cwengert
%wengert@vision.ee.ethz.ch
function [Hcam2marker_, err] = hand_eye_dual_quaternion(Hmarker2world, Hgrid2cam)
%Our quaternions are like this (q1 q2 q3,s )
    %Get n
    n = size(Hmarker2world,3);
    %Make movements (a,B) which are interposition transformations
    %(marker2wordl and cam2grid)
    %transform A,B into dual quaternions
    for i=1:n-1
        A = inv(Hmarker2world(:,:,i+1))*Hmarker2world(:,:,i);
        B = Hgrid2cam(:,:,i+1)*inv(Hgrid2cam(:,:,i));
        [q,qprime] = getDualQuaternion(A(1:3,1:3),A(1:3,4));
        Qa(i).q = q;
        Qa(i).qprime = qprime;

        [q,qprime] = getDualQuaternion(B(1:3,1:3),B(1:3,4));
        Qb(i).q = q;
        Qb(i).qprime = qprime;
    end

    %The dual quaternion is (Q.q + epsilon*Q.prime)
    %a = Qa.q, a' = Qa.prime  idem for b
    S = [];
    for i=1:n-1
        S(:,:,i) = [Qa(i).q(1:3)-Qb(i).q(1:3)   crossprod(Qa(i).q(1:3)+Qb(i).q(1:3)) zeros(3,1) zeros(3,3);...
                    Qa(i).qprime(1:3)-Qb(i).qprime(1:3)   crossprod(Qa(i).qprime(1:3)+Qb(i).qprime(1:3)) Qa(i).q(1:3)-Qb(i).q(1:3)   crossprod(Qa(i).q(1:3)+Qb(i).q(1:3))];
    end

    %Construct T
    T = [];
    for i=1:n-1
        T = [T  S(:,:,i)'];
    end

    T = T';
    %SVD
    [U,S,V] = svd(T);

    %Solution, right null vectors of T
    v7 = V(:,7);
    v8 = V(:,8);

    u1 = v7(1:4);
    v1 = v7(5:8);

    u2 = v8(1:4);
    v2 = v8(5:8);
    %Now lambda1*v7+lambda2*v8 = [q;qprime]
    %
    %or other:
    %
    %lambda1^2*u1'*u1+2*lambda1*lambda2*u1'*u2+lambda2^2*u2'*u2 = 1
    %and
    %lambda1^2*u1'*v1 + lambda1*lambda2*(u1'*v2+u2'*v1)+lambda2*u2'*v1 = 0
    %Setting lambda1/lambda2 = s
    %lambda1^2/lambda2^2*u1'*v1 + lambda1*lambda2/lambda2^2*(u1'*v2+u2'*v1)+lambda2^2/lambda2^2*u2'*v1 = 0
    %s^2*u1'*v1 + s*(u1'*v2+u2'*v1)+u2'*v1 = 0
    %s^2*u1'*v1 + s*(u1'*v2+u2'*v1)+u2'*v1 = 0
    a = u1'*v1;
    b = (u1'*v2+u2'*v1);
%     c = u2'*v1;
    c = u2'*v2 ;
    s = roots([a b c]);

    %insert into equation
    val1 = s(1)^2*u1'*u1+2*s(1)*u1'*u2+u2'*u2;
    val2 = s(2)^2*u1'*u1+2*s(2)*u1'*u2+u2'*u2;
    %Take bigger value
    if(val1>val2)
        s = s(1);
        val = val1;
    else
        s = s(2);
        val = val2;
    end
    %Get lambdas
    lambda2 = sqrt(1/val);
    lambda1 = s*lambda2;

    %This algorithm gives quaternion with the form of (s, q1 q2
    %q3)->contrary to the notation we used above (q1 q2 q3,s )
    %Therefore we must rearrange the elements!
    qfinal = lambda1*v7+lambda2*v8;
    q = [qfinal(2:4);qfinal(1)];
    qprime = [qfinal(6:8);qfinal(5)];

    %Extract transformation
    R = q2dcm(q);
    t = 2*qmult(qprime,qconj(q));
    t = t(1:3);

    %Assign output arguments
    Hcam2marker_ = [R -R*t;[0 0 0 1]]^-1;
    err=[];

%Creates a dual quaternion from a rotation matrix and a translation vector
function [q,qprime] = getDualQuaternion(R,t)
    %Conversion from R,t to the screw representation [d,theta,l,m]

    r = rodrigues(R);
    theta = norm(r);
    l = r/norm(theta);
    %Pitch d
    d = l'*t;
    %Make point c
    c = .5*(t-d*l)+cot(theta/2)*cross(l,t);
    %moment vector
    m = cross(c,l);
    %Rotation quaternion
    %(q1 q2 q3,s )
    q = [sin(theta/2)*l; cos(theta/2)];
    %Get dual
    qprime = [.5*(q(4)*t+cross(t,q(1:3)));-.5*q(1:3)'*t];


function y = geo_mean( varargin )

%GEO_MEAN   Geometric mean.
%   Y=GEO_MEAN(X), where X is a vector, computes the geometrix mean of X. If any
%   of the elements of X are negative, then Y=-Inf. Otherwise, it is equivalent
%   to Y=PROD(X).^(1/LENGTH(X)). All elements must be real.
%
%   For matrices, GEO_MEAN(X) is a row vector containing the geometric means of
%   the columns. For N-D arrays, GEO_MEAN(X) is an array of the geometric means
%   taken along the first non-singleton dimension of X.
%
%   GEO_MEAN(X,DIM) takes the geometric mean along the dimension DIM of X.
%
%   GEO_MEAN(X,DIM,W), where W is a vector of nonnegative integers, computes a
%   weighted geometric mean Y = PROD(X.^W)^(1/SUM(W)). This is more efficient
%   than replicating the values of X W times. Note that W must be a vector,
%   even if X is a matrix, and its length must be the same as SIZE(X,DIM).
%
%   Disciplined convex programming information:
%       GEO_MEAN is concave  and nondecreasing; therefore, when used in CVX
%       specifications, its argument must be concave.

%
% Check arguments
%

persistent P
if isempty( P ),
    P.map = cvx_remap( { 'real' ; 'concave' ; 'l_convex' ; 'l_concave' } );
    P.map = bsxfun( @and, P.map, ~cvx_remap( 'negative' ) );
    P.funcs = { @geo_mean_1, @geo_mean_2, @geo_mean_2, @geo_mean_2 };
    P.zero = 1;
    P.constant = 1;
    P.reduce = true;
    P.reverse = false;
    P.name = 'geo_mean';
    P.errargs = [];
    P.dimarg = 2;
end
[ sx, x, dim, w ] = cvx_get_dimension( varargin, 2 );
if ~isempty( w ),
    if ~( numel(w)==length(w) && isnumeric(w) && isreal(w) && all(w>=0) ),
        cvx_throw( 'Third argument must be a vector of nonnegative numbers.' );
    elseif ~any( w ),
        cvx_throw( 'The weight vector cannot be all zeros.')
    elseif numel( w ) ~= sx(dim),
        cvx_throw( 'Third argument must be a vector of length %d', sx(dim) );
    else
        w = w(:) / sum(w);
    end
end
y = cvx_reduce_op( P, x, dim, w );

function y = geo_mean_1( x, w )
[ nx, nv ] = size( x );
if isempty( w ),
    w = 1 / nx;
else
    w = repmat( w, [ 1, nv ] );
end
y = prod( x .^ w, 1 );

function y = geo_mean_2( x, w ) %#ok
[ nx, nv ] = size( x );
cvx_begin
    hypograph variable y(1,nv);
    { linearize(x), y } == geo_mean_cone( [nx,nv], 1, w, 'func' ); %#ok
    cvx_setnneg(y);
cvx_end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


% EX_MAXWELL_SRC_CUBE: solve Maxwell source problem in a thick ring.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_thick_ring.txt';

% Type of boundary conditions
problem_data.nmnn_sides   = [1 2];
problem_data.drchlt_sides = [3 4 5 6];

% Physical parameters
problem_data.c_mass  = @(x, y, z) ones(size(x));
problem_data.c_stiff = @(x, y, z) ones(size(x));

% Source and boundary terms
problem_data.f = @(x, y, z) cat(1, ...
                    reshape (2*sin(y), [1, size(x)]), ...
                    reshape (2*sin(x), [1, size(x)]), ...
                    reshape (x .* y, [1, size(x)]));
problem_data.g = @(x, y, z, ind) test_maxwell_thick_ring_g_nmnn (x, y, z, ind);
problem_data.h = @(x, y, z, ind) cat(1, ...
                    reshape (sin(y), [1, size(x)]), ...
                    reshape (sin(x), [1, size(x)]), ...
                    reshape (x .* y, [1, size(x)]));

% Exact solution (optional)
problem_data.uex     = @(x, y, z) cat(1, ...
                    reshape (sin(y), [1, size(x)]), ...
                    reshape (sin(x), [1, size(x)]), ...
                    reshape (x .* y, [1, size(x)]));
problem_data.curluex = @(x, y, z) cat(1, ...
                    reshape (x, [1, size(x)]), ...
                    reshape (-y, [1, size(x)]), ...
                    reshape (cos(x) - cos(y), [1, size(x)]));

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.degree     = [2 2 2];     % Degree of the bsplines
method_data.regularity = [1 1 1];     % Regularity of the splines
method_data.nsub       = [3 3 3];     % Number of subdivisions
method_data.nquad      = [3 3 3];     % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER
[geometry, msh, space, u] = solve_maxwell_src (problem_data, method_data);

% 4) POST-PROCESSING
% 4.1) EXPORT TO PARAVIEW
output_file = 'maxwell_thick_ring_Deg2_Reg1_Sub3';

vtk_pts = {linspace(0, 1, 15), linspace(0, 1, 15), linspace(0, 1, 15)};
fprintf ('The result is saved in the file %s \n \n', output_file);
sp_to_vtk (u, space, geometry, vtk_pts, output_file, 'u')

% 4.2) Comparison with the exact solution
[error_hcurl, error_l2] = ...
    sp_hcurl_error (space, msh, u, problem_data.uex, problem_data.curluex)

%!demo
%! ex_maxwell_src_thick_ring

%!test
%! problem_data.geo_name = 'geo_thick_ring.txt';
%! problem_data.nmnn_sides   = [1 2];
%! problem_data.drchlt_sides = [3 4 5 6];
%! problem_data.c_mass  = @(x, y, z) ones(size(x));
%! problem_data.c_stiff = @(x, y, z) ones(size(x));
%! problem_data.f = @(x, y, z) cat(1, ...
%!                     reshape (2*sin(y), [1, size(x)]), ...
%!                     reshape (2*sin(x), [1, size(x)]), ...
%!                     reshape (x .* y, [1, size(x)]));
%! problem_data.g = @(x, y, z, ind) test_maxwell_thick_ring_g_nmnn (x, y, z, ind);
%! problem_data.h = @(x, y, z, ind) cat(1, ...
%!                     reshape (sin(y), [1, size(x)]), ...
%!                     reshape (sin(x), [1, size(x)]), ...
%!                     reshape (x .* y, [1, size(x)]));
%! problem_data.uex     = @(x, y, z) cat(1, ...
%!                     reshape (sin(y), [1, size(x)]), ...
%!                     reshape (sin(x), [1, size(x)]), ...
%!                     reshape (x .* y, [1, size(x)]));
%! problem_data.curluex = @(x, y, z) cat(1, ...
%!                     reshape (x, [1, size(x)]), ...
%!                     reshape (-y, [1, size(x)]), ...
%!                     reshape (cos(x) - cos(y), [1, size(x)]));
%! method_data.degree     = [2 2 2];     % Degree of the bsplines
%! method_data.regularity = [1 1 1];     % Regularity of the splines
%! method_data.nsub       = [3 3 3];     % Number of subdivisions
%! method_data.nquad      = [3 3 3];     % Points for the Gaussian quadrature rule
%! [geometry, msh, space, u] = solve_maxwell_src (problem_data, method_data);
%! [error_hcurl, error_l2] = sp_hcurl_error (space, msh, u, problem_data.uex, problem_data.curluex);
%! assert (msh.nel, 27)
%! assert (space.ndof, 300)
%! assert (error_l2, 0.0643150154230899, 1e-14)
%! assert (error_hcurl, 0.141137345617213, 1e-14)


function [M] = covar(x1,x2,k,n)
%
% this function calculates the lag k covariance matrice
% for three time series. Each time series is a 1xn vector
%
if k == 0
   %
   % lag 0 covariance matrix
   %
   p1=find(x1~=0);
   p2=find(x2~=0);
   %p3=find(x3~=0);
   p12=find(x1~=0 & x2~=0);
   %p13=find(x1~=0 & x3~=0);
   %p23=find(x2~=0 & x3~=0);
 %  m1=mean(x1(p1));
 %  m2=mean(x2(p2));
 %  m3=mean(x3(p3));
   m1=0;    % x1, x2 and x3 are residuals characterized by N(0,1)
   m2=0;
   %m3=0;
 %  M(1,1)=std(x1(p1));
 %  M(2,2)=std(x2(p2));
 %  M(3,3)=std(x3(p3));
   M(1,1)=1;   % x1, x2 and x3 are residuals characterized by N(0,1)
   M(2,2)=1;
   %M(3,3)=1;
   M(1,2)=sum((x1(p12)-m1).*(x2(p12)-m2))/(length(p12)-1);
   M(2,1)=M(1,2);
   %M(1,3)=sum((x1(p13)-m1).*(x3(p13)-m3))/(length(p13)-1);
   %M(3,1)=M(1,3);
   %M(2,3)=sum((x2(p23)-m2).*(x3(p23)-m3))/(length(p23)-1);
   %M(3,2)=M(2,3);
else
   %
   % lag k covariance matrix
   %
   x1t=x1(1:n-k);
   x1tk=x1(k+1:n);
   x2t=x2(1:n-k);
   x2tk=x2(k+1:n);
   %x3t=x3(1:n-k);
   %x3tk=x3(k+1:n);
   p1t=find(x1t~=0);
   p2t=find(x2t~=0);
   %p3t=find(x3t~=0);
   p1tk=find(x1tk~=0);
   p2tk=find(x2tk~=0);
   %p3tk=find(x3tk~=0);
 %  m1t=mean(x1t(p1t));
 %  m1tk=mean(x1tk(p1tk));
 %  m2t=mean(x2t(p2t));
 %  m2tk=mean(x2tk(p2tk));
 %  m3t=mean(x3t(p3t));
 %  m3tk=mean(x3tk(p3tk));
 m1t=0;   % x1, x2 and x3 are residuals characterized by N(0,1)
 m1tk=0;
 m2t=0;
 m2tk=0;
 %m3t=0;
 %m3tk=0;
   %
   p11=find(x1t~=0 & x1tk~=0);
   p22=find(x2t~=0 & x2tk~=0);
   %p33=find(x3t~=0 & x3tk~=0);
   p12=find(x1t~=0 & x2tk~=0);
   %p13=find(x1t~=0 & x3tk~=0);
   p21=find(x2t~=0 & x1tk~=0);
   %p23=find(x2t~=0 & x3tk~=0);
   %p31=find(x3t~=0 & x1tk~=0);
   %p32=find(x3t~=0 & x2tk~=0);
   %
   M(1,1)=sum((x1t(p11)-m1t).*(x1tk(p11)-m1tk))/(length(p11)-k-1);
   M(2,2)=sum((x2t(p22)-m2t).*(x2tk(p22)-m2tk))/(length(p22)-k-1);
   %M(3,3)=sum((x3t(p33)-m3t).*(x3tk(p33)-m3tk))/(length(p33)-k-1);
   M(1,2)=sum((x1t(p12)-m1t).*(x2tk(p12)-m2tk))/(length(p12)-k-1);
   M(2,1)=sum((x2t(p21)-m2t).*(x1tk(p21)-m1tk))/(length(p21)-k-1);
   %M(1,3)=sum((x1t(p13)-m1t).*(x3tk(p13)-m3tk))/(length(p13)-k-1);
   %M(3,1)=sum((x3t(p31)-m3t).*(x1tk(p31)-m1tk))/(length(p31)-k-1);
   %M(2,3)=sum((x2t(p23)-m2t).*(x3tk(p23)-m3tk))/(length(p23)-k-1);
   %M(3,2)=sum((x3t(p32)-m3t).*(x2tk(p32)-m2tk))/(length(p32)-k-1);
end


%% This function will generate the simulation data of an ODE function.
%You could determine the noise level by input variable "Noise". If you do
%not want any noise, set noise to zero. Please indicate whether your ODE
%function have control input, if the answer is yes, please set the
%"Control" as 1.

% Last Update: 2019/04/21
% Coded By: K

function [d_Data,Data]=Get_Sim_Data(ODE,state0,u,tspan,Noise,Control,Shuffle)
%% Get the size of the state and control
[N1,M1]=size(state0);
[N2,M2]=size(u);

%% Get simulation data by simulating the system using ODE113

% Determine the left hand side derivative
if Control==1
    y_list(1,:)=state0;
    d_y_list(1,:)=ODE(0,y_list(1,:),u(1,:));
    for i=2:length(u)
        [t_1,y_1] = ode15s(@(t_1,y_1)ODE(t_1,y_1,u(i-1,:)),tspan(1,i-1:i),state0);
        y_list(i,:)=y_1(end,:);
        d_y_list(i,:)=ODE(0,y_list(i,:),u(i,:));
        state0=y_list(i,:)';
    end
else
    %Simulate the system ODE
    [t,y]=ode15s(@(t,y)ODE(t,y),tspan,state0);
    y_list=y;
    % Get the derivative data
    d_y_list=ODE(0,y_list')';
end

%% Add some noise to the system
for i=1:N1
    Data(:,i)=y_list(:,i)+Noise*randn(size(y_list(:,i)));
end
%
for i=1:M2
    d_Data(:,i)=d_y_list(:,i)+Noise*randn(size(d_y_list(:,i)));
end

%% Shuffle the data
if Shuffle==1
    Sequence=randperm(size(Data,1));
    Data=Data(Sequence,:);
    d_Data=d_Data(Sequence,:);
end


function [out,varargout] = runningExtreme(input,nSamples,type)
% runningmin - Computes a running extreme of an input vector or matrix
% Optional file header info (to give more details about the function than in the H1 line)
% Syntax: out = runningmin(input,nSamples,type)
% input - vector or matrix of data to return the running min or max from
% nSamples - The size of the window to check for mins or maxes
% type - 'min','max', or 'both' (default) which type of extremes to return
% out - running minimum or maximum requested.  If both minimum is returned
% first, then maximum
%   Example
%  [runMin,runMax] = runningExtreme(data,31,'both')
%
% Subfunctions: vanherk
%   See also: min, max, sort
%% AUTHOR    : Dan Kominsky
%%

if nargin<2
	error('RunningExtreme:notEnoughInputs','Invalid Usage - must specify window size');
elseif nargin ==2
	type = 'both';
end

if ~mod(nSamples,2)
	warning('RunningExtreme:evenWindowSize','Running Min/Max is not meaningful for a windown with an even number of samples');
	nSamples = nSamples+1;
end

[input,dimShift]=shiftdim(input);

switch lower(type)
	case 'min'
		out=vanherk(input,nSamples,'min');% Create output for minimum
	case 'max'
		out=vanherk(input,nSamples,'max'); % Create output for maximum
	otherwise
		out=vanherk(input,nSamples,'max');% Create output for maximum
		varargout{1} = out; % Transfer to the second output
		out=vanherk(input,nSamples,'min');% Create output for minimum
end
if dimShift
	out = shiftdim(out,-dimShift);% if we transposed, undo to return the same shape as we got.
	if nargout>1
		varargout{1} = shiftdim(varargout{1},-dimShift);
	end
end

end

function Y = vanherk(X,N,TYPE)
%  VANHERK    Fast max/min 1D filter
%
%    Y = VANHERK(X,N,TYPE) performs the 1D max/min filtering of the row
%    vector X using a N-length filter.
%    The filtering type is defined by TYPE = 'max' or 'min'. This function
%    uses the van Herk algorithm for min/max filters that demands only 3
%    min/max calculations per element, independently of the filter size.
%
%    If X is a 2D matrix, each column will be filtered separately.
%    X can be uint8 or double. If X is uint8 the processing is quite faster, so
%    dont't use X as double, unless it is really necessary.
%

% Initialization
% if strcmp(direc,'col')
%    X = X';
% end
switch lower(TYPE)
	case 'max'
		maxfilt = 1;
	case 'min'
		maxfilt = 0;
	otherwise
		error([ 'TYPE must be ' char(39) 'max' char(39) ' or ' char(39) 'min' char(39) '.'])
end

% Correcting X size
fixsize = 0;
addel = 0;
if mod(size(X,1),N) ~= 0
	fixsize = 1;
	addel = N-mod(size(X,1),N);
	if maxfilt
		f = [X; -Inf*ones(addel,size(X,2)) ]; % Change from adding zeros
	else
		f = [X; Inf*ones([addel size(X,2)])];
		%       f = [X; repmat(X(end,:),addel,1)];  % Adds a replication of the end of the matrix
	end
else
	f = X;
end
lf = size(f,1); % # of elements in adjusted matrix
lx = size(X,1); % # of elements in original matrix
clear X

% Declaring aux. mat.
g = f;
h = g;

% Filling g & h (aux. mat.)
ig = (1:N:size(f,1)).'; % First element of each window
ih = ig + N - 1;    % Last element of each window

if maxfilt
	for i = 2 : N
		igold = ig;
		ihold = ih;

		ig = ig + 1;
		ih = ih - 1;

		g(ig,:) = max(f(ig,:),g(igold,:));
		h(ih,:) = max(f(ih,:),h(ihold,:));
	end
else
	for i = 2 : N
		igold = ig;
		ihold = ih;

		ig = ig + 1;
		ih = ih - 1;

		g(ig,:) = min(f(ig,:),g(igold,:));
		h(ih,:) = min(f(ih,:),h(ihold,:));
	end
end
clear f

if fixsize % If we had to pad the data
	if addel > (N-1)/2 % If the padding is more than half a zone
		ig =  (N : 1 : lf - addel + floor((N-1)/2)).' ;
		ih = ( 1 : 1 : lf-N+1 - addel + floor((N-1)/2)).';
		if maxfilt
			Y = [ g(1+ceil((N-1)/2):N-1,:);  max(g(ig,:), h(ih,:)) ];
		else
			Y = [ g(1+ceil((N-1)/2):N-1,:);  min(g(ig,:), h(ih,:)) ];
		end
	else
		ig = ( N : 1 : lf ).';
		ih = ( 1 : 1 : lf-N+1 ).';
		if maxfilt
			Y = [ g(1+ceil((N-1)/2):N-1,:);  max(g(ig,:), h(ih,:));  h(lf-N+2:lf-N+1+floor((N-1)/2)-addel,:) ];
		else
			Y = [ g(1+ceil((N-1)/2):N-1,:);  min(g(ig,:), h(ih,:));  h(lf-N+2:lf-N+1+floor((N-1)/2)-addel,:) ];
		end
	end
else % not fixsize (addel=0, lf=lx)
	ig = ( N : 1 : lx ).';
	ih = ( 1 : 1 : lx-N+1 ).';
	if maxfilt
		Y = [  g(N-ceil((N-1)/2):N-1,:); max( g(ig,:), h(ih,:) );  h(lx-N+2:lx-N+1+floor((N-1)/2),:) ];
	else
		Y = [  g(N-ceil((N-1)/2):N-1,:); min( g(ig,:), h(ih,:) );  h(lx-N+2:lx-N+1+floor((N-1)/2),:) ];
	end
end

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% Data File DTXY1
% Dynamics of a particle, thrown
% with initial velocity v0 under
% angle "alfa' to the Horizon
 m    =  'm';    % mass of the particle
 Fx   = '-k*xt'; % Projections of forces
 Fy   = '-k*yt - m*9.81'; % on axis x and y
 x0   = '0';     % Initial
 y0   = '0';     % coordinates
 v0   = 'v0';    % Initial velocity
 alfa = 'alfa';  % angle between v0 and horizon
 Tend = 20;      % upper bound of integration
 eps  = 1e-10;   % desirable accuracy
 np   = 2;       % number of parameters
 P{1} = 'm';     % mass of the particle
 P{2} = 'k';     % coefiicient of resistance


% Fig. 5.24  Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
% script for Figure 5.24, Lead design.
np=1;
dp=[1 1 0];
nc=[1 2];
dc=[1 10];
nol=conv(np,nc);
dol=conv(dp,dc);
rlocus(nol,dol)
axis([-19 1 -7.5 7.5])
title('Figure 5.24  Root locus for lead design')
hold on
r=roots([1 11 80 140]);
plot(r,'*')
z=0:.1:.9;
 wn=2:2:19;
 sgrid(z, wn)
 hold off


%This Matlab script can be used to reproduce Figure 7.3 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.01 (Last edited: 2019-03-27)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Number of BSs
L = 16;

%Number of UEs per BS
K = 10;

%Number of BS antennas
M = 100;

%Define the pilot reuse factor
f = 2;

%Select the number of setups with random UE locations
nbrOfSetups = 100;

%Select the number of channel realizations per setup
nbrOfRealizations = 200;

%% Propagation parameters

%Communication bandwidth
B = 20e6;

%Total uplink transmit power per UE (mW)
p = 100;

%Define noise figure at BS (in dB)
noiseFigure = 7;

%Compute noise power
noiseVariancedBm = -174 + 10*log10(B) + noiseFigure;

%Select length of coherence block
tau_c = 200;

%Use the approximation of the Gaussian local scattering model
accuracy = 2;

%Angular standard deviation in the local scattering model (in degrees)
ASDdeg = 10;

%Set the range of Delta values in the power control policy of (7.10)
DeltadB = 0:10:20;

%Generate uncorrelated covariance matrices
Runcorr = repmat(eye(M),[1 1 K L L]);

%Prepare to save simulation results
sumSE_MR = zeros(L*K*nbrOfSetups,length(DeltadB));
sumSE_ZF = zeros(L*K*nbrOfSetups,length(DeltadB));
sumSE_SMMSE = zeros(L*K*nbrOfSetups,length(DeltadB));
sumSE_RZF = zeros(L*K*nbrOfSetups,length(DeltadB));
sumSE_MMMSE = zeros(L*K*nbrOfSetups,length(DeltadB));

%% Go through all setups
for n = 1:nbrOfSetups

    %Output simulation progress
    disp([num2str(n) ' setups out of ' num2str(nbrOfSetups)]);

    %Compute channel statistics for one setup
    [R,channelGaindB] = functionExampleSetup(L,K,M,accuracy,ASDdeg);

    %Compute the normalized average channel gain, where the normalization
    %is based on the noise power
    channelGainOverNoiseOriginal = channelGaindB - noiseVariancedBm;

    %Go through all values of Delta
    for s = 1:length(DeltadB)

        %Extract the average channel gains before power control
        channelGainOverNoise = channelGainOverNoiseOriginal;

        %Go through all cells
        for j = 1:L

            %Compute beta_j,min in the power control policy of (7.10)
            betajMin = min(channelGainOverNoiseOriginal(:,j,j));

            %Scale the average channel gains by applying the power control
            %policy of (7.10). Note that we are including this power
            %control here so that we can then view it as if all UEs of
            %transmitting at maximum power
            differenceSNR = channelGainOverNoiseOriginal(:,j,j)-betajMin;
            backoff = differenceSNR-DeltadB(s);
            backoff(backoff<0) = 0;

            channelGainOverNoise(:,j,:) = channelGainOverNoiseOriginal(:,j,:)-repmat(backoff,[1 1 L]);

        end

        %Generate channel realizations with estimates and estimation
        %error correlation matrices
        [Hhat,C,tau_p,Rscaled] = functionChannelEstimates(R,channelGainOverNoise,nbrOfRealizations,M,K,L,p,f);

        %Compute SEs using Theorem 4.1
        [SE_MR,SE_ZF,SE_SMMSE,SE_RZF,SE_MMMSE] = functionComputeSE_UL(Hhat,C,Rscaled,tau_c,tau_p,nbrOfRealizations,M,K,L,p);

        %Save results
        sumSE_MR(1+(n-1)*K*L:n*K*L,s) = SE_MR(:);
        sumSE_ZF(1+(n-1)*K*L:n*K*L,s) = SE_ZF(:);
        sumSE_SMMSE(1+(n-1)*K*L:n*K*L,s) = SE_SMMSE(:);
        sumSE_RZF(1+(n-1)*K*L:n*K*L,s) = SE_RZF(:);
        sumSE_MMMSE(1+(n-1)*K*L:n*K*L,s) = SE_MMMSE(:);

        %Delete large matrices
        clear Hhat C Rscaled;

    end

    %Delete large matrices
    clear R;

end

%% Plot the simulation results

figure;
hold on; box on;

plot(sort(sumSE_MR(:,1)),linspace(0,1,K*L*nbrOfSetups),'k-','LineWidth',1);
plot(sort(sumSE_MR(:,2)),linspace(0,1,K*L*nbrOfSetups),'b-.','LineWidth',1);
plot(sort(sumSE_MR(:,3)),linspace(0,1,K*L*nbrOfSetups),'r--','LineWidth',1);
xlabel('SE per UE [bit/s/Hz]');
ylabel('CDF');
legend('\Delta=0 dB','\Delta=10 dB','\Delta=20 dB','Location','SouthEast');
xlim([0 8]);

figure;
hold on; box on;

plot(sort(sumSE_RZF(:,1)),linspace(0,1,K*L*nbrOfSetups),'k-','LineWidth',1);
plot(sort(sumSE_RZF(:,2)),linspace(0,1,K*L*nbrOfSetups),'b-.','LineWidth',1);
plot(sort(sumSE_RZF(:,3)),linspace(0,1,K*L*nbrOfSetups),'r--','LineWidth',1);
xlabel('SE per UE [bit/s/Hz]');
ylabel('CDF');
legend('\Delta=0 dB','\Delta=10 dB','\Delta=20 dB','Location','SouthEast');
xlim([0 8]);

figure;
hold on; box on;

plot(sort(sumSE_MMMSE(:,1)),linspace(0,1,K*L*nbrOfSetups),'k-','LineWidth',1);
plot(sort(sumSE_MMMSE(:,2)),linspace(0,1,K*L*nbrOfSetups),'b-.','LineWidth',1);
plot(sort(sumSE_MMMSE(:,3)),linspace(0,1,K*L*nbrOfSetups),'r--','LineWidth',1);
xlabel('SE per UE [bit/s/Hz]');
ylabel('CDF');
legend('\Delta=0 dB','\Delta=10 dB','\Delta=20 dB','Location','SouthEast');
xlim([0 8]);


function [data, Ht] = bekk_simulate(T,k,parameters,p,o,q,type)
% Simulation of symmetric and asymmetric BEKK(p,o,q) multivariate volatility models
%
% USAGE:
%  [DATA,HT] = bekk_simulate(T,K,PARAMETERS,P,O,Q,TYPE)
%
% INPUTS:
%   T          - Either a scalar containing the length of the series to simulate, or a T by K matrix
%                  of simulated random variables.  The default is to use standard normals.  Providing
%                  a T by K matrix allows other distributions to be used.
%   PARAMETERS - Vector of parameters.  The form of the parameters depends on the TYPE.
%                  'Scalar':
%                  [CC' a(1) ... a(p) g(1) ... g(o) b(1) ... b(q)]'  (all scalars)
%                  'Diagonal'
%                  [CC' diag(A(:,:,1))' ... diag(A(:,:,p))' diag(G(:,:,1))' ... diag(G(:,:,o))' diag(B(:,:,1))' ... diag(B(:,:,p))']'
%                  'Full'
%                  [CC' f(A(:,:,1)) ... f(A(:,:,p)) f(G(:,:,1)) ... f(G(:,:,o)) f(B(:,:,1)) ... f(B(:,:,q))]'
%                  where CC = chol2vec(C')' and f(M) = M(:)'
%   P          - Positive, scalar integer representing the number of symmetric innovations
%   O          - Non-negative, scalar integer representing the number of asymmetric innovations
%   Q          - Non-negative, scalar integer representing the number of conditional covariance lags
%   TYPE       - String, one of :
%                  'Scalar' (Default)
%                  'Diagonal'
%                  'Full'
%
% OUTPUTS:
%   DATA   - A T by K matrix of simulated data
%   HT     - A [K K T] dimension matrix of conditional covariances
%
% COMMENTS:
%   The dynamics of a BEKK are given by
%
%   H(:,:,t) = C*C' +
%       A(:,:,1)'*OP(:,:,t-1)*A(:,:,1) + ... + A(:,:,p)'*OP(:,:,t-1)*A(:,:,p) +
%       G(:,:,1)'*OPA(:,:,t-1)*G(:,:,1) + ... + G(:,:,o)'*OPA(:,:,t-1)*G(:,:,o) +
%       B(:,:,1)'*G(:,:,t-1)*B(:,:,1) + ... + B(:,:,q)'*OP(:,:,t-1)*B(:,:,q)
%
%   where in the scalar model A(:,:,i) = a(i)*eye(K) (similarly for G and B).
%
%  EXAMPLES:
%    % Scalar with A.^2=.05, G.^2=.1 and B.^2=.88
%    CCp = [1 .5;.5 4];
%    parameters = [chol2vec(chol(CCp)');sqrt([.05,.10,.88])']
%    [data,Ht] = bekk_simulate(1000,2,parameters,1,1,1,'Scalar')
%    % Diagonal
%    parameters = [chol2vec(chol(CCp)');sqrt([.05 .07 .93 .88])']
%    [data,Ht] = bekk_simulate(1000,2,parameters,1,0,1,'Diagonal')
%
% See also BEKK

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if isscalar(T)
    e = randn(2*T,k);
else
    e = T;
    if size(e,2)~=k
        error('T must have K columns when providing simulated random numbers.')
    end
    T = size(e,1);
    e = [e(ceil(rand(T,1)*T),:);e];
end

if strcmpi(type,'Scalar')
    type = 1;
elseif strcmpi(type,'Diagonal')
    type = 2;
elseif strcmpi(type,'Full')
    type = 3;
else
    error('TYPE must be ''Scalar'', ''Diagonal'' or ''Full''.')
end

if p<=0 || floor(p)~=p
    error('P must be a positive scalar integer.')
end
if o<0 || floor(o)~=o
    error('O must be a positive scalar integer.')
end
if q<0 || floor(q)~=q
    error('Q must be a positive scalar integer.')
end

k2 = k*(k+1)/2;
switch type
    case 1
        count = p+o+q;
    case 2
        count = (p+o+q)*k;
    case 3
        count = (p+o+q)*k*k;
end
count = count + k2;
if length(parameters)~=count
    error('PARAMETERS does not have the expected number of elements.')
end
[C,A,G,B] = bekk_parameter_transform(parameters,p,o,q,k,type);
m = zeros(k*k);
for i=1:p
    m = m + kron(A(:,:,i),A(:,:,i));
end
for i=1:o
    m = m + 0.5*kron(G(:,:,i),G(:,:,i));
end
for i=1:q
    m = m + kron(B(:,:,i),B(:,:,i));
end

if max(eigs(m))>1
    backCast = C/.001;
    warning('MFE:nonstationary','The parameters do not correspond to the stationary region.')
else
    backCast = ((eye(k*k)-m)\eye(k*k))*C(:);
    backCast = reshape(backCast,k,k);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Ht = repmat(eye(k),[1 1 2*T]);
eta = zeros(size(e));
for i=1:(2*T)
    Ht(:,:,i) = C;
    for j=1:p
        if (i-j)<=0
            Ht(:,:,i) = Ht(:,:,i) + A(:,:,j)'*backCast*A(:,:,j);
        else
            Ht(:,:,i) = Ht(:,:,i) + A(:,:,j)'*(e(i-j,:)'*e(i-j,:))*A(:,:,j);
        end
    end
    for j=1:o
        if (i-j)<=0
            Ht(:,:,i) = Ht(:,:,i) + G(:,:,j)'*backCast*G(:,:,j);
        else
            Ht(:,:,i) = Ht(:,:,i) + G(:,:,j)'*(eta(i-j,:)'*eta(i-j,:))*G(:,:,j);
        end
    end
    for j=1:q
        if (i-j)<=0
            Ht(:,:,i) = Ht(:,:,i) + B(:,:,j)'*backCast*B(:,:,j);
        else
            Ht(:,:,i) = Ht(:,:,i) + B(:,:,j)'*Ht(:,:,i-j)*B(:,:,j);
        end
    end
    Ht12 = Ht(:,:,i)^(0.5);
    e(i,:) = e(i,:)*Ht12;
    eta(i,:) = e(i,:).*(e(i,:)<0);
end

data = e(T+1:2*T,:);
Ht = Ht(:,:,T+1:2*T);


classdef CEC2017_F22 < PROBLEM
% <single> <real> <constrained>
% CEC'2017 constrained optimization benchmark problem

%------------------------------- Reference --------------------------------
% G. Wu, R. Mallipeddi, and P. N. Suganthan, Problem definitions and
% evaluation criteria for the CEC 2017 competition on constrained real-
% parameter optimization, National University of Defense Technology, China,
% 2016.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        O;      % Optimal decision vector
        Mat;	% Rotation matrix
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2017.mat'),'Data');
            obj.O = Data{12}.o;
            obj.M = 1;
            if isempty(obj.D) || obj.D < 30
                obj.D   = 10;
                obj.Mat = Data{12}.M_10;
            elseif obj.D < 50
                obj.D   = 30;
                obj.Mat = Data{12}.M_30;
            elseif obj.D < 100
                obj.D   = 50;
                obj.Mat = Data{12}.M_50;
            else
                obj.D   = 100;
                obj.Mat = Data{12}.M_100;
            end
            obj.lower    = zeros(1,obj.D) - 100;
            obj.upper    = zeros(1,obj.D) + 100;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            Z = Z*obj.Mat';
            PopObj = sum(100*(Z(:,1:end-1).^2-Z(:,2:end)).^2+(Z(:,1:end-1)-1).^2,2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            Z = Z*obj.Mat';
            PopCon(:,1) = sum(Z.^2-10*cos(2*pi*Z)+10,2) - 100;
            PopCon(:,2) = sum(Z,2) - 2*size(Z,2);
            PopCon(:,3) = 5 - sum(Z,2);
        end
    end
end


% first casadi test for mpc fpr mobile robots
clear all
close all
clc

% CasADi v3.4.5
% addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5')
% CasADi v3.5.5
addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.5.5')

import casadi.*

T = 0.2; %[s]
N = 10; % prediction horizon
rob_diam = 0.3;

v_max = 0.6; v_min = -v_max;
omega_max = pi/4; omega_min = -omega_max;

x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta');
states = [x;y;theta]; n_states = length(states);

v = SX.sym('v'); omega = SX.sym('omega');
controls = [v;omega]; n_controls = length(controls);
rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s

f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u)
U = SX.sym('U',n_controls,N); % Decision variables (controls)
P = SX.sym('P',n_states + n_states);
% parameters (which include the initial state and the reference state)

X = SX.sym('X',n_states,(N+1));
% A vector that represents the states over the optimization problem.

obj = 0; % Objective function
g = [];  % constraints vector

Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states)
R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls)

st  = X(:,1); % initial state
g = [g;st-P(1:3)]; % initial condition constraints
for k = 1:N
    st = X(:,k);  con = U(:,k);
    obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj
    st_next = X(:,k+1);
    f_value = f(st,con);
    st_next_euler = st+ (T*f_value);
    g = [g;st_next-st_next_euler]; % compute constraints
end
% make the decision variable one column  vector
OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)];

nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P);

opts = struct;
opts.ipopt.max_iter = 2000;
opts.ipopt.print_level =0;%0,3
opts.print_time = 0;
opts.ipopt.acceptable_tol =1e-8;
opts.ipopt.acceptable_obj_change_tol = 1e-6;

solver = nlpsol('solver', 'ipopt', nlp_prob,opts);

args = struct;

args.lbg(1:3*(N+1)) = 0;  % -1e-20  % Equality constraints
args.ubg(1:3*(N+1)) = 0;  % 1e-20   % Equality constraints

args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound
args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound
args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound
args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound
args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound
args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound

args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound
args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound
args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound
args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound
%----------------------------------------------
% ALL OF THE ABOVE IS JUST A PROBLEM SET UP

% THE SIMULATION LOOP SHOULD START FROM HERE
%-------------------------------------------
t0 = 0;
x0 = [0 ; 0 ; 0.0];    % initial condition.
xs = [1.5 ; 1.5 ; 0.0]; % Reference posture.

xx(:,1) = x0; % xx contains the history of states
t(1) = t0;

u0 = zeros(N,2);        % two control inputs for each robot
X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables

sim_tim = 20; % Maximum simulation time

% Start MPC
mpciter = 0;
xx1 = [];
u_cl=[];

% the main simulaton loop... it works as long as the error is greater
% than 10^-6 and the number of mpc steps is less than its maximum
% value.
tic
while(norm((x0-xs),2) > 1e-2 && mpciter < sim_tim / T)
    args.p   = [x0;xs]; % set the values of the parameters vector
    % initial value of the optimization variables
    args.x0  = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)];
    sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,...
        'lbg', args.lbg, 'ubg', args.ubg,'p',args.p);
    u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution
    xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY
    u_cl= [u_cl ; u(1,:)];
    t(mpciter+1) = t0;
    % Apply the control and shift the solution
    [t0, x0, u0] = shift(T, t0, x0, u,f);
    xx(:,mpciter+2) = x0;
    X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY
    % Shift trajectory to initialize the next step
    X0 = [X0(2:end,:);X0(end,:)];
    mpciter
    mpciter = mpciter + 1;
end;
toc

ss_error = norm((x0-xs),2)
Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam)


function [x,y]=als_prec(mat,rhs,y,niter)
%Computes an approximate rank-1 solution to the linear system
%   [Y]=ALS_PREC(MAT,RHS,X) Computes (approximate) rank-1 solution to the
%   problems MAT*P = RHS, where A is a low-Kronecker rank matrix
%   P=X x Y, RHS is a low-Kronecker rank matrix
%   B is given as a two-2d cell array, the same is for X
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

a=mat{1}; %The first cell
b=mat{2};
R=size(a,3); %The rank
n=size(a,1);
m=size(b,1);
a1=permute(a,[1,3,2]); a1=reshape(a1,[n*R,n]);
b1=permute(b,[1,3,2]); b1=reshape(b1,[m*R,m]);
rhs1=rhs;
if ( nargin < 5 )
   niter=10;
end
u=rhs{1};
v=rhs{2};
rr=size(u,3);
u1=reshape(u,[n*n,rr]);
v1=reshape(v,[m*m,rr]);

for i=1:niter
    %fprintf('i=%d \n',i);
    %at=a1*x;
    %bt=b1*y;
    %Iterate over x
    bt=b1*y;
    %bt is n*R*n, scalar product of
    bt1=reshape(bt,[m,R,m]); bt1=permute(bt1,[1,3,2]); bt1=reshape(bt1,[m*m,R]);
    p=bt1'*bt1; %p matrix is ready
    %Compute the local matrix sum_{a,b} p_{ab} A^{\top}_b A_a,
    %it is A(k,i,b)*A(k,j,a)*p(a,b)
    a2=reshape(a,[n*n,R]); a2=a2*p; %a2 is (k,j,b), sum over (k,b) with (k,i,b)
    a3=reshape(a,[n,n,R]); a3=permute(a3,[2,1,3]); a3=reshape(a3,[n,n*R]);
    a2=reshape(a2,[n,n,R]); a2=permute(a2,[2,1,3]); a2=reshape(a2,[n,n*R]);
    loc_mat=a3*a2'; %Should be valid even for complex numbers
    %For the right hand side
    %Compute the q matrix <b,v> = b is m x m x R, V is m x m x rr
    %the result is R x rr
    q=bt1'*v1; %q is R x rr
    %Now compute right-hand side as
    %q(R,rr)*U(i,k,rr)*A(j,k,R)

    u2=u1*q'; % u2 is (i,k,R)
    u2=reshape(u2,[n,n*R]);
    a2=reshape(a,[n,n*R]);
    rhs=u2*a2';
    x= loc_mat \ rhs;
    %cond(loc_mat)
    %Iterate over y
    at=a1*x;
    %bt is n*R*n, scalar product of
    at1=reshape(at,[n,R,n]); at1=permute(at1,[1,3,2]); at1=reshape(at1,[n*n,R]);
    p=at1'*at1; %p matrix is ready
    %Compute the local matrix sum_{a,b} p_{ab} A^{\top}_b A_a,
    %or the summation: \sum_{a,b,k} p(a,b) A(i,k,a)*A(j,k,b)
    b2=reshape(b,[m*m,R]); b2=b2*p; %a2 is (k,j,b), sum over (k,b) with (k,i,b)
    b3=reshape(b,[m,m,R]); b3=permute(b3,[2,1,3]); b3=reshape(b3,[m,m*R]);
    b2=reshape(b2,[m,m,R]); b2=permute(b2,[2,1,3]); b2=reshape(b2,[m,m*R]);
    loc_mat=b3*b2'; %Should be valid even for complex numbers
    %For the right hand side
    %Compute the q matrix <b,v> = b is m x m x R, V is m x m x rr
    %the result is R x rr
    q=at1'*u1; %q is R x rr
    %Now compute right-hand side as
    %q(R,rr)*U(i,k,rr)*A(j,k,R)

    v2=v1*q'; % u2 is (i,k,R)
    v2=reshape(v2,[m,m*R]);
    b2=reshape(b,[m,m*R]);
    rhs=v2*b2';
    y= loc_mat \ rhs;
    %    norm(tt_matrix(mat)*kron(tt_matrix(x,1e-10),tt_matrix(y,1e-10))-tt_matrix(rhs1))
    %keyboard;
    %norm(tt_matrix(mat)*kron(tt_matrix(x,1e-10),tt_matrix(y,1e-10))-tt_mat
    %rix(rhs1))
end
return


%WAVELET  1D Wavelet transform with optional singificance testing
%
%   [WAVE,PERIOD,SCALE,COI] = wavelet(Y,DT,PAD,DJ,S0,J1,MOTHER,PARAM)
%
%   Computes the wavelet transform of the vector Y (length N),
%   with sampling rate DT.
%
%   By default, the Morlet wavelet (k0=6) is used.
%   The wavelet basis is normalized to have total energy=1 at all scales.
%
%
% INPUTS:
%
%    Y = the time series of length N.
%    DT = amount of time between each Y value, i.e. the sampling time.
%
% OUTPUTS:
%
%    WAVE is the WAVELET transform of Y. This is a complex array
%    of dimensions (N,J1+1). FLOAT(WAVE) gives the WAVELET amplitude,
%    ATAN(IMAGINARY(WAVE),FLOAT(WAVE) gives the WAVELET phase.
%    The WAVELET power spectrum is ABS(WAVE)^2.
%    Its units are sigma^2 (the time series variance).
%
%
% OPTIONAL INPUTS:
%
% *** Note *** setting any of the following to -1 will cause the default
%               value to be used.
%
%    PAD = if set to 1 (default is 0), pad time series with enough zeroes to get
%         N up to the next higher power of 2. This prevents wraparound
%         from the end of the time series to the beginning, and also
%         speeds up the FFT's used to do the wavelet transform.
%         This will not eliminate all edge effects (see COI below).
%
%    DJ = the spacing between discrete scales. Default is 0.25.
%         A smaller # will give better scale resolution, but be slower to plot.
%
%    S0 = the smallest scale of the wavelet.  Default is 2*DT.
%
%    J1 = the # of scales minus one. Scales range from S0 up to S0*2^(J1*DJ),
%        to give a total of (J1+1) scales. Default is J1 = (LOG2(N DT/S0))/DJ.
%
%    MOTHER = the mother wavelet function.
%             The choices are 'MORLET', 'PAUL', or 'DOG'
%
%    PARAM = the mother wavelet parameter.
%            For 'MORLET' this is k0 (wavenumber), default is 6.
%            For 'PAUL' this is m (order), default is 4.
%            For 'DOG' this is m (m-th derivative), default is 2.
%
%
% OPTIONAL OUTPUTS:
%
%    PERIOD = the vector of "Fourier" periods (in time units) that corresponds
%           to the SCALEs.
%
%    SCALE = the vector of scale indices, given by S0*2^(j*DJ), j=0...J1
%            where J1+1 is the total # of scales.
%
%    COI = if specified, then return the Cone-of-Influence, which is a vector
%        of N points that contains the maximum period of useful information
%        at that particular time.
%        Periods greater than this are subject to edge effects.
%        This can be used to plot COI lines on a contour plot by doing:
%
%              contour(time,log(period),log(power))
%              plot(time,log(coi),'k')
%
%----------------------------------------------------------------------------
%   Copyright (C) 1995-2004, Christopher Torrence and Gilbert P. Compo
%
%   This software may be used, copied, or redistributed as long as it is not
%   sold and this copyright notice is reproduced on each copy made. This
%   routine is provided as is without any express or implied warranties
%   whatsoever.
%
% Notice: Please acknowledge the use of the above software in any publications:
%    ``Wavelet software was provided by C. Torrence and G. Compo,
%      and is available at URL: http://paos.colorado.edu/research/wavelets/''.
%
% Reference: Torrence, C. and G. P. Compo, 1998: A Practical Guide to
%            Wavelet Analysis. <I>Bull. Amer. Meteor. Soc.</I>, 79, 61-78.
%
% Please send a copy of such publications to either C. Torrence or G. Compo:
%  Dr. Christopher Torrence               Dr. Gilbert P. Compo
%  Research Systems, Inc.                 Climate Diagnostics Center
%  4990 Pearl East Circle                 325 Broadway R/CDC1
%  Boulder, CO 80301, USA                 Boulder, CO 80305-3328, USA
%  E-mail: chris[AT]rsinc[DOT]com         E-mail: compo[AT]colorado[DOT]edu
%----------------------------------------------------------------------------
function [wave,period,scale,coi] = ...
	wavelet(Y,dt,pad,dj,s0,J1,mother,param);

if (nargin < 8), param = -1;, end
if (nargin < 7), mother = -1;, end
if (nargin < 6), J1 = -1;, end
if (nargin < 5), s0 = -1;, end
if (nargin < 4), dj = -1;, end
if (nargin < 3), pad = 0;, end
if (nargin < 2)
	error('Must input a vector Y and sampling time DT')
end

n1 = length(Y);

if (s0 == -1), s0=2*dt;, end
if (dj == -1), dj = 1./4.;, end
if (J1 == -1), J1=fix((log(n1*dt/s0)/log(2))/dj);, end
if (mother == -1), mother = 'MORLET';, end

%....construct time series to analyze, pad if necessary
x(1:n1) = Y - mean(Y);
if (pad == 1)
	base2 = fix(log(n1)/log(2) + 0.4999);   % power of 2 nearest to N
	x = [x,zeros(1,2^(base2+1)-n1)];
end
n = length(x);

%....construct wavenumber array used in transform [Eqn(5)]
k = [1:fix(n/2)];
k = k.*((2.*pi)/(n*dt));
k = [0., k, -k(fix((n-1)/2):-1:1)];

%....compute FFT of the (padded) time series
f = fft(x);    % [Eqn(3)]

%....construct SCALE array & empty PERIOD & WAVE arrays
scale = s0*2.^((0:J1)*dj);
period = scale;
wave = zeros(J1+1,n);  % define the wavelet array
wave = wave + i*wave;  % make it complex

% loop through all scales and compute transform
for a1 = 1:J1+1
	[daughter,fourier_factor,coi,dofmin]=wave_bases(mother,k,scale(a1),param);
	wave(a1,:) = ifft(f.*daughter);  % wavelet transform[Eqn(4)]
end

period = fourier_factor*scale;
coi = coi*dt*[1E-5,1:((n1+1)/2-1),fliplr((1:(n1/2-1))),1E-5];  % COI [Sec.3g]
wave = wave(:,1:n1);  % get rid of padding before returning

return

%WAVE_BASES  1D Wavelet functions Morlet, Paul, or DOG
%
%  [DAUGHTER,FOURIER_FACTOR,COI,DOFMIN] = ...
%      wave_bases(MOTHER,K,SCALE,PARAM);
%
%   Computes the wavelet function as a function of Fourier frequency,
%   used for the wavelet transform in Fourier space.
%   (This program is called automatically by WAVELET)
%
% INPUTS:
%
%    MOTHER = a string, equal to 'MORLET' or 'PAUL' or 'DOG'
%    K = a vector, the Fourier frequencies at which to calculate the wavelet
%    SCALE = a number, the wavelet scale
%    PARAM = the nondimensional parameter for the wavelet function
%
% OUTPUTS:
%
%    DAUGHTER = a vector, the wavelet function
%    FOURIER_FACTOR = the ratio of Fourier period to scale
%    COI = a number, the cone-of-influence size at the scale
%    DOFMIN = a number, degrees of freedom for each point in the wavelet power
%             (either 2 for Morlet and Paul, or 1 for the DOG)
%
%----------------------------------------------------------------------------
%   Copyright (C) 1995-1998, Christopher Torrence and Gilbert P. Compo
%   University of Colorado, Program in Atmospheric and Oceanic Sciences.
%   This software may be used, copied, or redistributed as long as it is not
%   sold and this copyright notice is reproduced on each copy made.  This
%   routine is provided as is without any express or implied warranties
%   whatsoever.
%----------------------------------------------------------------------------
function [daughter,fourier_factor,coi,dofmin] = ...
	wave_bases(mother,k,scale,param);

mother = upper(mother);
n = length(k);

if (strcmp(mother,'MORLET'))  %-----------------------------------  Morlet
	if (param == -1), param = 6.; end
	k0 = param;
	expnt = -(scale.*k - k0).^2/2.*(k > 0.);
	norm = sqrt(scale*k(2))*(pi^(-0.25))*sqrt(n);    % total energy=N   [Eqn(7)]
	daughter = norm*exp(expnt);
	daughter = daughter.*(k > 0.);     % Heaviside step function
	fourier_factor = (4*pi)/(k0 + sqrt(2 + k0^2)); % Scale-->Fourier [Sec.3h]
	coi = fourier_factor/sqrt(2);                  % Cone-of-influence [Sec.3g]
	dofmin = 2;                                    % Degrees of freedom
elseif (strcmp(mother,'PAUL'))  %--------------------------------  Paul
	if (param == -1), param = 4.; end
	m = param;
	expnt = -(scale.*k).*(k > 0.);
	norm = sqrt(scale*k(2))*(2^m/sqrt(m*prod(2:(2*m-1))))*sqrt(n);
	daughter = norm*((scale.*k).^m).*exp(expnt);
	daughter = daughter.*(k > 0.);     % Heaviside step function
	fourier_factor = 4*pi/(2*m+1);
	coi = fourier_factor*sqrt(2);
	dofmin = 2;
elseif (strcmp(mother,'DOG'))  %--------------------------------  DOG
	if (param == -1), param = 2.;, end
	m = param;
	expnt = -(scale.*k).^2 ./ 2.0;
	norm = sqrt(scale*k(2)/gamma(m+0.5))*sqrt(n);
	daughter = -norm*(i^m)*((scale.*k).^m).*exp(expnt);
	fourier_factor = 2*pi*sqrt(2./(2*m+1));
	coi = fourier_factor/sqrt(2);
	dofmin = 1;
else
	error('Mother must be one of MORLET,PAUL,DOG')
end

return

% end of code


function plotbernstein(B,a,b)
%PLOTBERNSTEIN  Plots Berstein points with connecting lines
%
%For univariate Bernstein points,
%
%   plotbernstein(B)
%
%plots Bernstein points in default interval [0,1]. Correspondingly,
%
%   plotbernstein(B,a,b)
%
%plots Bernstein points in the interval [a,b].
%
%For given polynomial P and an interval [a,b], a typical call for plotting P together with
%its Bernstein points is
%
%   B = bernsteincoeff(ptrans(P,a,b,0,1));
%   plotpoly(P,a,b), hold on, plotbernstein(B,a,b), hold off
%

% written  12/25/02     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 11/20/05     S.M. Rump  fast check for rounding to nearest
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  if ~isreal(B.c)
    error('polynomial must be real (point or interval)')
  end

  if size(B.e,2)==1                   % univariate polynomial

    if nargin==1
      a = 0;
      b = 1;
    end
    n = length(B.c);
    X = linspace(a,b,n);

    if isa(B.c,'intval')

      Bc = fliplr([B.c.inf B.c.sup]);
      XX = [X X];
      if n>1
        index = convhull(XX,Bc);
      else
        index = 1:2;
      end
      plot(XX,Bc,'o',XX(index),Bc(index),'-o')

    else

      Bc = fliplr(B.c);
      if n>2
        index = convhull(X,Bc);
      else
        index = 1:length(Bc);
      end
      plot(X,Bc,'o',X(index),Bc(index),'-o')

    end

  elseif size(B.e,2)==2                  % multivariate polynomial in two unknowns
    error('not yet implemented')
  else
    error('Bernstein plot only for polynomials in one or two unknowns')
  end

  setround(rndold)


function [ x, seed ] = r8vec_uniform_ab ( n, a, b, seed )

%*****************************************************************************80
%
%% R8VEC_UNIFORM_AB returns a scaled pseudorandom R8VEC.
%
%  Discussion:
%
%    Each dimension ranges from A to B.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, real A, B, the range of the pseudorandom values.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X(N,1), the vector of pseudorandom values.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_UNIFORM_AB - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8VEC_UNIFORM_AB - Fatal error!' );
  end

  x = zeros ( n, 1 );

  for i = 1 : n

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + 2147483647;
    end

    x(i) = a + ( b - a ) * seed * 4.656612875E-10;

  end

  return
end


function [rhsu] = AdvecRHS3D(u,time)

% function [rhsu] = AdvecRHS3D(u,time)
% Purpose  : Evaluate RHS flux in 3D advection

Globals3D;

% form field differences at faces
alpha=1; du = zeros(Nfp*Nfaces,K);
du(:) = 0.5*(u(vmapM)-u(vmapP)).*(nx(:) - alpha*abs(nx(:)));

% impose boundary condition at x=0
ubc  = exp(-1*( (Fx(mapB)-time).^2 + Fy(mapB).^2 + Fz(mapB).^2));
du(mapB) = 0.5*(u(vmapB)-ubc).*(nx(mapB)-alpha*abs(nx(mapB)));

% compute right hand sides of the semi-discrete PDE
rhsu = -(rx.*(Dr*u)+sx.*(Ds*u)+tx.*(Dt*u)) + LIFT*(Fscale.*(du));
return


function y = atan_pos(x,rnd)
% Rigorous atan(x) for nonnegative double vector x, rounding corresponding
%   to rnd
% For internal use in atan, asin, acos, asec, acsc

% written  12/30/98     S.M. Rump
% modified 08/31/99     S.M. Rump  extreme input, improved speed and accuracy
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/13/12     S.M. Rump  INTLAB_INTVAL_STDFCTS
%

  INTLAB_STDFCTS_PI = getappdata(0,'INTLAB_STDFCTS_PI');

  y = x;

  % transformation of large arguments
  index = ( x>=4 );
  if any(index(:))
    setround(-rnd)
    xx = x(index);
    xx = (-2) ./ ( 1./xx - xx );       % with correct rounding -rnd
    yy = atan_pos_small(xx,-rnd);
    setround(rnd)
    if rnd==-1
      y(index) = INTLAB_STDFCTS_PI.PI2INF - yy/2;
    else
      y(index) = INTLAB_STDFCTS_PI.PI2SUP - yy/2;
    end
  end

  index = ~index;
  if any(index(:))
    y(index) = atan_pos_small(x(index),rnd);
  end

function y = atan_pos_small(x,rnd)
% Rigorous atan(x) for nonnegative double vector x with  0 <= x < 4
% rounding corresponding to rnd

  INTLAB_STDFCTS_ATAN = getappdata(0,'INTLAB_STDFCTS_ATAN');

  setround(0)                               % maximum first 15 bits, no bit
  xs = pow2( floor(pow2(x,13)) , -13 );     %   below 2^-13
  d = x - xs;                               % 0 <= d < 2^-15*x
  atanxs = atan(xs);

  % atan(xs+d) = atan(xs) + atan(d/(1+x*xs))
  setround(-rnd)
  E = 1 + x.*xs;
  setround(rnd)
  E = d./E;                                 % 0 <= E < d/(1+x*xs) < 2^-13

  if rnd==-1                         % 0 <= err <= E^5/5 <= d^4/5*d < 5e-17*d
    % atanE <= atan(E)
    atanE = E + (((-E).*E).*E)/3;
    y = atanxs + ( atanE + (-INTLAB_STDFCTS_ATAN.EPS)*atanxs );
 else
    % atanE >= atan(E)
    atanE = ((( E.*E/5 + (-1)/3 ).*E).*E).*E + E;
    y = atanxs + ( atanE + INTLAB_STDFCTS_ATAN.EPS*atanxs );
  end


classdef Gumbel2C
%%GUMBEL2C A collection of functions for the bivariate Gumbel copulae.
%         Implemented functions include: PDF, CDF, tau.
%
%REFERENCES:
%[1] H. Joe and D. Kurowicka, Dependence Modeling: Vine Copula Handbook.
%    World Scientific, 2011.
%[2] C. Brechmann and U. Schepsmeier, "Dependence modeling with c- and
%    d-vine copulas: The r-package cdvine," Jan. 2011.
%
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    methods(Static)
        function jprob = PDF(u,theta,rot)
            %PDF Generates a joint probability for column vector u or
            %    a row vector of joint probabilities for matrix u
            %    with each column as the input vector.
            %
            %INPUTS:
            % u: A 2-by-n matrix where each column represents a
            %    2-dimensional random vector and n is the number of
            %    random vectors. Entries must be in [0,1].
            % theta: A scalar coupling parameter in the interval
            %        [1,inf).
            % rot: A scalar specifying one of four rotations of the
            %      standard bivariate Gumbel copula. Allowed values are 0,
            %      90, 180, and 270. Default is 0.
            %
            %OUTPUTS: jprob: A 1-by-n vector of joint probabilities.
            %
            %EXAMPLE 1: Make a contour plot of the density.
            %x = linspace(0,1,1e2);
            %[X,Y] = meshgrid(x);
            %Z = zeros(length(X),length(Y));
            %for idx = 1:length(X)
            %    for iidx = 1:length(Y)
            %        Z(idx,iidx) = Gumbel2C.PDF([X(idx,iidx);Y(idx,iidx)],3);
            %    end
            %end
            %contourf(X,Y,Z)
            %
            %EXAMPLE 2: Generate an exact joint density plot.
            % x = linspace(0,1,1e2);
            % [X,Y] = meshgrid(x);
            % Z = zeros(length(X),length(Y));
            % for idx = 1:length(X)
            % for iidx = 1:length(Y)
            % Z(idx,iidx) = Gumbel2C.PDF([X(idx,iidx);Y(idx,iidx)],3);
            % X(idx,iidx) = GumbelD.invCDF(X(idx,iidx),1,2);
            % Y(idx,iidx) = GaussianD.invCDF(Y(idx,iidx));
            % Z(idx,iidx) = Z(idx,iidx).*GumbelD.PDF(X(idx,iidx),1,2).*GaussianD.PDF(Y(idx,iidx));
            % end
            % end
            % surf(X,Y,Z)
            %
            %October 2020 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
            %
            if ~exist('u','var') || isempty(u)
                jprob = [];
                return
            end
            if ~exist('theta','var') || isempty(theta)
                theta = 1;
            elseif theta < 1
                error('Theta parameter cannot be less than 1.')
            end
            if ~exist('rot','var') || isempty(rot)
                rot = 0;
            end

            if rot==0
                %No modifications necessary.
            elseif rot==90
                u(1,:) = 1-u(1,:);
            elseif rot==180
                u(1,:) = 1-u(1,:);
                u(2,:) = 1-u(2,:);
            elseif rot==270
                u(2,:) = 1-u(2,:);
            else
                error('Invalid rotation. Allowed values are 0, 90, 180, and 270.')
            end

            a = -log(u(1,:));
            b = -log(u(2,:));

            jprob = Gumbel2C.CDF(u,theta).*(a.*b).^(theta-1)...
                    .*((a.^theta+b.^theta).^(1/theta)+theta-1)...
                    ./(u(1,:).*u(2,:).*(a.^theta+b.^theta).^(2-(1/theta)));

            %NaN results from 0*inf in the calculation.
            %These should be 0s.
            nanIdxs = isnan(jprob);
            jprob(nanIdxs) = 0;

        end

        function jdist = CDF(u,theta,rot)
            %CDF Generates a joint distribution for column vector u or
            %    a row vector of joint probabilities for matrix u
            %    with each column as the input vector.
            %
            %INPUTS:
            % u: A 2-by-n matrix where each column represents a
            %    2-dimensional random vector and n is the number of
            %    random vectors. Entries must be in [0,1].
            % theta: A scalar coupling parameter in the interval
            %        [1,inf).
            % rot: A scalar specifying one of four rotations of the
            %      standard bivariate Gumbel copula. Allowed values are 0,
            %      90, 180, and 270. Default is 0.
            %
            %OUTPUTS: jprob: A 1-by-n vector of joint distribution values.
            %
            %EXAMPLE 1: Make a contour plot of the distribution.
            %x = linspace(0,1,1e2);
            %[X,Y] = meshgrid(x);
            %Z = zeros(length(X),length(Y));
            %for idx = 1:length(X)
            %    for iidx = 1:length(Y)
            %        Z(idx,iidx) = Gumbel2C.CDF([X(idx,iidx);Y(idx,iidx)],3);
            %    end
            %end
            %contourf(X,Y,Z)
            %
            %EXAMPLE 2: Generate an exact joint distribution plot.
            % x = linspace(0,1,1e2);
            % [X,Y] = meshgrid(x);
            % Z = zeros(length(X),length(Y));
            % for idx = 1:length(X)
            % for iidx = 1:length(Y)
            % Z(idx,iidx) = Gumbel2C.CDF([X(idx,iidx);Y(idx,iidx)],3);
            % X(idx,iidx) = GumbelD.invCDF(X(idx,iidx),1,2);
            % Y(idx,iidx) = GaussianD.invCDF(Y(idx,iidx));
            % Z(idx,iidx) = Z(idx,iidx);
            % end
            % end
            % surf(X,Y,Z)
            %
            %October 2020 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
            %
            if ~exist('u','var') || isempty(u)
                jdist = [];
                return
            end
            if ~exist('theta','var') || isempty(theta)
                theta = 1;
            elseif theta < 1
                error('Theta parameter cannot be less than 1.')
            end
            if ~exist('rot','var') || isempty(rot)
                rot = 0;
            end

            if rot==0
                %No modifications necessary.
                a = -log(u(1,:));
                b = -log(u(2,:));

                jdist = exp(-(a.^theta+b.^theta).^(1/theta));
            elseif rot==90
                a = -log(1-u(1,:));
                b = -log(u(2,:));

                jdist = u(2,:)-exp(-(a.^theta+b.^theta).^(1/theta));
            elseif rot==180
                a = -log(1-u(1,:));
                b = -log(1-u(2,:));

                jdist = u(1,:)+u(2,:)-1+exp(-(a.^theta+b.^theta).^(1/theta));
            elseif rot==270
                a = -log(u(1,:));
                b = -log(1-u(2,:));

                jdist = u(1,:)-exp(-(a.^theta+b.^theta).^(1/theta));
            else
                error('Invalid rotation. Allowed values are 0, 90, 180, and 270.')
            end

        end

        function t = tau(theta)
            %%TAU Compute Kendall's tau for the copula.
            %
            %INPUTS:
            % theta: A scalar coupling parameter in the interval
            %        [1,inf).
            %
            %OUTPUTS:
            % t: The scalar value of Kendall's tau.
            %
            %October 2020 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
            %
            if ~exist('theta','var') || isempty(theta)
                theta = 1;
            elseif theta < 1
                error('Theta parameter cannot be less than 1.')
            end

            t = 1-1/theta;
        end
    end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function stress=outputResult(displacements,numberElements,...
        elementNodes,nodeCoordinates,D)
stress=zeros(3,1);
for e=1:numberElements
  numNodePerElement = length(elementNodes(e,:));
  numEDOF = 2*numNodePerElement;
  elementDof=zeros(1,numEDOF);
  for i = 1:numNodePerElement
      elementDof(2*i-1)=2*elementNodes(e,i)-1;
      elementDof(2*i)=2*elementNodes(e,i);
  end

  %  B matrix
  x1 = nodeCoordinates(elementNodes(e,1),1);
  y1 = nodeCoordinates(elementNodes(e,1),2);
  x2 = nodeCoordinates(elementNodes(e,2),1);
  y2 = nodeCoordinates(elementNodes(e,2),2);
  x3 = nodeCoordinates(elementNodes(e,3),1);
  y3 = nodeCoordinates(elementNodes(e,3),2);
  A = 1/2*det([1 x1 y1; 1 x2 y2; 1 x3 y3]);
  B = 1/(2*A).*[y2-y3 0 y3-y1 0 y1-y2 0;
                        0 x3-x2 0 x1-x3 0 x2-x1;
                        x3-x2 y2-y3 x1-x3 y3-y1 x2-x1 y1-y2];

  stress(e,:)=stress+D*B*displacements(elementDof)/2;
end


function [coeffs, poles, k] = residue(u, v, k)
%RESIDUE   Partial-fraction expansion (residues).
%   [R, P, K] = RESIDUE(B, A) finds the residues, poles and direct term of a
%   partial fraction expansion of the ratio of two CHEBFUN objects B(s)/A(s).
%   If there are no multiple roots,
%        B(s)       R(1)       R(2)             R(n)
%        ----  =  -------- + -------- + ... + -------- + K(s)
%        A(s)     s - P(1)   s - P(2)         s - P(n)
%   B and A are CHEBFUN objects consisting of a single fun. The residues are
%   returned in the column vector R, the pole locations in column vector P, and
%   the direct terms in the CHEBFUN K. The number of poles is n = length(A) - 1
%   = length(R) = length(P). The direct term CHEBFUN is zero if length(B) <
%   length(A), otherwise length(K) = length(B) - length(A) + 1.
%
%   If P(j) = ... = P(j+m-1) is a pole of multiplicity m, then the expansion
%   includes terms of the form
%                  R(j)        R(j+1)                R(j+m-1)
%                -------- + ------------   + ... + ------------
%                s - P(j)   (s - P(j))^2           (s - P(j))^m
%
%   [B, A] = RESIDUE(R, P, K), with 3 input arguments and 2 output arguments,
%   converts the partial fraction expansion back to the polynomials with CHEBFUN
%   representation in B and A.
%
%   Warning: Numerically, the partial fraction expansion of a ratio of
%   polynomials represents an ill-posed problem. If the denominator polynomial,
%   A(s), is near a polynomial with multiple roots, then small changes in the
%   data, including roundoff errors, can make arbitrarily large changes in the
%   resulting poles and residues. Problem formulations making use of state-space
%   or zero-pole representations are preferable.

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

if ( min(size(u)) > 1 || min(size(v)) > 1 )
    error('CHEBFUN:CHEBFUN:residue:quasi', ...
          'Residue does not support CHEBFUN objects with multiple columns.');
end

if ( nargin == 2 )

    if ( (numel(u.funs) > 1) || (numel(v.funs) > 1) )
        error('CHEBFUN:CHEBFUN:residue:multipleFuns', ...
              'Residue does not support CHEBFUNs consisting of multiple FUNs.');
    end
    if ( ~isfinite(u) || ~isfinite(v) )
        error('CHEBFUN:CHEBFUN:residue:inf', ...
              'Residue does not support functions with nonzero exponents.');
    end

    b = poly(u);
    a = poly(v);
    [coeffs, poles, k] = residue(b, a);
    k = chebfun(@(x) polyval(k, x), max(length(k), 1), 'vectorize');

elseif ( nargin == 3 )

    % Interpret an empty CHEBFUN as a zero CHEBFUN for the user's convenience.
    if ( isempty(k) )
        k = 0;
    else
        if ( numel(k.funs) > 1 )
            error('CHEBFUN:CHEBFUN:residue:multipleFuns', ...
                  'RESIDUE does not support CHEBFUNs with multiple FUNS.');
        end

        if ( ~isfinite(k) )
            error('CHEBFUN:CHEBFUN:residue:inf', ...
                  'RESIDUE does not support functions which are unbounded.');
        end

        k = poly(k);
    end

    [b, a] = residue(u, v, k);
    coeffs = chebfun(@(x) polyval(b, x), max(length(b), 1), 'vectorize');
    poles = chebfun(@(x) polyval(a, x), max(length(a), 1), 'vectorize');

else

    error('CHEBFUN:CHEBFUN:residue:args', ...
        'Residue requires either 2 or 3 arguments.');

end

end


function [V,D,Ann] = eign(A1, A2,A3, args)
% Find the principal eigenvalues and eigenvectors of a matrix with Nystr�m's low rank approximation method
%
% >>  D     = eign(A, nb)
% >> [V, D] = eign(A, nb)
%
% In the case of using this method for low rank approximation and
% decomposing the kernel matrix, one can call the function without
% explicit construction of the matrix A.
%
% >>  D     = eign(X, kernel, kernel_par, nb)
% >> [V, D] = eign(X, kernel, kernel_par, nb)
%
%
% Full syntax
%  (We denote the size of positive definite matrix A with a*a.)
%
%     1. Given the full matrix:
%
% >>  D    = eign(A,nb)
% >> [V,D] = eign(A,nb)
%
%
%       Outputs
%         V(*)  : a x nb matrix with estimated principal eigenvectors of A
%         D     : nb x 1 vector with principal estimated eigenvalues of A
%       Inputs
%         A     : a*a positive definite symmetric matrix
%         nb(*) : Number of approximated principal eigenvalues/eigenvectors
%
%
%     2. Given the function to calculate the matrix elements:
%
% >>  D = eign(X, kernel, kernel_par, nb)
% >> [V,D] = eign(X, kernel, kernel_par, nb)
%
%       Outputs
%         V(*)       : a x nb matrix with estimated principal eigenvectors of A
%         D          : nb x 1 vector with estimated principal eigenvalues of A
%       Inputs
%         X          : N x d matrix with the training data
%         kernel     : Kernel type (e.g. 'RBF_kernel')
%         kernel_par : Kernel parameter (bandwidth in the case of the 'RBF_kernel')
%         nb(*)      : Number of eigenvalues/eigenvectors used in the eigenvalue decomposition approximation
%
% See also:
%   eig, eigs, kpca, bay_lssvm

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.be/sista/lssvmlab

% AFUN?
 if nargin~=2
  X = A1;
  kernel = A2;
  kernel_par = A3;
  N = size(X,1);

  %eval(['if args<1, error(''strict positive number of eigenvalues required;'');else n = args; end;'],...
  %'n=min(6,ceil(N*.75));');
    if args<1
      error('Strict positive number of eigenvalues required');
  else
      n = args;
  end;

  % random sampling
  s = randperm(N); sr=s(n+1:end); s=s(1:n);
  %s = ceil(1:(N-1)/(n-1):N); s = s(1:n);

  ANn = zeros(n,N);
  ANn = kernel_matrix(X,kernel,kernel_par,X(s,:));
  Ann = ANn(s,:);

  % centering of matrix
  Zc = eye(n) - 1/n;
  %ZC = eye(N) - 1/N;
  Ann = Zc*Ann*Zc;
  %ANn = ZC*ANn*Zc;
  ANn = (Zc*(ANn*Zc)')';
else
  A = A1;
  N = size(A,1);

  %eval(['if args<1, error(''strict positive number of eigenvalues required;'');else n = args; end;'],...
  %'n=min(6,ceil(N*.75));');

  if A2<1
      error('Strict positive number of eigenvalues required');
  else
      n = A2;
  end;

  % random sampling
  s = randperm(N); sr=s(n+1:end); s=s(1:n);
  %s = ceil(1:(N-1)/(n-1):N); s = s(1:n);

  ANn = A(:,s);
  Ann = A(s,s);
end

%
% compute eigenvalues en vectors of low rank approximation
%
[Vn,Dn] = eig(Ann);Dn = diag(Dn);

%
% select only relevant eigenvalues and sort
% (only largest eigenvectors are orthogonal)
%
[Dn,peff] = sort(Dn(find(Dn>1000*eps)));
Dn = Dn(end:-1:1); peff = peff(end:-1:1);
Vn = Vn(:,peff);

%
% Nystrom correction
%
D = (N/n).*Dn;

%
% eigenvectoren correctie
%
if nargout>1,
  %V = zeros(n,length(peff));
  for i=1:length(peff),
    V(:,i) = (sqrt(n/N)/Dn(peff(i)))*ANn*Vn(:,peff(i));
  end

  %
  % correction of found eigenvectors:
  % orthogonal and unit length
  %

  % svd
  %[V,D2,ff] = svd(V*diag(D.^.5));   V = V(:,1:length(peff));

  % gram schmidt
  %[V,r] = gramschmidt2(V);
  %D = D.*r;

  %D = diag(D.^2);
else
  V=D;
end


function [u,eqn,info,node,elem,Neumann] = fracLap1d(square,h,pde,option)

global s
alpha = 1-2*s;
if s == 0.5
    gamma = 1;
else
    gamma = 3/(2*s)+0.1;
end

tic;
%% Mesh
if option.gradmesh
   [node,elem,y] = squaregradmeshquad(square,h,gamma);
else
   [node,elem] = squarequadmesh(square,h);
    y0 = square(3); y1 = square(4);
    y = (y0:h:y1)';
end
N = size(node,1); NT = size(elem,1); Ndof = N;
% x0 = square(1); x1 = square(2);

%% Compute Local matrix
hy = diff(y);
NTy = length(hy);
NTx = NT/NTy;
% stiffness matrix in y direction
Ay = zeros(NTy,3);
intyalpha = diff(y.^(alpha+1)/(alpha+1));
Ay(:,1) = intyalpha./hy.^2;  % Ay(1,1)
Ay(:,2) = -Ay(:,1);          % Ay(1,2) and Ay(2,1)
Ay(:,3) = Ay(:,1);           % Ay(2,2)
% mass matrix in y direction
My = zeros(NTy,3);
a3 = diff(y.^(alpha+3)/(alpha+3));
a2 = diff(y.^(alpha+2)/(alpha+2));
a1 = diff(y.^(alpha+1)/(alpha+1));
yleft = y(1:end-1);
yright = y(2:end);
My(:,1) = (a3 - 2*yleft.*a2 + yleft.^2.*a1)./hy.^2;  % My(1,1)
My(:,2) = (-a3 + (yleft+yright).*a2 - yleft.*yright.*a1)./hy.^2;    % My(1,2) and My(2,1)
My(:,3) = (a3 - 2*yright.*a2 + yright.^2.*a1)./hy.^2;  % My(2,2)
% stiffness matrix in x direction
hx = h;
Ax = 1/hx*[1 -1; -1 1];
Mx = hx/6*[2 1; 1 2];

%% Assemble stiffness matrix
% index map
ixiy = [1 1; 2 1; 2 2; 1 2];
ij2k(1,1) = 1; ij2k(1,2) = 2; ij2k(2,1) = 2; ij2k(2,2) = 3;
% generate sparse pattern
ii = zeros(10*NT,1); jj = zeros(10*NT,1);
index = 0;
for i = 1:4
    for j = i:4
        ii(index+1:index+NT) = double(elem(:,i));
        jj(index+1:index+NT) = double(elem(:,j));
        index = index + NT;
    end
end
% compute non-zeros
sA = zeros(10*NT,1);
index = 0;
for i = 1:4
    for j = i:4
        ix = ixiy(i,1); iy = ixiy(i,2);
        jx = ixiy(j,1); jy = ixiy(j,2);
        Aij = Ax(ix,jx)*My(:,ij2k(iy,jy)) + Mx(ix,jx)*Ay(:,ij2k(iy,jy));
        sA(index+1:index+NT,1) = repmat(Aij,NTx,1);
        index = index + NT;
    end
end

% assemble the matrix
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
A = A + AU + AU';
clear Aij ii jj AU

%% Right hand side
% f = 0
b = zeros(Ndof,1);

%% Set up boundary conditions
% find boundary nodes and Neumann edges
nx = NTx + 1;
ny = NTy + 1;
fixedNode = [1:ny (2:nx-1)*ny (nx-1)*ny+(1:ny)];
bottom = transpose(1:ny:(nx-1)*ny+1);
Neumann = [bottom(1:end-1) bottom(2:end)];
isBdNode = false(N,1);
isBdNode(fixedNode) = true;
freeNode = find(~isBdNode);

%
% bdFlag = setboundary(node,elem,'Dirichlet','abs(y)>eps','Neumann','abs(y)<=eps');
% [fixedNode,Neumann,isBdNode] = findboundary(elem,bdFlag);

% Modify the matrix to include the Dirichlet boundary condition
bdidx = zeros(Ndof,1);
bdidx(fixedNode) = 1;
Tbd = spdiags(bdidx,0,Ndof,Ndof);
T = spdiags(1-bdidx,0,Ndof,Ndof);
A = T*A*T + Tbd;

% Neumann boundary condition
el = sqrt(sum((node(Neumann(:,1),:) - node(Neumann(:,2),:)).^2,2));
if ~isfield(option,'gNquadorder')
    option.gNquadorder = 4;   % default order exact for linear gN
end
[lambdagN,weightgN] = quadpts1(option.gNquadorder);
phigN = lambdagN;                 % linear bases
nQuadgN = size(lambdagN,1);
ge = zeros(size(Neumann,1),2);
for pp = 1:nQuadgN
    % quadrature points in the x-y coordinate
    ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
         + lambdagN(pp,2)*node(Neumann(:,2),:);
    gNp = pde.g_N(ppxy);
    for igN = 1:2
        ge(:,igN) = ge(:,igN) + weightgN(pp)*phigN(pp,igN)*gNp;
    end
end
ge = ge.*repmat(el,1,2);
b = b + accumarray(Neumann(:), ge(:),[Ndof,1]);

% Modify right handside for Dirichlet boundary condition
% Neumann edges are considered as open set. So the corner points should be
% set as Dirichlet boundary condition!
b(fixedNode) = 0;

eqn = struct('A',A,'b',b,'freeNode',freeNode);

%% Record assembling time
assembleTime = toc;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve
u = zeros(Ndof,1);
switch option.solver
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
        return
    case 'direct'
        u(freeNode) = A(freeNode,freeNode)\b(freeNode);
        residual = norm(b - A*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'amg'
        option.solver = 'CG';
        [u(freeNode),info] = amg(A(freeNode,freeNode),b(freeNode),option);
end
info.assembleTime = assembleTime;

%% Compute error using boundary integral
[lambdagN,weightgN] = quadpts1(option.gNquadorder);
phigN = lambdagN;                 % linear bases
nQuadgN = size(lambdagN,1);
err = zeros(size(Neumann,1),1);
for pp = 1:nQuadgN
    % quadrature points in the x-y coordinate
    ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
         + lambdagN(pp,2)*node(Neumann(:,2),:);
    uhp = u(Neumann(:,1))*phigN(pp,1) + u(Neumann(:,2))*phigN(pp,2);
    up = pde.exactu(ppxy);
    gNp = pde.g_N(ppxy);
    err = err + weightgN(pp)*gNp.*(up - 2*uhp);
end
err = sum(err.*el) + u'*A*u;
errH1 = sqrt(abs(err));
info.errH1 = errH1;


function [W,H] = nmfsc( V, rdim, sW, sH, fname, showflag )
% nmfsc - non-negative matrix factorization with sparseness constraints
%
% SYNTAX:
% [W,H] = nmfsc( V, rdim, sW, sH, fname, showflag );
%
% INPUTS:
% V          - data matrix
% rdim       - number of components (inner dimension of factorization)
% sW         - sparseness of W, in [0,1]. (give [] if no constraint)
% sH         - sparseness of H, in [0,1]. (give [] if no constraint)
% fname      - name of file to write results into
% showflag   - binary flag. if set then graphically show progress
%
% Note: Sparseness is measured on the scale [0,1] where 0 means
% completely distributed and 1 means ultimate sparseness.
%
% NOTE: There is NO CONVERGENCE CRITERION. The estimation never ends,
% but rather has to be terminated manually. See README file of code
% package for details.
%

% Check that we have non-negative data
if min(V(:))<0, error('Negative values in data!'); end

% Globally rescale data to avoid potential overflow/underflow
V = V/max(V(:));

% Data dimensions
vdim = size(V,1);
samples = size(V,2);

% Create initial matrices
W = abs(randn(vdim,rdim));
H = abs(randn(rdim,samples));
H = H./(sqrt(sum(H.^2,2))*ones(1,samples));

% Make initial matrices have correct sparseness
if ~isempty(sW),
    L1a = sqrt(vdim)-(sqrt(vdim)-1)*sW;
    for i=1:rdim, W(:,i) = projfunc(W(:,i),L1a,1,1); end
end
if ~isempty(sH),
    L1s = sqrt(samples)-(sqrt(samples)-1)*sH;
    for i=1:rdim, H(i,:) = (projfunc(H(i,:)',L1s,1,1))'; end
end

% Initialize displays
if showflag,
   figure(1); clf; % this will show the energies and sparsenesses
   figure(2); clf; % this will show the objective function
   drawnow;
end

% Calculate initial objective
objhistory = 0.5*sum(sum((V-W*H).^2));

% Initial stepsizes
stepsizeW = 1;
stepsizeH = 1;

timestarted = clock;

% Start iteration
iter = 0;
while 1,

    % Show progress
    fprintf('[%d]: %.5f\n',iter,objhistory(end));

    % Save every once in a while
    if rem(iter,5)==0,
	elapsed = etime(clock,timestarted);
	fprintf('Saving...');
	save(fname,'W','H','sW','sH','iter','objhistory','elapsed');
	fprintf('Done!\n');
    end

    % Show stats
    if showflag & (rem(iter,5)==0),
	figure(1);
	subplot(3,1,1); bar(sqrt(sum(W.^2)).*sqrt(sum(H'.^2)));
	cursW = (sqrt(vdim)-(sum(abs(W))./sqrt(sum(W.^2))))/(sqrt(vdim)-1);
	subplot(3,1,2); bar(cursW);
	cursH = (sqrt(samples)-(sum(abs(H'))./sqrt(sum(H'.^2)))) ...
		/(sqrt(samples)-1);
	subplot(3,1,3); bar(cursH);
	if iter>1,
	    figure(2);
	    plot(objhistory(2:end));
    end
    % added for now
    figure(100); imstiled(reshape(W,15,15,43),[],'gray')
	drawnow;
    end

    % Update iteration count
    iter = iter+1;

    % Save old values
    Wold = W;
    Hold = H;

    % ----- Update H ---------------------------------------

    if ~isempty(sH),

	% Gradient for H
	dH = W'*(W*H-V);
	begobj = objhistory(end);

	% Make sure we decrease the objective!
	while 1,

	    % Take step in direction of negative gradient, and project
	    Hnew = H - stepsizeH*dH;
	    for i=1:rdim, Hnew(i,:) = (projfunc(Hnew(i,:)',L1s,1,1))'; end

	    % Calculate new objective
	    newobj = 0.5*sum(sum((V-W*Hnew).^2));

	    % If the objective decreased, we can continue...
	    if newobj<=begobj,
		break;
	    end

	    % ...else decrease stepsize and try again
	    stepsizeH = stepsizeH/2;
	    fprintf('.');
	    if stepsizeH<1e-200,
		fprintf('Algorithm converged.\n');
		return;
	    end

	end

	% Slightly increase the stepsize
	stepsizeH = stepsizeH*1.2;
	H = Hnew;

    else

	% Update using standard NMF multiplicative update rule
	H = H.*(W'*V)./(W'*W*H + 1e-9);

	% Renormalize so rows of H have constant energy
	norms = sqrt(sum(H'.^2));
	H = H./(norms'*ones(1,samples));
	W = W.*(ones(vdim,1)*norms);

    end

    % ----- Update W ---------------------------------------

    if ~isempty(sW),

	% Gradient for W
	dW = (W*H-V)*H';
	begobj = 0.5*sum(sum((V-W*H).^2));

	% Make sure we decrease the objective!
	while 1,

	    % Take step in direction of negative gradient, and project
	    Wnew = W - stepsizeW*dW;
	    norms = sqrt(sum(Wnew.^2));
	    for i=1:rdim,
		Wnew(:,i) = projfunc(Wnew(:,i),L1a*norms(i),(norms(i)^2),1);
	    end

	    % Calculate new objective
	    newobj = 0.5*sum(sum((V-Wnew*H).^2));

	    % If the objective decreased, we can continue...
	    if newobj<=begobj,
		break;
	    end

	    % ...else decrease stepsize and try again
	    stepsizeW = stepsizeW/2;
	    fprintf(',');
	    if stepsizeW<1e-200,
		fprintf('Algorithm converged.\n');
		return;
	    end

	end

	% Slightly increase the stepsize
	stepsizeW = stepsizeW*1.2;
	W = Wnew;

    else

	% Update using standard NMF multiplicative update rule
	W = W.*(V*H')./(W*H*H' + 1e-9);

    end

    % Calculate objective
    newobj = 0.5*sum(sum((V-W*H).^2));
    objhistory = [objhistory newobj];

end


echo on

% Script to demonstrate use of the lp_solve toolkit

clc;
lp=mxlpsolve('make_lp',0,4);
mxlpsolve('add_constraint',lp,[3, 2, 2, 1],3,4);
mxlpsolve('add_constraint',lp,[0, 4, 3, 1],2,3);
mxlpsolve('set_obj_fn',lp,[2, 3, -2, 3]);
result=mxlpsolve('solve',lp)
obj=mxlpsolve('get_objective', lp)
x=mxlpsolve('get_variables', lp)
pause;

% Change a single element, and maximize

clc;
mxlpsolve('set_mat',lp,2,1,0.5);
mxlpsolve('set_maxim',lp);
result=mxlpsolve('solve',lp)
obj=mxlpsolve('get_objective', lp)
x=mxlpsolve('get_variables', lp)
pause;

% Change RHS

clc;
mxlpsolve('set_rh',lp,1,7.45);
result=mxlpsolve('solve',lp)
obj=mxlpsolve('get_objective', lp)
x=mxlpsolve('get_variables', lp)
pause;

% Set var 4 to an integer

clc;
mxlpsolve('set_int',lp,4,1)
result=mxlpsolve('solve',lp)
obj=mxlpsolve('get_objective', lp)
x=mxlpsolve('get_variables', lp)
pause;

% Put in lower and upper bounds

clc;
mxlpsolve('set_lowbo',lp,2,2);
mxlpsolve('set_upbo',lp,4,5.3);
result=mxlpsolve('solve',lp)
obj=mxlpsolve('get_objective', lp)
x=mxlpsolve('get_variables', lp)
pause;

% Delete a constraint

clc;
mxlpsolve('del_constraint',lp,1);
mxlpsolve('add_constraint',lp,[1, 2, 1, 4],3,8);
result=mxlpsolve('solve',lp)
obj=mxlpsolve('get_objective', lp)
x=mxlpsolve('get_variables', lp)
mxlpsolve('delete_lp',lp)
pause;

%%%%%%%%%%%%%

% More examples

% ex1.lp from the lp_solve distribution

clc;
lp=mxlpsolve('make_lp',2,2);
mxlpsolve('set_mat',lp,[2, 1;-4, 4]);
mxlpsolve('set_obj_fn',lp,[-1, 2]);
mxlpsolve('set_int',lp,[1,1]);
mxlpsolve('set_rh_vec',lp,[5, 5]);
mxlpsolve('set_maxim',lp);
result=mxlpsolve('solve',lp)
obj=mxlpsolve('get_objective', lp)
x=mxlpsolve('get_variables', lp)
mxlpsolve('delete_lp',lp);
pause;

% Example 2

clc;
f = [50, 100];
A = sparse([10, 5;4, 10; 1, 1.5]);
b = [2500, 2000, 450];
e = [-1, -1, -1];

[m,n] = size(A);
lp=mxlpsolve('make_lp',m,n);
mxlpsolve('set_obj_fn',lp,f);
mxlpsolve('set_mat',lp,A);
mxlpsolve('set_rh_vec',lp,b);
mxlpsolve('set_maxim',lp);
result=mxlpsolve('solve',lp)
obj=mxlpsolve('get_objective', lp)
x=mxlpsolve('get_variables', lp)
mxlpsolve('delete_lp',lp);
pause;

% Example 3

clc;

f = -[40, 36];
vub = [8, 10];
A = sparse([5, 3]);
b = [45];
e = 1;

[m,n] = size(A);
lp=mxlpsolve('make_lp',m,n);
mxlpsolve('set_obj_fn',lp,f);
mxlpsolve('set_mat',lp,A);
mxlpsolve('set_rh_vec',lp,b);
mxlpsolve('set_constr_type',lp,1,2);
mxlpsolve('set_upbo',lp,1,8);
mxlpsolve('set_upbo',lp,2,10);
mxlpsolve('set_maxim',lp);
result=mxlpsolve('solve',lp)
obj=mxlpsolve('get_objective', lp)
x=mxlpsolve('get_variables', lp)
mxlpsolve('delete_lp',lp);
pause;

% L1 Data fitting example with integer constraint on the intercept

% Generate data

clc;
n = 40;
t = (0:n-1)';
y = 3.5 -.2*t;
y = y + 0.5*randn(size(y));

m = [ones(n,1),t(:)];
a = [m,-m,speye(n)];
f = -[sum(m),sum(-m),2*ones(1,n)];
e = ones(n,1);

vub = [10, 10, 10, 10, 5*ones(1,n)];

[v,x] = lp_solve(f,sparse(a),y,e,[],vub,[1,3]);
p = x(1:2)-x(3:4);
err = y-m*p;

plot(t,y,'o',t,m*p);
xlabel('t');
ylabel('y');

disp('Press any key to continue.');
pause;

clc;
% Now solve bigger problem

n = 200;
m = 100;
a = rand(m,n);
idx = find(a<0.8);
a(idx) = zeros(length(idx),1);
a = sparse(a);
z = rand(n,1);
b = a*z;

[v,x] = lp_solve(-ones(1,n),a,b,zeros(m,1));

plot(a*x-b);
title('Residuals');
xlabel('Equation Number');

echo off