#!/usr/bin/env octave
%% Machine Learning Online Class
%  Exercise 6 | Support Vector Machines
%
%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  exercise. You will need to complete the following functions:
%
%     gaussianKernel.m
%     dataset3Params.m
%     processEmail.m
%     emailFeatures.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%

%% Initialization
clear ; close all; clc

%% =============== Part 1: Loading and Visualizing Data ================
%  We start the exercise by first loading and visualizing the dataset.
%  The following code will load the dataset into your environment and plot
%  the data.
%

fprintf('Loading and Visualizing Data ...\n')

% Load from ex6data1:
% You will have X, y in your environment
load('ex6data1.mat');

% Plot training data
plotData(X, y);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% ==================== Part 2: Training Linear SVM ====================
%  The following code will train a linear SVM on the dataset and plot the
%  decision boundary learned.
%

% Load from ex6data1:
% You will have X, y in your environment
load('ex6data1.mat');

fprintf('\nTraining Linear SVM ...\n')

% You should try to change the C value below and see how the decision
% boundary varies (e.g., try C = 1000)
C = 1;
model = svmTrain(X, y, C, @linearKernel, 1e-3, 20);
visualizeBoundaryLinear(X, y, model);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% =============== Part 3: Implementing Gaussian Kernel ===============
%  You will now implement the Gaussian kernel to use
%  with the SVM. You should complete the code in gaussianKernel.m
%
fprintf('\nEvaluating the Gaussian Kernel ...\n')

x1 = [1 2 1]; x2 = [0 4 -1]; sigma = 2;
sim = gaussianKernel(x1, x2, sigma);

fprintf(['Gaussian Kernel between x1 = [1; 2; 1], x2 = [0; 4; -1], sigma = 0.5 :' ...
         '\n\t%f\n(this value should be about 0.324652)\n'], sim);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% =============== Part 4: Visualizing Dataset 2 ================
%  The following code will load the next dataset into your environment and
%  plot the data.
%

fprintf('Loading and Visualizing Data ...\n')

% Load from ex6data2:
% You will have X, y in your environment
load('ex6data2.mat');

% Plot training data
plotData(X, y);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% ========== Part 5: Training SVM with RBF Kernel (Dataset 2) ==========
%  After you have implemented the kernel, we can now use it to train the
%  SVM classifier.
%
fprintf('\nTraining SVM with RBF Kernel (this may take 1 to 2 minutes) ...\n');

% Load from ex6data2:
% You will have X, y in your environment
load('ex6data2.mat');

% SVM Parameters
C = 1; sigma = 0.1;

% We set the tolerance and max_passes lower here so that the code will run
% faster. However, in practice, you will want to run the training to
% convergence.
model= svmTrain(X, y, C, @(x1, x2) gaussianKernel(x1, x2, sigma));
visualizeBoundary(X, y, model);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% =============== Part 6: Visualizing Dataset 3 ================
%  The following code will load the next dataset into your environment and
%  plot the data.
%

fprintf('Loading and Visualizing Data ...\n')

% Load from ex6data3:
% You will have X, y in your environment
load('ex6data3.mat');

% Plot training data
plotData(X, y);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% ========== Part 7: Training SVM with RBF Kernel (Dataset 3) ==========

%  This is a different dataset that you can use to experiment with. Try
%  different values of C and sigma here.
%

% Load from ex6data3:
% You will have X, y in your environment
load('ex6data3.mat');

% Try different SVM Parameters here
[C, sigma] = dataset3Params(X, y, Xval, yval);

% Train the SVM
model= svmTrain(X, y, C, @(x1, x2) gaussianKernel(x1, x2, sigma));
visualizeBoundary(X, y, model);

fprintf('Program paused. Press enter to continue.\n');
pause;


function [x,flag,relres,iter,resvec] = Pgmres_deflation(A,b,restart,tol,maxit,M1,M2,x,varargin)
%GMRES   Generalized Minimum Residual Method.
%   X = GMRES(A,B) attempts to solve the system of linear equations A*X = B
%   for X.  The N-by-N coefficient matrix A must be square and the right
%   hand side column vector B must have length N. This uses the unrestarted
%   method with MIN(N,10) total iterations.
%
%   X = GMRES(AFUN,B) accepts a function handle AFUN instead of the matrix
%   A. AFUN(X) accepts a vector input X and returns the matrix-vector
%   product A*X. In all of the following syntaxes, you can replace A by
%   AFUN.
%
%   X = GMRES(A,B,RESTART) restarts the method every RESTART iterations.
%   If RESTART is N or [] then GMRES uses the unrestarted method as above.
%
%   X = GMRES(A,B,RESTART,TOL) specifies the tolerance of the method.  If
%   TOL is [] then GMRES uses the default, 1e-6.
%
%   X = GMRES(A,B,RESTART,TOL,MAXIT) specifies the maximum number of outer
%   iterations. Note: the total number of iterations is RESTART*MAXIT. If
%   MAXIT is [] then GMRES uses the default, MIN(N/RESTART,10). If RESTART
%   is N or [] then the total number of iterations is MAXIT.
%
%   X = GMRES(A,B,RESTART,TOL,MAXIT,M) and
%   X = GMRES(A,B,RESTART,TOL,MAXIT,M1,M2) use preconditioner M or M=M1*M2
%   and effectively solve the system inv(M)*A*X = inv(M)*B for X. If M is
%   [] then a preconditioner is not applied.  M may be a function handle
%   returning M\X.
%
%   X = GMRES(A,B,RESTART,TOL,MAXIT,M1,M2,X0) specifies the first initial
%   guess. If X0 is [] then GMRES uses the default, an all zero vector.
%
%   [X,FLAG] = GMRES(A,B,...) also returns a convergence FLAG:
%    0 GMRES converged to the desired tolerance TOL within MAXIT iterations.
%    1 GMRES iterated MAXIT times but did not converge.
%    2 preconditioner M was ill-conditioned.
%    3 GMRES stagnated (two consecutive iterates were the same).
%
%   [X,FLAG,RELRES] = GMRES(A,B,...) also returns the relative residual
%   NORM(B-A*X)/NORM(B). If FLAG is 0, then RELRES <= TOL. Note with
%   preconditioners M1,M2, the residual is NORM(M2\(M1\(B-A*X))).
%
%   [X,FLAG,RELRES,ITER] = GMRES(A,B,...) also returns both the outer and
%   inner iteration numbers at which X was computed: 0 <= ITER(1) <= MAXIT
%   and 0 <= ITER(2) <= RESTART.
%
%   [X,FLAG,RELRES,ITER,RESVEC] = GMRES(A,B,...) also returns a vector of
%   the residual norms at each inner iteration, including NORM(B-A*X0).
%   Note with preconditioners M1,M2, the residual is NORM(M2\(M1\(B-A*X))).
%
%   Example:
%      n = 21; A = gallery('wilk',n);  b = sum(A,2);
%      tol = 1e-12;  maxit = 15; M = diag([10:-1:1 1 1:10]);
%      x = gmres(A,b,10,tol,maxit,M);
%   Or, use this matrix-vector product function
%      %-----------------------------------------------------------------%
%      function y = afun(x,n)
%      y = [0; x(1:n-1)] + [((n-1)/2:-1:0)'; (1:(n-1)/2)'].*x+[x(2:n); 0];
%      %-----------------------------------------------------------------%
%   and this preconditioner backsolve function
%      %------------------------------------------%
%      function y = mfun(r,n)
%      y = r ./ [((n-1)/2:-1:1)'; 1; (1:(n-1)/2)'];
%      %------------------------------------------%
%   as inputs to GMRES:
%      x1 = gmres(@(x)afun(x,n),b,10,tol,maxit,@(x)mfun(x,n));
%
%   Class support for inputs A,B,M1,M2,X0 and the output of AFUN:
%      float: double
%
%   See also BICG, BICGSTAB, BICGSTABL, CGS, LSQR, MINRES, PCG, QMR, SYMMLQ,
%   TFQMR, ILU, FUNCTION_HANDLE.

%   References
%   H.F. Walker, "Implementation of the GMRES Method Using Householder
%   Transformations", SIAM J. Sci. Comp. Vol 9. No 1. January 1988.

%   Copyright 1984-2011 The MathWorks, Inc.
%   $Revision: 1.21.4.15 $ $Date: 2011/05/17 02:33:07 $
global ZH AH
if (nargin < 2)
    error(message('MATLAB:gmres:NumInputs'));
end

% Determine whether A is a matrix or a function.
[atype,afun,afcnstr] = iterchk(A);
if strcmp(atype,'matrix')
    % Check matrix and right hand side vector inputs have appropriate sizes
    [m,n] = size(A);
    if (m ~= n)
        error(message('MATLAB:gmres:SquareMatrix'));
    end
    if ~isequal(size(b),[m,1])
        error(message('MATLAB:gmres:VectorSize', m));
    end
else
    m = size(b,1);
    n = m;
    if ~iscolumn(b)
        error(message('MATLAB:gmres:Vector'));
    end
end

% Assign default values to unspecified parameters
if (nargin < 3) || isempty(restart) || (restart == n)
    restarted = false;
else
    restarted = true;
end
if (nargin < 4) || isempty(tol)
    tol = 1e-6;
end
warned = 0;
if tol < eps
    warning(message('MATLAB:gmres:tooSmallTolerance'));
    warned = 1;
    tol = eps;
elseif tol >= 1
    warning(message('MATLAB:gmres:tooBigTolerance'));
    warned = 1;
    tol = 1-eps;
end
if (nargin < 5) || isempty(maxit)
    if restarted
        maxit = min(ceil(n/restart),10);
    else
        maxit = min(n,10);
    end
end

if restarted
    outer = maxit;
    if restart > n
        warning(message('MATLAB:gmres:tooManyInnerItsRestart',restart, n));
        restart = n;
    end
    inner = restart;
else
    outer = 1;
    if maxit > n
        warning(message('MATLAB:gmres:tooManyInnerItsMaxit',maxit, n));
        maxit = n;
    end
    inner = maxit;
end

% Check for all zero right hand side vector => all zero solution
n2b = norm(b);                   % Norm of rhs vector, b
if (n2b == 0)                    % if    rhs vector is all zeros
    x = zeros(n,1);              % then  solution is all zeros
    flag = 0;                    % a valid solution has been obtained
    relres = 0;                  % the relative residual is actually 0/0
    iter = [0 0];                % no iterations need be performed
    resvec = 0;                  % resvec(1) = norm(b-A*x) = norm(0)
    if (nargout < 2)
        itermsg('gmres',tol,maxit,0,flag,iter,NaN);
    end
    return
end

if ((nargin >= 6) && ~isempty(M1))
    existM1 = 1;
    [m1type,m1fun,m1fcnstr] = iterchk(M1);  %Check the size of Precondioner.
    if strcmp(m1type,'matrix')
        if ~isequal(size(M1),[m,m])
            error(message('MATLAB:gmres:PreConditioner1Size', m));
        end
    end
else
    existM1 = 0;
    m1type = 'matrix';
end

if ((nargin >= 7) && ~isempty(M2))
    existM2 = 1;
    [m2type,m2fun,m2fcnstr] = iterchk(M2);
    if strcmp(m2type,'matrix')
        if ~isequal(size(M2),[m,m])
            error(message('MATLAB:gmres:PreConditioner2Size', m));
        end
    end
else
    existM2 = 0;
    m2type = 'matrix';
end

if ((nargin >= 8) && ~isempty(x))
    if ~isequal(size(x),[n,1])
        error(message('MATLAB:gmres:XoSize', n));
    end
else
    x = zeros(n,1);
end

if ((nargin > 8) && strcmp(atype,'matrix') && ...
        strcmp(m1type,'matrix') && strcmp(m2type,'matrix'))
    error(message('MATLAB:gmres:TooManyInputs'));
end

% Set up for the method
flag = 1;
xmin = x;                        % Iterate which has minimal residual so far
imin = 0;                        % "Outer" iteration at which xmin was computed
jmin = 0;                        % "Inner" iteration at which xmin was computed
tolb = tol * n2b;                % Relative tolerance
evalxm = 0;
stag = 0;
moresteps = 0;
maxmsteps = min([floor(n/50),5,n-maxit]);
maxstagsteps = 3;
minupdated = 0;

x0iszero = (norm(x) == 0);
r = b - iterapp('mtimes',afun,atype,afcnstr,x,varargin{:});
normr = norm(r);                 % Norm of initial residual
if (normr <= tolb)               % Initial guess is a good enough solution
    flag = 0;
    relres = normr / n2b;
    iter = [0 0];
    resvec = normr;
    if (nargout < 2)
        itermsg('gmres',tol,maxit,[0 0],flag,iter,relres);
    end
    return
end
minv_b = b;

if existM1

%    r = P*r;
    r = r-A*(ZH*(AH\(ZH'*r)));
    r = iterapp('mldivide',m1fun,m1type,m1fcnstr,r,varargin{:});
    if ~x0iszero

        b0 = b-A*(ZH*(AH\(ZH'*b)));
        minv_b = iterapp('mldivide',m1fun,m1type,m1fcnstr,b0,varargin{:});
    else
        minv_b = r;
    end
    if ~all(isfinite(r)) || ~all(isfinite(minv_b))
        flag = 2;
        x = xmin;
        relres = normr / n2b;
        iter = [0 0];
        resvec = normr;
        return
    end
end

if existM2
    r = iterapp('mldivide',m2fun,m2type,m2fcnstr,r,varargin{:});
    if ~x0iszero
        minv_b = iterapp('mldivide',m2fun,m2type,m2fcnstr,minv_b,varargin{:});
    else
        minv_b = r;
    end
    if ~all(isfinite(r)) || ~all(isfinite(minv_b))
        flag = 2;
        x = xmin;
        relres = normr / n2b;
        iter = [0 0];
        resvec = normr;
        return
    end
end

normr = norm(r);                 % norm of the preconditioned residual
n2minv_b = norm(minv_b);         % norm of the preconditioned rhs
clear minv_b;
tolb = tol * n2minv_b;
if (normr <= tolb)               % Initial guess is a good enough solution
    flag = 0;
    relres = normr / n2minv_b;
    iter = [0 0];
    resvec = n2minv_b;
    if (nargout < 2)
        itermsg('gmres',tol,maxit,[0 0],flag,iter,relres);
    end
    return
end

resvec = zeros(inner*outer+1,1);  % Preallocate vector for norm of residuals
resvec(1) = normr;                % resvec(1) = norm(b-A*x0)
normrmin = normr;                 % Norm of residual from xmin

%  Preallocate J to hold the Given's rotation constants.
J = sparse(2,inner);

U = sparse(n,inner);
R = sparse(inner,inner);
w = sparse(inner+1,1);

for outiter = 1 : outer
    %  Construct u for Householder reflector.
    %  u = r + sign(r(1))*||r||*e1
    u = r;
    normr = norm(r);
    beta = scalarsign(r(1))*normr;
    u(1) = u(1) + beta;
    u = u / norm(u);

    U(:,1) = u;

    %  Apply Householder projection to r.
    %  w = r - 2*u*u'*r;
    w(1) = -beta;

    for initer = 1 : inner
        %  Form P1*P2*P3...Pj*ej.
        %  v = Pj*ej = ej - 2*u*u'*ej
        v = -2*(u(initer)')*u;
        v(initer) = v(initer) + 1;
        %  v = P1*P2*...Pjm1*(Pj*ej)
        for k = (initer-1):-1:1
            v = v - U(:,k)*(2*(U(:,k)'*v));
        end
        %  Explicitly normalize v to reduce the effects of round-off.
        v = v/norm(v);

        %  Apply A to v.
        v = iterapp('mtimes',afun,atype,afcnstr,v,varargin{:});
        %  Apply Preconditioner.
        if existM1

            v = v-A*(ZH*(AH\(ZH'*v)));
            v = iterapp('mldivide',m1fun,m1type,m1fcnstr,v,varargin{:});
            %v = P*v;
            if ~all(isfinite(v))
                flag = 2;
                break
            end
        end

        if existM2
            v = iterapp('mldivide',m2fun,m2type,m2fcnstr,v,varargin{:});
            if ~all(isfinite(v))
                flag = 2;
                break
            end
        end
        %  Form Pj*Pj-1*...P1*Av.
        for k = 1:initer
            v = v - U(:,k)*(2*(U(:,k)'*v));
        end

        %  Determine Pj+1.
        if (initer ~= length(v))
            %  Construct u for Householder reflector Pj+1.
            u = [zeros(initer,1); v(initer+1:end)];
            alpha = norm(u);
            if (alpha ~= 0)
                alpha = scalarsign(v(initer+1))*alpha;
                %  u = v(initer+1:end) +
                %        sign(v(initer+1))*||v(initer+1:end)||*e_{initer+1)
                u(initer+1) = u(initer+1) + alpha;
                u = u / norm(u);
                U(:,initer+1) = u;

                %  Apply Pj+1 to v.
                %  v = v - 2*u*(u'*v);
                v(initer+2:end) = 0;
                v(initer+1) = -alpha;
            end
        end

        %  Apply Given's rotations to the newly formed v.
        for colJ = 1:initer-1
            tmpv = v(colJ);
            v(colJ)   = conj(J(1,colJ))*v(colJ) + conj(J(2,colJ))*v(colJ+1);
            v(colJ+1) = -J(2,colJ)*tmpv + J(1,colJ)*v(colJ+1);
        end

        %  Compute Given's rotation Jm.
        if ~(initer==length(v))
            rho = norm(v(initer:initer+1));
            J(:,initer) = v(initer:initer+1)./rho;
            w(initer+1) = -J(2,initer).*w(initer);
            w(initer) = conj(J(1,initer)).*w(initer);
            v(initer) = rho;
            v(initer+1) = 0;
        end

        R(:,initer) = v(1:inner);

        normr = abs(w(initer+1));
        resvec((outiter-1)*inner+initer+1) = normr;
        normr_act = normr;

        if (normr <= tolb || stag >= maxstagsteps || moresteps)
            if evalxm == 0
                ytmp = R(1:initer,1:initer) \ w(1:initer);
                additive = U(:,initer)*(-2*ytmp(initer)*conj(U(initer,initer)));
                additive(initer) = additive(initer) + ytmp(initer);
                for k = initer-1 : -1 : 1
                    additive(k) = additive(k) + ytmp(k);
                    additive = additive - U(:,k)*(2*(U(:,k)'*additive));
                end
                if norm(additive) < eps*norm(x)
                    stag = stag + 1;
                else
                    stag = 0;
                end
                xm = x + additive;
                evalxm = 1;
            elseif evalxm == 1
                addvc = [-(R(1:initer-1,1:initer-1)\R(1:initer-1,initer))*...
                    (w(initer)/R(initer,initer)); w(initer)/R(initer,initer)];
                if norm(addvc) < eps*norm(xm)
                    stag = stag + 1;
                else
                    stag = 0;
                end
                additive = U(:,initer)*(-2*addvc(initer)*conj(U(initer,initer)));
                additive(initer) = additive(initer) + addvc(initer);
                for k = initer-1 : -1 : 1
                    additive(k) = additive(k) + addvc(k);
                    additive = additive - U(:,k)*(2*(U(:,k)'*additive));
                end
                xm = xm + additive;
            end
            r = b - iterapp('mtimes',afun,atype,afcnstr,xm,varargin{:});
            if norm(r) <= tol*n2b
                x = xm;
                flag = 0;
                iter = [outiter, initer];
                break
            end
            minv_r = r;
            if existM1

                %minv_r = P*minv_r;
                minv_r = r-A*(ZH*(AH\(ZH'*r)));
                minv_r = iterapp('mldivide',m1fun,m1type,m1fcnstr,minv_r,varargin{:});
                if ~all(isfinite(minv_r))
                    flag = 2;
                    break
                end
            end
            if existM2
                minv_r = iterapp('mldivide',m2fun,m2type,m2fcnstr,minv_r,varargin{:});
                if ~all(isfinite(minv_r))
                    flag = 2;
                    break
                end
            end

            normr_act = norm(minv_r);
            resvec((outiter-1)*inner+initer+1) = normr_act;

            if normr_act <= normrmin
                normrmin = normr_act;
                imin = outiter;
                jmin = initer;
                xmin = xm;
                minupdated = 1;
            end

            if normr_act <= tolb
                x = xm;
                flag = 0;
                iter = [outiter, initer];
                break
            else
                if stag >= maxstagsteps && moresteps == 0
                    stag = 0;
                end
                moresteps = moresteps + 1;
                if moresteps >= maxmsteps
                    if ~warned
                        warning(message('MATLAB:gmres:tooSmallTolerance'));
                    end
                    flag = 3;
                    iter = [outiter, initer];
                    break;
                end
            end
        end

        if normr_act <= normrmin
            normrmin = normr_act;
            imin = outiter;
            jmin = initer;
            minupdated = 1;
        end

        if stag >= maxstagsteps
            flag = 3;
            break;
        end
    end         % ends inner loop

    evalxm = 0;

    if flag ~= 0
        if minupdated
            idx = jmin;
        else
            idx = initer;
        end
        y = R(1:idx,1:idx) \ w(1:idx);
        additive = U(:,idx)*(-2*y(idx)*conj(U(idx,idx)));
        additive(idx) = additive(idx) + y(idx);
        for k = idx-1 : -1 : 1
            additive(k) = additive(k) + y(k);
            additive = additive - U(:,k)*(2*(U(:,k)'*additive));
        end
        x = x + additive;
        xmin = x;
        r = b - iterapp('mtimes',afun,atype,afcnstr,x,varargin{:});
        minv_r = r;
        if existM1

            minv_r = r-A*(ZH*(AH\(ZH'*r)));
            minv_r = iterapp('mldivide',m1fun,m1type,m1fcnstr,minv_r,varargin{:});
            if ~all(isfinite(minv_r))
                flag = 2;
                break
            end
        end
        if existM2
            minv_r = iterapp('mldivide',m2fun,m2type,m2fcnstr,minv_r,varargin{:});
            if ~all(isfinite(minv_r))
                flag = 2;
                break
            end
        end
        normr_act = norm(minv_r);
        r = minv_r;
    end

    if normr_act <= normrmin
        xmin = x;
        normrmin = normr_act;
        imin = outiter;
        jmin = initer;
    end

    if flag == 3
        break;
    end
    if normr_act <= tolb
        flag = 0;
        iter = [outiter, initer];
        break;
    end
    minupdated = 0;
end         % ends outer loop

% returned solution is that with minimum residual
if flag == 0
    relres = normr_act / n2minv_b;
else
    x = xmin;
    iter = [imin jmin];
    relres = normr_act / n2minv_b;
end

% truncate the zeros from resvec
if flag <= 1 || flag == 3
    resvec = resvec(1:(outiter-1)*inner+initer+1);
    indices = resvec==0;
    resvec = resvec(~indices);
else
    if initer == 0
        resvec = resvec(1:(outiter-1)*inner+1);
    else
        resvec = resvec(1:(outiter-1)*inner+initer);
    end
end

% only display a message if the output flag is not used
% if nargout < 2
%     if restarted
%         itermsg(sprintf('gmres(%d)',restart),tol,maxit,[outiter initer],flag,iter,relres);
%     else
%         itermsg(sprintf('gmres'),tol,maxit,initer,flag,iter(2),relres);
%     end
% end

function sgn = scalarsign(d)
sgn = sign(d);
if (sgn == 0)
    sgn = 1;
end

function y = iterapp(op,afun,atype,afcnstr,x,varargin)
%ITERAPP   Apply matrix operator to vector and error gracefully.
%   ITERAPP(OP,AFUN,ATYPE,AFCNSTR,X) applies matrix operator AFUN to vector
%   X. If ATYPE is 'matrix, then AFUN is a matrix and the OP is applied
%   directly. OP is either 'mtimes' or 'mldivide'.
%   ATYPE and AFCNSTR are used in case of error.
%   ITERAPP(OP,AFUN,ATYPE,AFCNSTR,X,P1,P2,...) allows extra arguments to
%   AFUN(X,P1,P2,...) although this usage is now discouraged in favor of
%   using anonymous functions.
%   AFUN(X,P1,P2,...,PN,TFLAG) should accept a TFLAG as its final input if
%   the calling function is BICG, LSQR or QMR. TFLAG is either 'transp' or
%   'notransp' depending on whether A' OP X or A OP X is required.
%   ITERAPP is designed for use by iterative methods like PCG which
%   require matrix operators AFUN representing matrices A to operate on
%   vectors X and return A*X and may also have operators MFUN representing
%   preconditioning matrices M operate on vectors X and return M\X.
%
%   See also BICG, BICGSTAB, CGS, GMRES, LSQR, MINRES, PCG, QMR, SYMMLQ.

%   Copyright 1984-2011 The MathWorks, Inc.
%   $Revision: 1.7.4.8 $ $Date: 2011/05/17 02:33:16 $

if strcmp(atype,'matrix')
    switch lower(op)
        case 'mtimes'
            if (nargin >= 6) && isequal(varargin{end},'transp')
                y = afun' * x;
            else
                y = afun * x;
            end
        case 'mldivide'
            if (nargin >= 6) && isequal(varargin{end},'transp')
                y = afun' \ x;
            else
                y = afun \ x;
            end
        otherwise
            error(message('MATLAB:iterapp:InvalidOp'))
    end
else
    try
        if (nargin >= 6) && isequal(varargin{end},'notransp')
            % A request for A*x coming from BICG, LSQR and QMR
            try
                % New syntax: we now request afun(x,P1,P2,...,PN,'notransp')
                y = afun(x,varargin{:});
            catch
                % Old syntax: we used to accept afun(x,P1,P2,...,PN)
                y = afun(x,varargin{1:end-1});
            end
        else
            % A request for A*x
            % coming from BICGSTAB, CGS, GMRES, MINRES, PCG or SYMMLQ
            % with the call always afun(P1,P2,...,PN)
            % or a request for A'*x coming from
            % BICG, LSQR and QMR in the afun(x,P1,P2,...,PN,'transp') case
            y = afun(x,varargin{:});
        end
    catch ME
        error(message('MATLAB:iterapp:InvalidInput', atype,afcnstr, ME.message));
    end

    if ~iscolumn(y)
        error(message('MATLAB:iterapp:MustReturnColumn', atype, afcnstr));
    end
end

function [atype,afun,afcnstr] = iterchk(A)
%ITERCHK  Checks arguments to iterative methods.
%   [ATYPE,AFUN,AFCNSTR] = ITERCHK(A) returns the following:
%   ATYPE is either 'matrix', 'function', 'expression' or 'inline object'.
%   AFUN is the function name or inline object.
%   AFUN is '' if ATYPE is 'matrix'.
%   AFCNSTR is the function name if ATYPE is 'function'.
%   AFCNSTR is the formula of the function if ATYPE is 'expression' or
%   'inline object'.  AFCNSTR is '' if ATYPE is 'matrix'.
%
%   See also BICG, BICGSTAB, CGS, GMRES, LSQR, MINRES, PCG, QMR, SYMMLQ.

%   Copyright 1984-2004 The MathWorks, Inc.
%   $Revision: 1.8.4.3 $ $Date: 2010/08/23 23:12:57 $

[afun,afunmsg] = fcnchk(A);
if isempty(afunmsg)
   if isa(afun,'inline')
      if isa(A,'inline')
         atype = 'inline object';
      else
         atype = 'expression';
      end
      afcnstr = formula(afun);
   else % both function_handles @fun and function names 'fun'
      atype = 'function';
      if isa(A,'function_handle')
          afcnstr = func2str(A);
      else
          afcnstr = A;
      end
   end
elseif isa(A,'float')
   afun = A;
   atype = 'matrix';
   afcnstr = '';
else
   error(message('MATLAB:iterchk:InvalidInput'));
end


function hc = binHistIndep(mu)
% hc = binHistIndep(mu)
% 	Computes expected histogram under independence assumption
%   P(X)=PROD(P(x_i))
%
% Code from the paper: 'Generating spike-trains with specified
% correlations', Macke et al., submitted to Neural Computation
%
% www.kyb.mpg.de/bethgegroup/code/efficientsampling

% generate all possible binary patterns
n = size(mu,1);
c = 0:2^n-1;
pattern = zeros(n,size(c,2));

for i=n:-1:1
    idx = c>=2^(i-1);
    pattern(i,idx)=1;
    c(idx) = c(idx) - 2^(i-1);
end

pattern = flipud(pattern);

% transform to probabilities
mu = mu/2+.5;

% find relevant probabilities for independent model
pMat = (repmat(mu,1,size(pattern,2)).*pattern) + (repmat(1-mu,1,size(pattern,2)).* (~pattern));

% calculate histogram
hc = prod(pMat);


function [ t, rank ] = ksubset_colex_successor ( k, n, t, rank )

%*****************************************************************************80
%
%% KSUBSET_COLEX_SUCCESSOR computes the K subset colex successor.
%
%  Discussion:
%
%    In the original code, there is a last element with no successor.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer K, the number of elements each K subset must
%    have.  1 <= K <= N.
%
%    Input, integer N, the number of elements in the master set.
%    N must be positive.
%
%    Input/output, integer T(K), describes a K subset.  T(I) is the
%    I-th element.  The elements must be listed in DESCENDING order.
%    On input, T describes a K subset.
%    On output, T describes the next K subset in the ordering.
%    If the input T was the last in the ordering, then the output T
%    will be the first.
%
%    Input/output, integer RANK, the rank.
%    If RANK = -1 on input, then the routine understands that this is
%    the first call, and that the user wishes the routine to supply
%    the first element in the ordering, which has RANK = 0.
%    In general, the input value of RANK is increased by 1 for output,
%    unless the very last element of the ordering was input, in which
%    case the output value of RANK is 0.
%

%
%  Return the first element.
%
  if ( rank == -1 )
    for i = 1 : k
      t(i) = k + 1 - i;
    end
    rank = 0;
    return
  end
%
%  Check.
%
  ierror = ksubset_colex_check ( k, n, t );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'KSUBSET_COLEX_SUCCESSOR - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal.\n' );
    fprintf ( 1, '  IERROR = %d\n', ierror );
    error ( 'KSUBSET_COLEX_SUCCESSOR - Fatal error!' );
  end

  for i = k - 1 : -1 : 1
    if ( t(k+1-i) + 1 < t(k-i) )
      t(k+1-i) = t(k+1-i) + 1;
      rank = rank + 1;
      return
    end
  end

  if ( t(1) < n )
    t(1) = t(1) + 1;
    for i = 1 : k - 1
      t(k+1-i) = i;
    end
    rank = rank + 1;
    return
  end
%
%  The last K subset was input.
%  Return the first one.
%
  for i = 1 : k
    t(i) = k + 1 - i;
  end

  rank = 0;

  return
end


function [dpsi, deps] = nut2000a (date1, date2)

% nutation based on iau 2000a theory

% input

%  date1, date2 = tt julian date
%  (julian date = date1 + date2)

% output

%  dpsi = nutation in longitude in radians

%  deps = nutation in obliquity in radians

% reference

%  Nutation Series Evaluation in NOVAS 3.0
%  USNO Circular No. 181, December 15, 2009

% ported from NOVAS 3.0 Fortran subroutine

% Orbital Mechanics with MATLAB

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

global jplephem_inutate nals napl icpl cls

if (jplephem_inutate == 1)

    % read data files

    nals = csvread('nals.csv');

    napl = csvread('napl.csv');

    icpl = csvread('icpl.csv');

    cls = csvread('cls.csv');

    % transpose matrices

    nals = nals';

    napl = napl';

    icpl = icpl';

    cls = cls';

    % reset flag

    jplephem_inutate = 0;

end

% arc seconds to radians

das2r = 4.848136811095359935899141d-6;

% arc seconds in a full circle

turnas = 1296000.0d0;

% 2 * pi

d2pi = 6.283185307179586476925287d0;

% units of 0.1 microarcsecond to radians

u2r = das2r / 1.0d7;

% reference epoch (j2000)

dj0 = 2451545.0d0;

% days per julian century

djc = 36525.0d0;

% -------------------------
% luni-solar nutation model
% -------------------------

%  ---------------
%  planetary terms
%  ---------------

% interval between fundamental epoch j2000.0 and given date (jc)

t = ((date1 - dj0) + date2) / djc;

% -------------------
% luni-solar nutation
% -------------------

% fundamental (delaunay) arguments from simon et al. (1994)

% mean anomaly of the moon (radians)

el = mod (485868.249036d0 + t * (1717915923.2178d0 ...
    + t * (31.8792d0 + t * (0.051635d0 ...
    + t * (-0.00024470d0)))), turnas) * das2r;

% mean anomaly of the sun (radians)

elp = mod (1287104.79305d0 + t * (129596581.0481d0 ...
    + t * (-0.5532d0 + t * (0.000136d0 ...
    + t * (-0.00001149d0)))), turnas) * das2r;

% mean argument of the latitude of the moon (radians)

f = mod (335779.526232d0 + t * (1739527262.8478d0 ...
    + t * (-12.7512d0 + t * (-0.001037d0 ...
    + t * (0.00000417d0)))), turnas) * das2r;

% mean elongation of the moon from the sun (radians)

d = mod (1072260.70369d0 + t * (1602961601.2090d0 ...
    + t * (-6.3706d0 + t * (0.006593d0 ...
    + t * (-0.00003169d0)))), turnas) * das2r;

% mean longitude of the ascending node of the moon (radians)

om = mod (450160.398036d0 + t * (-6962890.5431d0 ...
    + t * (7.4722d0 + t * (0.007702d0 ...
    + t * (-0.00005939d0)))), turnas) * das2r;

% summation of luni-solar nutation series (in reverse order)

arg = mod ((nals(1, :)) * el  + (nals(2, :)) * elp + (nals(3, :)) * f ...
    + (nals(4, :)) * d  + (nals(5, :)) * om, d2pi);

sarg = sin(arg);

carg = cos(arg);

dp = sum((cls(1,:) + cls(2,:) * t) .* sarg + cls(3,:) .* carg);

de = sum((cls(4,:) + cls(5,:) * t) .* carg + cls(6,:) .* sarg);

% convert from 0.1 microarcsec units to radians

dpsils = dp * u2r;

depsls = de * u2r;

% ------------------
% planetary nutation
% ------------------

% mean anomaly of the moon (radians)

al = mod (2.35555598d0 + 8328.6914269554d0 * t, d2pi);

% mean anomaly of the sun (radians)

alsu = mod (6.24006013d0 + 628.301955d0 * t, d2pi);

% mean argument of the latitude of the moon (radians)

af = mod (1.627905234d0 + 8433.466158131d0 * t, d2pi);

% mean elongation of the moon from the sun (radians)

ad = mod (5.198466741d0 + 7771.3771468121d0 * t, d2pi);

% mean longitude of the ascending node of the moon (radians)

aom = mod (2.18243920d0 - 33.757045d0 * t, d2pi);

% general accumulated precession in longitude (radians)

apa = (0.02438175d0 + 0.00000538691d0 * t) * t;

% planetary longitudes, mercury through neptune (souchay et al. 1999)

alme = mod (4.402608842d0 + 2608.7903141574d0 * t, d2pi);

alve = mod (3.176146697d0 + 1021.3285546211d0 * t, d2pi);

alea = mod (1.753470314d0 +  628.3075849991d0 * t, d2pi);

alma = mod (6.203480913d0 +  334.0612426700d0 * t, d2pi);

alju = mod (0.599546497d0 +   52.9690962641d0 * t, d2pi);

alsa = mod (0.874016757d0 +   21.3299104960d0 * t, d2pi);

alur = mod (5.481293871d0 +    7.4781598567d0 * t, d2pi);

alne = mod (5.321159000d0 +    3.8127774000d0 * t, d2pi);

% summation of planetary nutation series (in reverse order)

arg = mod ((napl(1, :)) * al + (napl(2, :)) * alsu + (napl(3, :)) * af ...
    + (napl(4, :)) * ad + (napl(5, :)) * aom  + (napl(6, :)) * alme ...
    + (napl(7, :)) * alve + (napl(8, :)) * alea + (napl(9, :)) * alma ...
    + (napl(10, :)) * alju + (napl(11, :)) * alsa + (napl(12, :)) * alur ...
    + (napl(13, :)) * alne + (napl(14, :)) * apa, d2pi);
sarg = sin(arg);

carg = cos(arg);

dp = sum((icpl(1,:)) .* sarg + (icpl(2,:)) .* carg);

de = sum((icpl(3,:)) .* sarg + (icpl(4,:)) .* carg);

% convert from 0.1 microarcsec units to radians

dpsipl = dp * u2r;

depspl = de * u2r;

% add planetary and luni-solar components

dpsi = dpsipl + dpsils;

deps = depspl + depsls;

end


%% file example_rMTFL.m
% this file shows the usage of Least_rMTFL.m function
% and study how to detect outlier tasks.
%
%% OBJECTIVE
%  argmin_W ||X(P+Q) - Y||_F^2 + lambda1*||P||_{1,2} + lambda2*||Q^T||_{1,2}
%   s.t. W = P + Q
%
%% Copyright (C) 2012 Jiayu Zhou, and Jieping Ye
%
% You are suggested to first read the Manual.
% For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
% Last modified on April 16, 2012.
%
%% Related papers
%
% [1] Gong, P. and Ye, J. and Zhang, C. Robust Multi-Task Feature Learning,
% Submitted, 2012
%

clear;
clc;
close all;

addpath('../MALSAR/functions/rMTFL/'); % load function
addpath('../MALSAR/utils/'); % load utilities

%rng('default');     % reset random generator. Available from Matlab 2011.

%generate synthetic data.
dimension = 500;
sample_size = 50;
task = 50;
X = cell(task ,1);
Y = cell(task ,1);
for i = 1: task
    X{i} = rand(sample_size, dimension);
    Y{i} = rand(sample_size, 1);
end

opts.init = 0;      % guess start point from data.
opts.tFlag = 1;     % terminate after relative objective value does not changes much.
opts.tol = 10^-6;   % tolerance.
opts.maxIter = 500; % maximum iteration number of optimization.

rho_1 = 90;%   rho1: P
rho_2 = 280; %   rho2: Q

[W funcVal P Q] = Least_rMTFL(X, Y, rho_1, rho_2, opts);

% draw figure
close;
figure();
subplot(3,1,1);
%imshow(1- (abs(S')~=0), 'InitialMagnification', 'fit');
imshow(P'==0, 'InitialMagnification', 'fit')
ylabel('P^T (feature)');
title('Visualization of Robust Multi-Task Feature Learning Model');
subplot(3,1,2);
%imshow(1- (zscore(L')), 'InitialMagnification', 'fit')
imshow(Q'==0, 'InitialMagnification', 'fit')
ylabel('Q^T (outliers)');
subplot(3,1,3);
%imshow(1- (zscore(W')), 'InitialMagnification', 'fit')
imshow(W'==0, 'InitialMagnification', 'fit')
ylabel('W^T');
xlabel('Dimension')
print('-dpdf', '-r600', 'LeastrMTFLExp');


function [assignment, cost] = assignmentoptimal(distMatrix)
%ASSIGNMENTOPTIMAL    Compute optimal assignment by Munkres algorithm
%		ASSIGNMENTOPTIMAL(DISTMATRIX) computes the optimal assignment for the
%		given rectangular distance (or weight) matrix, for example the assignment
%		of tracks (in rows) to observations (in columns). The result is a column
%		vector containing the assigned column number in each row (or 0 if no
%		assignment could be done).
%
%		[ASSIGNMENT, COST] = ASSIGNMENTOPTIMAL(DISTMATRIX) returns the assignment
%		vector and the overall cost.
%
%		The distance matrix may contain infinite values (forbidden
%		assignments). Internally, the infinite values are set to a very large
%		finite number, so that the Munkres algorithm itself works on finite-number
%		matrices. Before returning the assignment, all assignments with infinite
%		distance are deleted (i.e. set to zero).
%
%		A description of Munkres algorithm (also called Hungarian algorithm) can
%		easily be found on the web.
%
%		Written by Markus Buehren, www.Lss.uni-stuttgart.de
%		Last modified 14.12.2004

% save original distMatrix for cost computation
originalDistMatrix    = distMatrix;

% check for negative elements
if any(distMatrix(:) < 0)
	error('All matrix elements have to be non-negative.');
end

% get matrix dimensions
[nOfRows, nOfColumns] = size(distMatrix);

% check for infinite values
finiteIndex   = isfinite(distMatrix);
infiniteIndex = find(~finiteIndex);
if ~isempty(infiniteIndex)
	% set infinite values to large finite value
	maxFiniteValue = max(max(distMatrix(finiteIndex)));
	if maxFiniteValue > 0
		infValue = abs(10 * maxFiniteValue * nOfRows * nOfColumns);
	else
		infValue = 10;
	end
	if isempty(infValue)
		% all elements are infinite
		assignment = zeros(nOfRows, 1);
		cost       = 0;
		return
	end
	distMatrix(infiniteIndex) = infValue;
end

% memory allocation
coveredColumns = zeros(1,nOfColumns);
coveredRows    = zeros(nOfRows,1);
starMatrix     = zeros(nOfRows, nOfColumns);
primeMatrix    = zeros(nOfRows, nOfColumns);

% preliminary steps
if nOfRows <= nOfColumns
	minDim = nOfRows;

	% find the smallest element of each row
	minVector = min(distMatrix,[],2);

	% subtract the smallest element of each row from the row
	distMatrix = distMatrix - repmat(minVector, 1, nOfColumns);

	% Steps 1 and 2
	for row = 1:nOfRows
		for col = find(distMatrix(row,:)==0)
			if ~coveredColumns(col)%~any(starMatrix(:,col))
				starMatrix(row, col) = 1;
				coveredColumns(col)  = 1;
				break
			end
		end
	end

else % nOfRows > nOfColumns
	minDim = nOfColumns;

	% find the smallest element of each column
	minVector = min(distMatrix);

	% subtract the smallest element of each column from the column
	distMatrix = distMatrix - repmat(minVector, nOfRows, 1);

	% Steps 1 and 2
	for col = 1:nOfColumns
		for row = find(distMatrix(:,col)==0)'
			if ~coveredRows(row)
				starMatrix(row, col) = 1;
				coveredColumns(col)  = 1;
				coveredRows(row)     = 1;
				break
			end
		end
	end
	coveredRows(:) = 0; % was used auxiliary above

end

if sum(coveredColumns) == minDim
	% algorithm finished
	assignment = buildassignmentvector__(starMatrix);
else
	% move to step 3
	[assignment, distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows] = step3__(distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows, minDim);
end

% compute cost and remove invalid assignments
[assignment, cost] = computeassignmentcost__(assignment, originalDistMatrix, nOfRows);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function assignment = buildassignmentvector__(starMatrix)

[maxValue, assignment] = max(starMatrix, [], 2);
assignment(maxValue == 0) = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [assignment, cost] = computeassignmentcost__(assignment, distMatrix, nOfRows)

rowIndex   = find(assignment);
costVector = distMatrix(rowIndex + nOfRows * (assignment(rowIndex)-1));
finiteIndex = isfinite(costVector);
cost = sum(costVector(finiteIndex));
assignment(rowIndex(~finiteIndex)) = 0;

% Step 2: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [assignment, distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows] = step2__(distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows, minDim)

% cover every column containing a starred zero
maxValue = max(starMatrix);
coveredColumns(maxValue == 1) = 1;

if sum(coveredColumns) == minDim
	% algorithm finished
	assignment = buildassignmentvector__(starMatrix);
else
	% move to step 3
	[assignment, distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows] = step3__(distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows, minDim);
end

% Step 3: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [assignment, distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows] = step3__(distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows, minDim)

zerosFound = 1;
while zerosFound

	zerosFound = 0;
	for col = find(~coveredColumns)
		for row = find(~coveredRows')
			if distMatrix(row,col) == 0

				primeMatrix(row, col) = 1;
				starCol = find(starMatrix(row,:));
				if isempty(starCol)
					% move to step 4
					[assignment, distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows] = step4__(distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows, row, col, minDim);
					return
				else
					coveredRows(row)        = 1;
					coveredColumns(starCol) = 0;
					zerosFound              = 1;
					break % go on in next column
				end
			end
		end
	end
end

% move to step 5
[assignment, distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows] = step5__(distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows, minDim);

% Step 4: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [assignment, distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows] = step4__(distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows, row, col, minDim)

newStarMatrix          = starMatrix;
newStarMatrix(row,col) = 1;

starCol = col;
starRow = find(starMatrix(:, starCol));

while ~isempty(starRow)

	% unstar the starred zero
	newStarMatrix(starRow, starCol) = 0;

	% find primed zero in row
	primeRow = starRow;
	primeCol = find(primeMatrix(primeRow, :));

	% star the primed zero
	newStarMatrix(primeRow, primeCol) = 1;

	% find starred zero in column
	starCol = primeCol;
	starRow = find(starMatrix(:, starCol));

end
starMatrix = newStarMatrix;

primeMatrix(:) = 0;
coveredRows(:) = 0;

% move to step 2
[assignment, distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows] = step2__(distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows, minDim);

% Step 5: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [assignment, distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows] = step5__(distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows, minDim)

% find smallest uncovered element
uncoveredRowsIndex    = find(~coveredRows');
uncoveredColumnsIndex = find(~coveredColumns);
[s, index1] = min(distMatrix(uncoveredRowsIndex,uncoveredColumnsIndex));
[s, index2] = min(s);
h = distMatrix(uncoveredRowsIndex(index1(index2)), uncoveredColumnsIndex(index2));

% add h to each covered row
index = find(coveredRows);
distMatrix(index, :) = distMatrix(index, :) + h;

% subtract h from each uncovered column
distMatrix(:, uncoveredColumnsIndex) = distMatrix(:, uncoveredColumnsIndex) - h;

% move to step 3
[assignment, distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows] = step3__(distMatrix, starMatrix, primeMatrix, coveredColumns, coveredRows, minDim);


function uvTformDataT = sr_src_domain_tform(uvPlaneID, modelPlane, uvTformAData, srcPos, trgPos)
% SR_SRC_DOMAIN_TFORM
%
% The function estimates the source patch domain transformation based on
% the source and target position, the plane parameters and apply the affine
% transformation
%
% Input:
%     - uvPlaneID:    [numUvPix] x [1], the plane label of uv pixels
%     - modelPlane:   plane model
%     - uvTformAData: [numUvPix] x 4, affine transformation
%     - srcPos:       [numUvPix] x [2], the center position of source patch
%     - trgPos:       [numUvPix] x [2], the center position of target patch
% Output:
%     - uvTformDataT: [numUvPix] X [9]
%
% The function of computing homography transformation induced by plane
% is modified from the following paper:
%
% Jia-Bin Huang, Sing Bing Kang, Narendra Ahuja, and Johannes Kopf,
% Image Completion using Planar Structure Guidance,
% ACM Transactions on Graphics (Proceedings of SIGGRAPH 2014), 33(4), 2014
% =========================================================================

% TO-DO: Make sure that the predicted transformation can point it
% the desired source patch position

% =========================================================================
% Compute homography transformation induced by plane
% =========================================================================

numUvPix = size(srcPos, 1);
uvTformDataH = zeros(numUvPix, 9, 'single');
I = eye(3);

for indPlane = 1: modelPlane.numPlane
    % The rectifying transformation for the plane
    rectMat = modelPlane.rectMat{indPlane};
    h7 = rectMat(3,1);
    h8 = rectMat(3,2);

    % Retrieve the uv pixels that have the current plane label
    uvPlaneIndCur = uvPlaneID == indPlane;
    numPlanePixCur = sum(uvPlaneIndCur);

    if(numPlanePixCur)
        % Target patch center position in the rectified domain
        trgPosCur = trgPos(uvPlaneIndCur, :) - 1;
        trgPosCurR = sr_apply_tform_H(trgPosCur, h7, h8);

        % Source patch center position in the rectified domain
        srcPosCur = srcPos(uvPlaneIndCur, :) - 1;
        srcPosCurR = sr_apply_tform_H(srcPosCur, h7, h8);

        % Displacement vector from target to source position
        dRect = srcPosCurR - trgPosCurR;

        % Compute the transformation that maps from target to source
        % (See Eqn 8 in the paper [Huang et al. TOG 2014])
        uvTformCur = zeros(numPlanePixCur, 9, 'single');
        uvTformCur(:,[1,4,7]) = bsxfun(@times, dRect(:,1), [h7, h8, 1]);
        uvTformCur(:,[2,5,8]) = bsxfun(@times, dRect(:,2), [h7, h8, 1]);

        dTemp = dRect*[h7;h8]; % dTemp = dx*h7 + dy*h8
        uvTformCur(:,[3,6,9]) = bsxfun(@times, dTemp, -[h7, h8, 1]);
        uvTformCur = bsxfun(@plus, uvTformCur, I(:)');

        % Apply the offset to cancel out the dependency of the target position
        % (See Eqn 9 in the paper [Huang et al. TOG 2014])
        uvTformDataH(uvPlaneIndCur, :)   = sr_trans_tform(uvTformCur, trgPosCur);
        uvTformDataH(uvPlaneIndCur, 7:8) = uvTformDataH(uvPlaneIndCur, 7:8) + 1;
    end
end

% =========================================================================
% Apply the similarity transformation
% =========================================================================
% The transformation T first map from original reference points to affine
% transformation coordinate using A, and then map to the desired target
% position using H
% -> T = H * A
uvTformDataT = uvTformDataH;
uvTformDataT(:,1:3) = bsxfun(@times, uvTformDataH(:,1:3), uvTformAData(:,1)) + ...
    bsxfun(@times, uvTformDataH(:,4:6), uvTformAData(:,2));
uvTformDataT(:,4:6) = bsxfun(@times, uvTformDataH(:,4:6), uvTformAData(:,3)) + ...
    bsxfun(@times, uvTformDataH(:,4:6), uvTformAData(:,4));

uvTformDataT = bsxfun(@rdivide, uvTformDataT, uvTformDataT(:,9));

end

function x = sr_apply_tform_H(x, h7, h8)

% Apply homography H with third row [h7, h8, 1] to 2D points x

y = x(:,1)*h7 + x(:,2)*h8 + 1;
x = bsxfun(@rdivide, x(:,1:2), y + eps);

% numPix = size(x, 1);
% x = cat(2, x, ones(numPix, 1, 'single'));
% x = x*H';
% x = bsxfun(@rdivide, x(:,1:2), x(:,3) + eps);

end


function [irf_record]=irf(beta_gibbs,It,Bu,IRFperiods,n,m,p,k)

% function [irf_record]=irf(beta_gibbs,It,Bu,IRFperiods,n,m,p,k)
% runs the gibbs sampler to obtain draws from the posterior distribution of IRFs
% inputs:  - matrix 'beta_gibbs': record of the gibbs sampler draws for the beta vector
%          - integer 'It': total number of iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - integer 'Bu': number of burn-in iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - integer 'IRFperiods': number of periods for IRFs
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'm': number of exogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'p': number of lags included in the model (defined p 7 of technical guide)
%          - integer 'k': number of coefficients to estimate for each equation in the BVAR model (defined p 7 of technical guide)
% outputs: -  cell 'irf_record': record of the gibbs sampler draws for the IRFs

% this function implements algorithm 2.2.1

% create the cell aray that will store the values from the simulations
irf_record=cell(n,n);

Bgibbs=reshape(beta_gibbs,k,n,It-Bu);

% deal with shocks in turn
for ii=1:n

   % step 1: repeat the simulation process a number of times equal to the number of Gibbs iterations
   for kk=1:It-Bu

   % step 3: draw beta from its posterior distribution
   B=squeeze(Bgibbs(:,:,kk));
   % create a matrix of zeros of dimension p*n
   Y=zeros(p,n);
   %  step 2: set the value of the last row, column i, equal to 1
   Y(p,ii)=1;

% if prior~=61
      % step 4: for each iteration kk, repeat the algorithm for periods T+1 to T+h
      for jj=1:IRFperiods-1
      % use the function lagx to obtain a matrix temp, containing the endogenous regressors
      temp=bear.lagx(Y,p-1);
      % define the vector X
      X=[temp(end,:) zeros(1,m)];
      % obtain the predicted value for T+jj
      yp=X*B;
      % concatenate yp at the top of Y
      Y=[Y;yp];
      % repeat until values are obtained for T+h
      end
% elseif prior==61 %for prior=61 mean adjusted model
% 	  % for each iteration kk, repeat the algorithm for periods T+1 to T+h
%       for jj=1:IRFperiods
%       % use the function lagx to obtain the matrix X
%       X=bear.lagx(Y,p-1);
%       X=X(end,:);
%       % obtain predicted value for T+jj
%       yp=X*B;
%       % concatenate yp at the top of Y
%       Y=[Y;yp];
%       % repeat until values are obtained for T+h
%       end
% end

   % step 5: record the results from current iteration in cell irf_record
      % loop over variables
      for jj=1:n
      % consider column jj of matrix 'Y' and trim the (p-1) initial periods: what remains is the series of IRFs for period T to period T+h-1, for variable jj
      temp=Y(p:end,jj);
      % record these values in the corresponding matrix of irf_record
      irf_record{jj,ii}(kk,:)=temp';
      end

   % then go for next iteration
   end

% conduct the same process with shocks in other variables
end


function f = fracInt(f, mu)
%FRACINT  Fractional integral of a CHEBFUN.
%   FRACINT(F, MU) gives the order MU fractional integral of a CHEBFUN object F.
%
%   Currently this only supports the situation where F is smooth (i.e., it has
%   no breakpoints or endpoint singularities) and on a finite domain.

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

% No piecewise support yet:
if ( numel(domain(f)) > 2 )
    error('CHEBFUN:CHEBFUN:fracInt:breakpoints', ...
        'FRACINT does not currently support piecewise functions.');
end

% Extract the fractional part of mu:
mu_int = floor(mu);
mu_frac = mu - mu_int;

% Computer the integer CUMSUMs:
if ( mu_int > 0 )
    % Ensure mu is in [0, 1):
    f = cumsum(f, mu_int);
    f = fracInt(f, mu_frac);
    return
elseif ( mu_frac == 0 )
    % Nothing more to do (non-fractional integral).
    return
end

% Quasimatrix / Array-valued support:
% TODO: This should be overhauled once SINGFUN supports array-valuedness.
m = numColumns(f);
if ( m > 1 )
    % Convert to cell-array of scalar CHEBFUNs.
    f = mat2cell(f);
    % Loop over columns:
    for k = 1:m
        f{k} = fracInt(f{k}, mu_frac);
    end
    % Convert from cell array to quasimatrix.
    f = cat(2-f{1}.isTransposed, f{:});
    return
end

% From here on f is scalar-valued:

% Call BNDFUN/FRACINT():
f.funs{1} = fracInt(f.funs{1}, mu_frac);

% Update the pointValues:
f.pointValues = chebfun.getValuesAtBreakpoints(f.funs);

end


% Semantic Soft Segmentation
% This function implements the soft segmentation approach described in
% Yagiz Aksoy, Tae-Hyun Oh, Sylvain Paris, Marc Pollefeys, Wojciech Matusik
% "Semantic Soft Segmentation", ACM TOG (Proc. SIGGRAPH) 2018

function [softSegments, initSoftSegments, Laplacian, affinities, features, superpixels, eigenvectors, eigenvalues] = SemanticSoftSegmentation(image, features)

    disp('Semantic Soft Segmentation')
    % Prepare the inputs and superpixels
    image = im2double(image);
    if size(features, 3) > 3 % If the features are raw, hyperdimensional, preprocess them
        features = preprocessFeatures(features, image);
    else
        features = im2double(features);
    end
    superpixels = Superpixels(image);
    [h, w, ~] = size(image);

    disp('     Computing affinities')
    % Compute the affinities and the Laplacian
    affinities{1} = mattingAffinity(image);
    affinities{2} = superpixels.neighborAffinities(features); % semantic affinity
    affinities{3} = superpixels.nearbyAffinities(image); % non-local color affinity
    Laplacian = affinityMatrixToLaplacian(affinities{1} + 0.01 * affinities{2} + 0.01 * affinities{3}); % Equation 6

    disp('     Computing eigenvectors')
    % Compute the eigendecomposition
    eigCnt = 100; % We use 100 eigenvectors in the optimization
    [eigenvectors, eigenvalues] = eigs(Laplacian, eigCnt, 'SM');

    disp('     Initial optimization')
    % Compute initial soft segments
    initialSegmCnt = 40;
    sparsityParam = 0.8;
    iterCnt = 40;
    % feeding features to the function below triggers semantic intialization
    initSoftSegments = softSegmentsFromEigs(eigenvectors, eigenvalues, Laplacian, ...
                                            h, w, features, initialSegmCnt, iterCnt, sparsityParam, [], []);

    % Group segments w.r.t. their mean semantic feature vectors
    groupedSegments = groupSegments(initSoftSegments, features);

    disp('     Final optimization')
    % Do the final sparsification
    softSegments = sparsifySegments(groupedSegments, Laplacian, imageGradient(image, false, 6));

    disp('     Done.')
end


%DEMO_SURVIVAL_COXPH  Survival model using Cox proportional model
%
%  Description
%    Survival model using Cox proportional model with a piecewise
%    log-constant baseline hazard. The hazard rate is
%
%       h(t) = h_0(t)*exp(f),
%
%    where the baseline hazard is assumed to piecewise log-constant.
%
%    The inference is conducted via Laplace, where we find
%    Gaussian approximation for p(f| th, data), where th is the
%    maximum a posterior (MAP) estimate for the parameters.
%
%    The censoring indicator ye is
%
%      ye = 0 for uncensored event
%      ye = 1 for right censored event.
%
%    If survival times y for n observation are given as nx2 matrix with
%    entry times into follow-up in the first column and exit times from
%    follow-up in the second column, left truncated right censored
%    modelling is possible, for instance, in cases where age is wanted to
%    be set as a baseline hazard.
%
%    Example data set is leukemia survival data in Northwest England
%    presented in (Henderson, R., Shimakura, S., and Gorst, D. (2002).
%    Modeling spatial variation in leukemia survival data. Journal of the
%    American Statistical Association, 97:965–972). Data set was downloaded
%    from http://www.math.ntnu.no/%7Ehrue/r-inla.org/examples/leukemia/leuk.dat
%
%  See also  DEMO_SURVIVAL_WEIBULL

% Copyright (c) 2011 Jaakko Riihimäki

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% First load data
S = which('demo_survival_weibull');
L = strrep(S,'demo_survival_weibull.m','demodata/leukemia.txt');
leukemiadata=load(L);

% leukemiadata consists of:
% 'time', 'cens', 'xcoord', 'ycoord', 'age', 'sex', 'wbc', 'tpi', 'district'

% survival times
y=leukemiadata(:,1);
% scale survival times
y=y/max(y);

ye=1-leukemiadata(:,2); % event indicator, ye = 0 for uncensored event
                  %                        ye = 1 for right censored event

% choose (for example) 'age', 'sex', 'wbc', and 'tpi' covariates
x0=leukemiadata(:,5:8);
x=x0;
% normalize continuous covariates
x(:,[1 3:4])=bsxfun(@rdivide,bsxfun(@minus,x0(:,[1 3:4]),mean(x0(:,[1 3:4]),1)),std(x0(:,[1 3:4]),1));

[n, nin]=size(x);

% number of time intervals
ntime=50;
% create finite partition of time axis
S=linspace(0,max(y)+0.001,ntime+1);

% Create the covariance functions
pl = prior_t('s2',1, 'nu', 4);
pm = prior_t('s2',1, 'nu', 4);

% covariance for hazard function
gpcfh = gpcf_sexp('lengthScale', 1, 'magnSigma2', 1.1, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
% covariance for proportional part
gpcf = gpcf_sexp('lengthScale', ones(1,size(x,2)), 'magnSigma2', 1.2, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);

% Create the likelihood structure
lik = lik_coxph('S', S);

% NOTE! if multiple covariance functions per latent is used, define
% gp.comp_cf as follows:
% gp = gp_set(..., 'comp_cf' {[1 2] [5 6]};
% where [1 2] are for hazard function, and [5 6] for proportional part
gp = gp_set('lik', lik, 'cf', {gpcfh gpcf}, 'jitterSigma2', 1e-6, 'comp_cf', {1 2});

% Set the approximate inference method to Laplace
gp = gp_set(gp, 'latent_method', 'Laplace');

opt=optimset('TolFun',1e-2,'TolX',1e-2,'Display','iter');
gp=gp_optim(gp,x,y,'z',ye,'opt',opt);

% Make prediction
xt1=zeros(200,nin); xt1(:,2)=1;
xt2=zeros(200,nin); xt2(:,2)=-1;

xt1(:,1)=linspace(min(x(:,1)), max(x(:,1)), 200);
xt2(:,1)=linspace(min(x(:,1)), max(x(:,1)), 200);
xt01(:,1)=linspace(min(x0(:,1)), max(x0(:,1)), 200);
xt02(:,1)=linspace(min(x0(:,1)), max(x0(:,1)), 200);

[Ef1, Covf1] = gp_pred(gp, x, y, xt1, 'z', ye);
[Ef2, Covf2] = gp_pred(gp, x, y, xt2, 'z', ye);
Varf1 = diag(Covf1);
Varf2 = diag(Covf2);

figure, hold on, set(gcf, 'color', 'w'),
plot(gp.lik.xtime, Ef1(1:ntime),'k', 'linewidth', 3)
plot(gp.lik.xtime, Ef1(1:ntime)+1.96*sqrt(Varf1(1:ntime)),'--k', 'linewidth', 2)
plot(gp.lik.xtime, Ef1(1:ntime)-1.96*sqrt(Varf1(1:ntime)),'--k', 'linewidth', 2)
title('log-baseline hazard (follow-up time)')
xlabel('time')

% Compute posterior mean and 95% credible intervals of latent function as a
% function of age and sex
col1=ones(1,3)*0.7;
col2=ones(1,3)*0.3;
figure, hold on, set(gcf, 'color', 'w'),
plot(xt01(:,1), Ef1(ntime+1:end), 'color', col1, 'linewidth', 3)
plot(xt01(:,1), Ef1(ntime+1:end)+1.96*sqrt(Varf1(ntime+1:end)), '--', 'color', col1, 'linewidth', 2)
plot(xt01(:,1), Ef1(ntime+1:end)-1.96*sqrt(Varf1(ntime+1:end)), '--', 'color', col1, 'linewidth', 2)

plot(xt02(:,1), Ef2(ntime+1:end), 'color', col2, 'linewidth', 3)
plot(xt02(:,1), Ef2(ntime+1:end)+1.96*sqrt(Varf2(ntime+1:end)), '--', 'color', col2, 'linewidth', 2)
plot(xt02(:,1), Ef2(ntime+1:end)-1.96*sqrt(Varf2(ntime+1:end)), '--', 'color', col2, 'linewidth', 2)
xlabel('age')
title('effect of age for both sexes')

%- Age as baseline hazard

% Age in the beginning and in the end of follow-up
y=[x0(:,1) x0(:,1)+leukemiadata(:,1)/365];
y2=y;
% normalise ages
y2=y2-min(y2(:)); y2=y2./max(y(:));

x2=x;
x2(:,1)=[];

% covariance for proportional part
gpcf = gpcf_sexp('lengthScale', ones(1,size(x2,2)), 'magnSigma2', .5, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);

% NOTE! if multiple covariance functions per latent is used, define
% gp.comp_cf as follows:
% gp = gp_set(..., 'comp_cf', {[1 2] [5 6]});
% where [1 2] are for hazard function, and [5 6] for proportional part
gp = gp_set('lik', lik, 'cf', {gpcfh gpcf}, 'jitterSigma2', 1e-6, 'comp_cf', {[1] [2]});

% Set the approximate inference method to Laplace
gp = gp_set(gp, 'latent_method', 'Laplace');

opt=optimset('TolFun',1e-2,'TolX',1e-2,'Display','iter');
gp=gp_optim(gp,x2,y2,'z',ye,'opt',opt);

[Ef1, Covf1] = gp_pred(gp, x2, y2, x2, 'z', ye);
Varf1 = diag(Covf1);
xtmpl=linspace(min(y(:)),max(y(:)),50);

figure, hold on, set(gcf, 'color', 'w'),
plot(xtmpl, Ef1(1:ntime),'k', 'linewidth', 3)
plot(xtmpl, Ef1(1:ntime)+1.96*sqrt(Varf1(1:ntime)),'--k', 'linewidth', 2)
plot(xtmpl, Ef1(1:ntime)-1.96*sqrt(Varf1(1:ntime)),'--k', 'linewidth', 2)
title('log-baseline hazard (age)')
xlabel('age')


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

% Predictions or posteriors?
pop = 1; % Default: predictions
if r.c_obs.predorpost == 2
    pop = 3; % Alternative: posteriors
end

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

% Assumed structure of infStates:
% dim 1: time (ie, input sequence number)
% dim 2: HGF level
% dim 3: choice number
% dim 4: 1: muhat, 2: sahat, 3: mu, 4: sa

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

% Belief trajectories at 1st level
states = squeeze(infStates(:,1,:,pop));

% Log-volatility trajectory
mu3 = squeeze(infStates(:,3,1,3));

% Responses
y = r.y(:,1);

% Weed irregular trials out from inferred states and responses
states(r.irr,:) = [];
mu3(r.irr) = [];
y(r.irr) = [];

% Inverse decision temperature
be = exp(-mu3);
be = repmat(be,1,nc);

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

% Softmax probabilities
prob = exp(be.*states)./Z;

% Extract probabilities of chosen options
probc = prob(sub2ind(size(prob), 1:length(y), y'));

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

end


function [err,elemErr] = getL2errorRT0(node,elem,exactSigma,sigmah,d)
%% GETL2ERRORRT0 L2 norm of RT0 element.
%
%  The input exactSigma can be a function bundle or vector array.
%  When exactSigma is a vector array, it should be size NT*2, and
%  exactSigma = \nabla u (or exactSigma= K\nabla u), vector array of size
%  NT by 2.
%
%  [err,elemErr] = getL2errorRT0(node,elem,exactSigma,sigmah).
%  err gives the L2 norm between exact flux and approximate one from RT0,
%  elemErr, an NT by 1 array, gives the square of L2 norm between exact
%  flux and approximate one from RT0 on each element.
%
%  [err,elemErr] = getL2errorRT0(node,elem,exactSigma,sigmah,d).
%  If the coefficient 'd' is inlcuded, then the input should be in the way
%  sigmah     = K\nabla u_h  (including K),
%  exactSigma =  \nabla u    (function bundle, don't including K),    or
%  exactSigma = K\nabla u    (vector array, including K).
%  err=||K exactSigma - sigma_h||_{K^(-1)} = |||u-u_h||| is the energy norm.
%  elemErr, an NT by 1 array, gives the square of L2 norm between exact
%  flux and approximate one from RT0 on each element
%
% Example
%
%     maxIt = 5;
%     node = [1,0; 1,1; 0,1; -1,1; -1,0; -1,-1; 0,-1; 0,0]; % nodes
%     elem = [1,2,8; 3,8,2; 8,3,5; 4,5,3; 7,8,6; 5,6,8];    % elements
%     bdFlag = setboundary(node,elem,'Dirichlet');
%     pde = mixBCdata;
%     err = zeros(maxIt,1);
%     h = zeros(maxIt,1);
%     for k = 1:maxIt
%         [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%         [u,sigma] = PoissonRT0(node,elem,bdFlag,pde);
%         err(k) = getL2errorRT0(node,elem,pde.Du,sigma);
%         h(k) = 1./(sqrt(size(node,1))-1);
%     end
%     r1 = showrateh(h,err,2);
%     legend('||\sigma - \sigma_h||',['h^{' num2str(r1) '}'],...
%            'LOCATION','Best');
%
% See also getHdiverrorRT0, getL2error3RT0.
%
% Created by Ming Wang at Jan 17, 2011, M-lint modified at May 15, 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if exist('d','var') && ~isempty(d)
    if isreal(d)
        K = d;                  % d is an array
    else
    center = (node(elem(:,1),:) + node(elem(:,2),:) + ...
              node(elem(:,3),:))/3;
    K = d(center);              % d is a function
    end
else
    K = [];
end

%% Construct Data Structure
[elem2dof,~,elem2edgeSign] = dofedge(elem);
NT = size(elem,1);% Ndof = max(elem2dof(:)); %N = size(node,1);
[Dlambda,area] = gradbasis(node,elem);
locEdge = [2 3; 3 1; 1 2];

%% Compute square of the L2 error element-wise
[lambda,w] = quadpts(3); % quadrature order is 3
nQuad = size(lambda,1);
err = zeros(NT,1);
rotMat = [0 -1; 1 0]; % rotation matrix for computing rotLambda.
for p = 1:nQuad
    pxy = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:);
    if ~isnumeric(exactSigma)
        sigmap = exactSigma(pxy);
        if(nargin >=5)&& ~isempty(K) % multiply coeff. if fun bundle flux.
            sigmap = repmat(K,1,2).*sigmap;
        end
    else
        sigmap = exactSigma;         %  K*\nabla u_h.
    end
    sigmahp = zeros(NT,2);
    for k = 1:3 % for each basis
        i = locEdge(k,1); j = locEdge(k,2);
        % phi_k = lambda_iRot_j - lambda_jRot_i;
        sigmahp = sigmahp + repmat(elem2edgeSign(:,k).*sigmah(elem2dof(:,k)),1,2).*...
                   (lambda(p,i)*Dlambda(:,:,j)*rotMat-lambda(p,j)*Dlambda(:,:,i)*rotMat);
    end
    err = err + w(p)*sum((sigmap - sigmahp).^2,2);
end
err = err.*area;               % ||sigma - K\nabla u_h||^2
if(nargin >=5)&& ~isempty(K)   % ||sigma - K\nabla u_h||^2_{K^(-1)}
    err = err./K;
end
elemErr = err;           % ||sigma - K\nabla u_h||^2_{K^(-1)}
% modify the error
err(isnan(err)) = 0;
err = sqrt(sum(err));


%FREFINE refine estimate of fundamental matrix
%
% fr = frefine(F, uv1, uv2)
%
%  Return a refined estimate of fundamental matrix using non-linear
% optimization and enforcing the rank-2 constraint.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.

function fr = frefine(F, uv1, uv2)

	A = [];
	for i=1:numrows(uv1),
		a = [	uv1(i,1)*uv2(i,1)
			uv1(i,2)*uv2(i,1)
			uv2(i,1)
			uv1(i,1)*uv2(i,2)
			uv1(i,2)*uv2(i,2)
			uv2(i,2)
			uv1(i,1)
			uv1(i,2)
			1
		];
		A = [A; a'];
	end
	f = F';
	f = f(:);
	fprintf('Initial residual is %g\n', norm(A * f));
	fprintf('Initial determinant is %g\n', det(F));

	options = optimset('MaxFunEvals', 10000, ...
		'LevenbergMarquardt', 'on', ...
		'TolFun', 1e-16, ...
		'TolCon', 1e-16, ...
		'LargeScale', 'off' ...
		);
	fr = fmincon(@fun, f, [], [], [], [], [], [], ...
		@nlfun, options, A);
	fprintf('Final residual is %g\n', norm(A * fr));
	fr = reshape(fr, 3, 3)';
	det(fr)
	fprintf('Final determinant is %g\n', det(fr));

function e = fun(x, A)
	e = norm(A * x);

function [c,ceq] = nlfun(x, A)

	ceq = abs( det(reshape(x, 3, 3)) );
	c = [];


function fem2d_bvp_serene_test03 ( )

%*****************************************************************************80
%
%% FEM2D_BVP_SERENE_TEST03 carries out test case #3.
%
%  Discussion:
%
%    Use A3, C3, F3, EXACT3, EXACT_UX3, EXACT_UY3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 June 2014
%
%  Author:
%
%    John Burkardt
%
  nx = 5;
  ny = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FEM2D_BVP_SERENE_TEST03\n' );
  fprintf ( 1, '  Solve - del ( A del U ) + C U = F \n' );
  fprintf ( 1, '  on the unit square with zero boundary conditions.\n' );
  fprintf ( 1, '  A1(X,Y) = 0.0\n' );
  fprintf ( 1, '  C1(X,Y) = 1.0\n' );
  fprintf ( 1, '  F1(X,Y) = X * ( 1 - X ) * Y * ( 1 - Y ).\n' );
  fprintf ( 1, '  U1(X,Y) = X * ( 1 - X ) * Y * ( 1 - Y )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  This example is contrived so that the system matrix\n' );
  fprintf ( 1, '  is the WATHEN matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The grid uses %d by %d nodes.\n', nx, ny );
  node_num = fem2d_bvp_serene_node_num ( nx, ny );
  fprintf ( 1, '  The number of nodes is %d\n', node_num );
%
%  Geometry definitions.
%
  x = linspace ( 0.0, 1.0, nx );
  y = linspace ( 0.0, 1.0, ny );

  show11 = 1;

  u = fem2d_bvp_serene ( nx, ny, @a3, @c3, @f3, x, y, show11 );

  if ( nx * ny <= 25 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '     I     J    X         Y         U         Uexact    Error\n' );
    fprintf ( 1, '\n' );

    k = 0;

    for j = 1 : ny

      if ( mod ( j, 2 ) == 1 )
        inc = 1;
      else
        inc = 2;
      end

      for i = 1 : inc : nx
        k = k + 1;
        uexact = exact3 ( x(i), y(j) );
        fprintf ( 1, '  %4d  %4d  %8f  %8f  %8f  %8f  %8e\n', ...
          i, j, x(i), y(j), u(k), uexact, abs ( u(k) - uexact ) );
      end
    end

  end

  e1 = fem2d_l1_error_serene ( nx, ny, x, y, u, @exact3 );
  e2 = fem2d_l2_error_serene ( nx, ny, x, y, u, @exact3 );
  h1s = fem2d_h1s_error_serene ( nx, ny, x, y, u, @exact_ux3, @exact_uy3 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  l1 error   = %g\n', e1 );
  fprintf ( 1, '  L2 error   = %g\n', e2 );
  fprintf ( 1, '  H1S error  = %g\n', h1s );
%
%  Pull out the Wathen matrix from MATLAB.
%  It will have been multiplied by a random scale factor.
%  While my numbering scheme is
%    3  2  1
%    4     8
%    5  6  7
%  the numbering scheme used here is
%    1  2  3
%    4     5
%    6  7  8
%
%
  A = gallery ( 'wathen', 1, 1 );
  A = full ( A );
  s = 0.5 * A(1,3);
  A = A / s;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  WATHEN Matrix from "gallery(''wathen'',1,1)"\n' );
  fprintf ( 1, '\n' );
  i = [3,2,1,4,6,7,8,5];
  A(i,i)

  return
end
function value = a3 ( x, y )

%*****************************************************************************80
%
%% A3 evaluates A function #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of A(X).
%
  value = 0.0;

  return
end
function value = c3 ( x, y )

%*****************************************************************************80
%
%% C3 evaluates C function #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of C(X).
%
  value = 1.0;

  return
end
function value = exact3 ( x, y )

%*****************************************************************************80
%
%% EXACT3 evaluates exact solution #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of the solution.
%
  value = x .* ( 1.0 - x ) .* y .* ( 1.0 - y );

  return
end
function value = exact_ux3 ( x, y )

%*****************************************************************************80
%
%% EXACT_UX3 evaluates the derivative dUdX of exact solution #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of dUdX.
%
  value = ( 1.0 - 2.0 * x ) .* ( y - y .* y );

  return
end
function value = exact_uy3 ( x, y )

%*****************************************************************************80
%
%% EXACT_UY3 evaluates the derivative dUdY of exact solution #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of dUdX.
%
  value = ( x - x .* x ) .* ( 1.0 - 2.0 * y );

  return
end
function value = f3 ( x, y )

%*****************************************************************************80
%
%% F3 evaluates right hand side function #3.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, Y, the evaluation point.
%
%    Output, real VALUE, the value of the right hand side.
%
  value = x .* ( 1.0 - x ) .* y .* ( 1.0 - y );

  return
end


function [t1,pd] = relaxFitT1_lsq(data,flipAngles,tr,b1Map)
%
% [t1,pd] = relaxFitT1(data,flipAngles,tr,b1Map)
%
% Computes a linear fit of the the T1 estimate for all voxels. The data can
% be passed as either a 4d array of X x Y x Z x nT1Measurements or an array of
% size nVoxels x nT1Measurements. THe b1Map can be either be a scalar (to
% correct for a constant bias across the whole image), or an image the same
% size as one of the t1 measurments (ie. either X x Y x Z or nVoxels x 1).
%
% We allow multiple b1 maps to correct for different sets of measurements.
% If there is more than 1 b1 map, then each row of the flip angle array
% will be corrected by the corresponding b1 map. E.g., flipAngles(1,:) will
% be corrected by b1Map(:,:,:,1), flipAngles(2,:) will be corrected by
% b1Map(:,:,:,2), etc. The flip angle corresponding to each t1 should be
% specified using the linear index. E.g., flipAngle(1) for t1(:,:,:,1),
% flipAngle(2) for t1(:,:,:,2), etc.
%
% Returns:
%   T1: T1 estimate (seconds)
%   PD: a map that includes spin-density (M0), scanner scaling constant
%       (G), and T2*: PD = M0 * G * exp(-TE / T2*).
%
% SEE ALSO:
%
% relaxMtFit.m to fit the f and k maps to the output of this function.
%
% HISTORY:
% 2008.02.26 RFD: wrote it.

if(~exist('b1Map','var')||isempty(b1Map))
  b1Map = 1;
end

theta = flipAngles*pi/180;
szT1 = size(data);
szB1 = size(b1Map);
if(numel(szT1)>2)
   nVox = prod(szT1(1:3));
   nT1 = szT1(4);
   data = reshape(data,nVox,nT1);
   if(numel(b1Map)>1)
       nB1 = szB1(4);
       b1Map = reshape(b1Map,nVox,nB1);
   else
       nB1 = 1;
   end
else
   nVox = size(data,1);
   nT1 = size(data,2);
   nB1 = size(b1Map,2);
end
if(nB1>1)
    if(size(theta,1)~=nB1)
        error('The number of B1 maps must match the number of rows in the flip angle array.');
    end
    b1Inds = repmat([1:nB1]',1,size(theta,2));
else
    b1Inds = repmat([1],1,numel(theta));
end
theta = theta(:)';
b1Inds = b1Inds(:)';

% The code below (esp ndfun) doesn't work when we have Inf or NaN, so:
b1Map(~isfinite(b1Map)) = 1;
data(~isfinite(data)) = 0;

%% NON-LINEAR T1 FIT
%

% Fit a line to the data in each voxel to estimate T1.
% We'll use an eigenvector formulation since we already have a
% vectorized eigenvector decompostion coded up.

% Build a matrix M where M = [x1-x0 y1-y0; x2-x0 y2-y0; ... xn-x0 yn-y0].
% To make it work with ndfun, we need to reshape things a bit.
M = zeros(nT1,2,nVox);
for(ii=1:nT1)
  correctedFlip = theta(ii).*b1Map(:,b1Inds(ii));
  M(ii,1,:) = abs(data(:,ii)./tan(correctedFlip));
  M(ii,2,:) = abs(data(:,ii)./sin(correctedFlip));
end
M0 = mean(M,1);
for(ii=1:size(M,1))
  M(ii,:,:) = M(ii,:,:) - M0;
end
% The best-fitting line is the eigenvector corresponding to
% the largest eigenvalue of eig(M'*M):
[vec,val] = ndfun('eig',ndfun('mult',permute(M,[2 1 3]),M));
% The slope (m) of the line is simply the ratio of y to x and the
% intercept (b) is y-m*x
% Note that the eigenvalues from ndfun are sorted in descending
% order, opposite from Matlab's 'eig'.
m = vec(2,1,:)./vec(1,1,:);
%b = M0(:,2,:)-m.*M0(:,1,:);

% *** CHECK THIS
%m(m<=0) = NaN;
m = abs(squeeze(m));
m(m<0.5) = 0.5;

t1 = (-tr/1000)./log(m);
% Clip to plausible values
t1(isnan(t1)|t1<0.1) = 0.1;
t1(t1>5) = 5;

%pd = b./(1-m);
%pd(pd>5e5) = 5e5;

pd = zeros(nVox,nT1);
% We could use m here, but we've already clipped t1 to plausible values, so recomputing
% the slope here provides a sane range of pd values.
t1tr = exp(-tr/1000./t1);
for(ii=1:nT1)
    correctedFlip = theta(ii).*b1Map(:,b1Inds(ii));
    pd(:,ii) = data(:,ii)./sin(correctedFlip).*((1-cos(correctedFlip).*t1tr)./(1-t1tr));
end
% Could do least squares for better PDmap?
pd = mean(pd,2);

if(numel(szT1)>ndims(data))
   t1 = reshape(t1,szT1(1:3));
   pd = reshape(pd,szT1(1:3));
end

return


function indx = r82vec_sort_heap_index_a ( n, a )

%*****************************************************************************80
%
%% R82VEC_SORT_HEAP_INDEX_A does an indexed heap ascending sort of an R82VEC.
%
%  Discussion:
%
%    The sorting is not actually carried out.  Rather an index array is
%    created which defines the sorting.  This array may be used to sort
%    or index the array, or to sort or index related arrays keyed on the
%    original array.
%
%    Once the index array is computed, the sorting can be carried out
%    "implicitly:
%
%      A(1:2,INDX(I)), I = 1 to N is sorted,
%
%    or explicitly, by the call
%
%      A = R82VEC_PERMUTE ( N, A, INDX )
%
%    after which A(1:2,I), I = 1 to N is sorted.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real A(2,N), an array to be index-sorted.
%
%    Output, integer INDX(N), the sort index.  The
%    I-th element of the sorted array is A(1:2,INDX(I)).
%
  if ( n < 1 )
    return
  end

  if ( n == 1 )
    indx(1) = 1;
    return
  end

  indx = i4vec_indicator ( n );

  l = floor ( n / 2 ) + 1;
  ir = n;

  while ( 1 )

    if ( 1 < l )

      l = l - 1;
      indxt = indx(l);
      aval(1:2) = a(1:2,indxt);

    else

      indxt = indx(ir);
      aval(1:2) = a(1:2,indxt);
      indx(ir) = indx(1);
      ir = ir - 1;

      if ( ir == 1 )
        indx(1) = indxt;
        break
      end

    end

    i = l;
    j = l + l;

    while ( j <= ir )

      if ( j < ir )
        if (   a(1,indx(j)) <  a(1,indx(j+1)) | ...
             ( a(1,indx(j)) == a(1,indx(j+1)) & ...
               a(2,indx(j)) <  a(2,indx(j+1)) ) )
          j = j + 1;
        end
      end

      if (   aval(1) <  a(1,indx(j)) | ...
           ( aval(1) == a(1,indx(j)) & ...
             aval(2) <  a(2,indx(j)) ) )
        indx(i) = indx(j);
        i = j;
        j = j + j;
      else
        j = ir + 1;
      end

    end

    indx(i) = indxt;

  end

  return
end


%
% Author: Marius Drulea
% http://www.cv.utcluj.ro/optical-flow.html
%
% References
% M. Drulea and S. Nedevschi, "Total variation regularization of
% local-global optical flow," in Intelligent Transportation Systems (ITSC),
% 2011 14th International IEEE Conference on, 2011, pp. 318-323.
%
% Copyright (C) 2011 Technical University of Cluj-Napoca
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

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

function [u] = tv_min(u0, lambda)
%   The output u approximately minimizes the Rudin-Osher-Fatemi (ROF)
%   denoising model
%
%       Min  TV(u) + 1/(2*lambda)* || u - u0 ||^2_2,
%        u

u = zeros(size(u0));

% initialization of the dual variable
p1 = zeros(size(u0));
p2 = zeros(size(u0));

tau = 1/4;
max_iters = 100;

der_mask = [0 -1 1];
adjoint_der_mask = [-1 1 0];

for i=1:max_iters

    % the divergence
    div_p = imfilter(p1, adjoint_der_mask, 'replicate') + ...
        imfilter(p2, adjoint_der_mask', 'replicate');

    t = div_p - u0/lambda;

    % the derivatives
    tx = imfilter(t, der_mask, 'replicate');
    ty = imfilter(t, der_mask', 'replicate');

    denominator = 1 + tau * sqrt(tx.^2 + ty.^2);

    % update dual variable; gradient ascent and reprojection
    p1 = (p1 + tau*tx)./denominator;
    p2 = (p2 + tau*ty)./denominator;

    % update variable; gradient descent
    u = u0 - lambda*div_p;
end

end


function procNoiseParam=processNoiseSuggest(algorithm,maxVal,T,sigmaw2,manDur)
%%PROCESSNOISESUGGEST Use one of a number methods for choosing the scaling
%             parameter for the process noise covariance in a number of
%             continuous-time and discrete-time dynamic models. This only
%             provides the scaling parameter for one dimension of motion.
%             In a multidimensional system, if the measurement noise
%             variance varies between dimensions (such as tracking in range
%             and angle with a range-angle state), then one might use
%             different values in different dimensions. The method only
%             considers scalar measurements in the respective dimensions
%             and is thus only good for a rough approximation.
%
%INPUTS: algorithm A string specifying the algorithm and model used to
%           estimate a good process noise parameter. Possible values are:
%                  'PolyKal-ROT' Use a rule-of-thumb method for determining
%                                the q0 term for the QPolyKal function, for
%                                discrete-time models, and the DPoly
%                                function, for continuous-time models. This
%                                covers the continuous white noise
%                                acceleration (CWNA) model (order=1), the
%                                discretized continuous white noise
%                                acceleration (DCWNA) model (order=1), the
%                                continuous Wiener process acceleration
%                                (CWPA) model  (order=2), and the
%                                discretized continuous Wiener process
%                                acceleration (DCWPA) model (order=2),
%                                among others.
%           'PolyKalDirectDisc-ROT' Use a rule-of-thumb method for
%                                determining the sigmaV2 parameter for the
%                                QPolyKalDirectDisc function. This covers
%                                the discrete white noise acceleration
%                                (DWNA) model (order=1), among others.
%           'PolyKalDirectAlt-ROT' Use a rule-of-thumb method for
%                                determining the sigmaV2 parameter for the
%                                QPolyKalDirectAlt function. This covers
%                                the discrete Wiener process acceleration
%                                (DWPA) model (order=2), among others.
%                 'CWNA-OptMMSE' Use the method of Blair for determining
%                                the asymptotically optimal value of q0 in
%                                terms of MSE for the CWNA and DCWNA
%                                dynamic models for a maneuver having a
%                                fixed maximum acceleration.
%                 'DWNA-OptMMSE' Use the method of Blair for determining
%                                the asymptotically optimal value of
%                                sigmaV2 for the DWNA model for a maneuver
%                                having a fixed maximum acceleration.
%               'CWNA-ConstMeas' Use the method of Blair for determining
%                                the asymptotically optimal value of q0
%                                for the CWNA and DCWNA models such that
%                                the MSE of a Kalman filter under a maximum
%                                acceleration maneuver is not worse than
%                                the measurement accuracy.
%               'DWNA-ConstMeas' Use the method of Blair for determining
%                                the asymptotically optimal value of
%                                sigmaV2 for the DWNA model such that
%                                the MSE of a Kalman filter under a maximum
%                                acceleration maneuver is not worse than
%                                the measurement accuracy.
%    maxVal All of the methods require a maximum bound related to how the
%           target can maneuver. For algorithms CWNA-OptMMSE,
%           DWNA-OptMMSE, CWNA-ConstMeas, and DWNA-ConstMeas, maxVal is
%           the maximum acceleration of the target. For PolyKal-ROT and
%           PolyKalDirectDisc-ROT, and PolyKalDirectAlt-ROT maxVal is the
%           maximum value of a moment one order higher than the maximum
%           order of the dynamic model. For example, if order=1, then
%           maxVal is a maximum acceleration.  If order=2, then maxVal is
%           a maximum jerk.
%         T For all of the algorithms except PolyKalDirectDisc-ROT, this
%           parameter is required and is the typical time between
%           measurements of the target.
%   sigmaw2 For algorithms CWNA-OptMMSE, DWNA-OptMMSE, CWNA-ConstMeas, and
%           DWNA-ConstMeas, this parameter is required and is the variance
%           of the 1D measurement, which should be the lowest order moment
%           of the state. In other words, position.
%    manDur For algorithms CWNA-OptMMSE, DWNA-OptMMSE, CWNA-ConstMeas, and
%           DWNA-ConstMeas, this parameter is required and is the
%           maximum number of samples of duration T that a maximum
%           acceleration maneuver is expected to take. This can take
%           values, 3, 4, 5, 6 and infinity. The default if omitted or an
%           empty matrix is passed is Inf.
%
%OUTPUTS: procNoiseParam The value of q0 or sigmaV2 for the specified
%                        dynamic model, chosen according to the selected
%                        ad-hoc parameter.
%
%The output of this function goes into functions like QPolyKal. It is meant
%that one computes the process noise parameter once (for QPolyKal, this
%would correspond to the q0 parameter), and then one calls the other
%functions with different samples prediction intervals. This function
%should not be called to change the process noise parameter as the sampling
%period changes.
%
%All of the rule-of-thumb methods for choosing the process noise values are
%from Chapter 6.2 and 6.3 of [1] and modified slightly, as described in the
%comments for the implementation below.
%
%The solutions for the ConstMeas and MMSE methods are from [2], which is an
%extension of the work in [3] and [4].
%
%The rule-of thumb parameters might be suitable for other dynamic models.
%For example, when using the Singer model, given with aGaussMarkov and
%DPoly in continuous-time and with FGaussMarkov and QGaussMarkov in
%discrete-time, there does not appear to be a clear way to set the scaling
%value for the process noise in the literature. However, if tau=infinity in
%the model, then in continuous time, it reduces to the CWPA model. Thus,
%methods for choosing the process noise parameter in the CWPA and the DCWPA
%models might be good starting points for setting the process noise
%parameter in the continuous and discrete Singer models.
%
%REFERENCES:
%[1] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%[2] W. D. Blair, "Design of nearly constant velocity filters for radar
%    tracking of maneuvering targets," in Proceedings of the IEEE Radar
%    Conference, Atlanta, GA, 7-11 May 2012, pp. 1008-1013.
%[3] W. D. Blair, "Design of nearly constant velocity track filter for
%    brief maneuvers," in Proceedings of the 14th International Conference
%    on Information Fusion, Chicago, IL, 5-8 Jul. 2011.
%[4] W. D. Blair, "Design of nearly constant velocity track filters for
%    tracking maneuvering targets," in Proceedings of the 11th
%    International Conference on Information Fusion, Cologne, Germany, 30
%    Jun. - 3 Jul. 2008.
%
%April 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(manDur))
    manDur=Inf;
end

switch(algorithm)
    case 'PolyKal-ROT'
        %The rule of thumb is generalized from the rules of thumb in
        %Chapters 6.2.2 and 6.2.3 of Bar-Shalom's book, when considering
        %the CWNA, DCWNA, CWPA and DCWPA. For the CWNA and DCWNA models, it
        %is suggested that sqrt(q0*T) be on the order of the changes in
        %velocity over a sampling interval. In other words, it is the
        %average acceleration. It might be more useful to express things in
        %terms of a maximum acceleration, though. In Chapter 6.3.2 for the
        %DWNA model, it is suggested that  0.5*aMax<=sigmaV<=aMax. Using a
        %similar logic, one might try  0.5*aMax<=sqrt(q0*T)<=aMax. Using
        %the same logic for the CWPA and DCWPA models of Chapter 6.2.3, one
        %might choose q0 such that  0.5*jerkMax<=sqrt(q0*T)<=jerkMax, where
        %jerkMax is the maximum possible jerk (derivative of acceleration).
        %This function generalizes the rule  to any order and chooses the
        %point midway in that specified range.

        procNoiseParam=(0.75*maxVal)^2/T;
        return;
    case 'PolyKalDirectDisc-ROT'
        %The rule-of-thumb is generalized from that given in Chapter 6.3.2
        %of Bar-Shalom's book. In the book, it is suggested that
        %0.5*aMax<=sigmaV<=aMax for the DWNA model (order=1). This function
        %generalizes it to any order and chooses the point midway in that
        %specified range.

        procNoiseParam=(0.75*maxVal)^2;
        return;
    case 'PolyKalDirectAlt-ROT'
        %The rule-of-thumb is generalized from that given in Chapter 6.3.3
        %of Bar-Shalom's book. In the book, it is suggested that
        %0.5*deltaAMax<=sigmaV<=deltaAMax for the DWPA model (order=2),
        %where deltaAMax is the maximum change in acceleration over an
        %interval of duration T. This can be related to a maximum jerk as
        %0.5*jerkMax*T<=sigmaV<=jerkMax*T. This function generalizes it to
        %any order and chooses the point midway in that range.

        procNoiseParam=(0.5*maxVal*T)^2;
        return;
    case 'CWNA-OptMMSE'
        optType=0;
        isDiscrete=false;
    case 'DWNA-OptMMSE'
        optType=0;
        isDiscrete=true;
    case 'CWNA-ConstMeas'
        optType=1;
        isDiscrete=false;
    case 'DWNA-ConstMeas'
        optType=1;
        isDiscrete=true;
    otherwise
        error('Unknown algorithm chosen')
end

%Equation 3, the deterministic maneuvering index.
GammaD=maxVal*T^2/sqrt(sigmaw2);

if(0.01>GammaD||GammaD>10)
   warning('The deterministic maneuvering index is outside of the tabulated range of values. The process noise parameter might be inaccurate.')
end
%Blair's papers do not say the base, but if one uses base 10, one gets the
%results in the plots. If one uses base e, the results do not match the
%plots in the paper.
logVal=log10(GammaD);

%optType=0 means MMSE (so use max)
%optType=1 means limit the maximum error to the measurement noise (so use
%          min)
switch(manDur)
    case Inf
        if(optType)
            curRow=2;
        else
            curRow=1;
        end
    case 3
        if(optType)
            curRow=4;
        else
            curRow=3;
        end
    case 4
        if(optType)
            curRow=6;
        else
            curRow=5;
        end
    case 5
    %If one wants a five-sample maneuver, then we will average the values
    %from the length four and the length-six maneuvers.
        if(optType)
            curRow=[6;8];
        else
            curRow=[5;7];
        end
    case 6
        if(optType)
            curRow=8;
        else
            curRow=7;
        end
    otherwise
        error('An untabulated value for the maximum maneuver duration has been given')
end

%Table I in Blair's paper
k1CoeffTable=[1.677, -0.726,  0.230, -0.012, 0.005,  0.000;%k^{max}_{\inf}
              0.872, -0.102, -0.019,  0.010, 0.006,  0.001;%k^{min}_{\inf}
              1.539, -0.189, -0.651,  0.187, 0.284,  0.059;%k^{max}_{3}
              0.707,  0.346, -0.318, -0.097, 0.088,  0.029;%k^{min}_{3}
              1.630, -0.473, -0.403,  0.272, 0.173,  0.023;%k^{max}_{4}
              0.803,  0.197, -0.385, -0.009, 0.138,  0.035;%k^{min}_{4}
              1.671, -0.697,  0.025,  0.250,-0.0324,-0.028;%k^{max}_{6}
              0.869,  0.008, -0.320,  0.088, 0.123,  0.023];%k^{min}_{6}

a0=k1CoeffTable(curRow,1);
a1=k1CoeffTable(curRow,2);
a2=k1CoeffTable(curRow,3);
a3=k1CoeffTable(curRow,4);
a4=k1CoeffTable(curRow,5);
a5=k1CoeffTable(curRow,6);

%Equations 6 and 7. The mean command is for averaging if one wanted a
%length 5 maneuver. Note that equation 6 has a repeated cubed term. That
%appears to be a mistake.
k1=mean(a0+logVal*(a1+logVal*(a2+logVal*(a3+logVal*(a4+a5*logVal)))));

if(isDiscrete)%Discrete-time
    procNoiseParam=(k1*maxVal)^2;
else%Continuous-time
    procNoiseParam=T*(k1*maxVal)^2;
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 bcdof = BoundaryCondition(typeBC,coordinates)

%--------------------------------------------------------------------------
%   Purpose:
%           To determine the boundary conditions degree of freedom
%   Synopsis:
%           bcdof = BoundaryCondition(typeBC,coordinates)
%
%   Variable Description:
%           bcdof - boundary condition degree of freedom
%                   dof's are (UZ,RX,RY)
%           typeBC - string which gives the state of boundary condition
%           coordinates - geometric coordinates of nodes
%
%--------------------------------------------------------------------------

L1 = find(coordinates(:,2)==min(coordinates(:,2))) ; % at y = 0 (along X-axes)
L2 = find(coordinates(:,1)==max(coordinates(:,1))) ; % at x = a (along Y-axes)
L3 = find(coordinates(:,2)==max(coordinates(:,2))) ; % at y = b (along X-axes)
L4 = find(coordinates(:,1)==min(coordinates(:,1))) ; % at x = 0 (along Y-axes)
n = length(L1) ;

switch typeBC
    case 'ss-ss-ss-ss'
        disp('plate is simply supported at all the edges')
        dofL1 = zeros(1,2*n) ;
        dofL2 = zeros(1,2*n) ;
        dofL3 = zeros(1,2*n) ;
        dofL4 = zeros(1,2*n) ;
        for i = 1:n
            i1 = 2*(i-1)+2 ;
            i2 = i1-1 ;
            dofL1(i1) = 3*L1(i);
            dofL1(i2) = 3*L1(i)-2 ;
            dofL3(i1) = 3*L3(i) ;
            dofL3(i2) = 3*L3(i)-2 ;
        end
        for i = 1:n
            i1 = 2*(i-1)+2 ;
            i2 = i1-1 ;
            dofL2(i1) = 3*L2(i)-1 ;
            dofL2(i2) = 3*L2(i)-2 ;
            dofL4(i1) = 3*L4(i)-1 ;
            dofL4(i2) = 3*L4(i)-2 ;
        end
        L1UL3 = union(dofL1,dofL3) ;
        L2UL4 = union(dofL2,dofL4) ;
        bcdof = union(L1UL3,L2UL4) ;

    case 'c-c-c-c'
        disp('plate is clamped at all the edges')
        dofL1 = zeros(1,2*n) ;
        dofL2 = zeros(1,2*n) ;
        dofL3 = zeros(1,2*n) ;
        dofL4 = zeros(1,2*n) ;
        for i = 1:n
            i1 = 2*(i-1)+2 ;
            i2 = i1-1 ;
            dofL1(i1) = 3*L1(i)-1;
            dofL1(i2) = 3*L1(i)-2 ;
            dofL3(i1) = 3*L3(i)-1 ;
            dofL3(i2) = 3*L3(i)-2 ;
        end
        for i = 1:n
            i1 = 2*(i-1)+2 ;
            i2 = i1-1 ;
            dofL2(i1) = 3*L2(i) ;
            dofL2(i2) = 3*L2(i)-2 ;
            dofL4(i1) = 3*L4(i) ;
            dofL4(i2) = 3*L4(i)-2 ;
        end
        L1UL3 = union(dofL1,dofL3) ;
        L2UL4 = union(dofL2,dofL4) ;
        bcdof = union(L1UL3,L2UL4) ;
end