% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open 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.
%
% dslam_open 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 dslam_open. If not, see <http://www.gnu.org/licenses/>.

function [informationExchangeGaussSeidel, nrGaussSeidelIterations, ...
    informationExchangeDDF, commLinks] = getDistGaussSeidelStats(dir)

filename = [dir '/trace'];
trace_file = sprintf('%s_overall_error.txt',filename);
trace = dlmread(trace_file, ' ');

end_index = find(trace(1,:) == -1);
rotation_iterations = end_index-1;

end_index = find(trace(2,:) == -1);
pose_iterations = end_index-1;

centralized_error = trace(3,1);
distributed_error = trace(4,1);

sizePose = 6*8;  % Assumed to be 6 doubles, 1 double = 8 byte
sizeRotation = 9*8;  % Assumed to be 9 doubles, 1 double = 8 byte

commLinks = 2 * trace(5,1);

% Gauss-Seidel Communication
nrGaussSeidelIterations = pose_iterations + rotation_iterations;
informationExchangeGaussSeidel = pose_iterations.*(commLinks*sizePose) + rotation_iterations.*(commLinks*sizeRotation);

% Compare with DDF-SAM Communication
nrGNIterations = 1; % typically 3
sizePoseDDFSAM = 6*8;  % Assumed to be 12 doubles, 1 double = 8 byte
informationExchangeDDF = nrGNIterations.*((commLinks*sizePoseDDFSAM) + (commLinks*sizePoseDDFSAM).^2);


%% FUNCTION Least_Dirty
%   Dirty Multi-Task Learning with Least Squares Loss.
%
%% OBJECTIVE
%   argmin_W ||X(P+Q) - Y||_F^2 + lambda1*||P||_{1,inf} + lambda2*||Q||_{1,1}
%    s.t. W = P + Q
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   rho1: group sparsity regularization parameter
%   rho2: elementwise sparsity regularization parameter
%
%% OUTPUT
%   W: model: d * t
%   funcVal: function value vector.
%   P: group sparsity structure (joint feature slection)
%   Q: elementwise sparsity component
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou, Pinghua Gong 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 June 3, 2012.
%
%% RELATED PAPERS
%
%   [1] Jalali, A. and Ravikumar, P. and Sanghavi, S. and Ruan, C. A dirty
%       model for multi-task learning, NIPS 2010.
%
%% RELATED FUNCTIONS
%   init_opts, combine_input (utils), prf_lbm (c_files)

function [W, funcVal, P, Q] = Least_Dirty(X, Y, lambda1, lambda2, opts)

if nargin <4
    error('\n Inputs: X, Y, and lambda1, and lambda2 should be specified!\n');
end
if nargin <5
    opts = [];
end

% initialize options.
opts=init_opts(opts);

% initial Lipschiz constant.
if isfield(opts, 'lFlag')
    lFlag = opts.lFlag;
else
    lFlag = false;
end

task_num = length(X);
[X, y, ~, samplesize] = combine_input(X, Y);
dimension = size(X, 2);

% initialize a starting point
if opts.init==2
    P0 = zeros(dimension, task_num);
    Q0 = zeros(dimension, task_num);
elseif opts.init == 0
    P0 = randn(dimension, task_num);
    Q0 = randn(dimension, task_num);
else
    if isfield(opts,'P0')
        P0=opts.P0;
        if (nnz(size(P0)-[dimension, task_num]))
            error('\n Check the input .P0');
        end
    else
        P0=zeros(dimension, task_num);
    end

    if isfield(opts,'Q0')
        Q0=opts.Q0;
        if (nnz(size(Q0)-[dimension, task_num]))
            error('\n Check the input .Q0');
        end
    else
        Q0=zeros(dimension, task_num);
    end
end

% Set an array to save the objective value
funcVal = [];

P = P0; Q = Q0;

%[d,m] = size(P); % d: dimension, m: the number of tasks
X = diagonalize(X,samplesize);
XtX = X'*X; Xty = X'*y;

Pn = P;  Qn = Q;
t_new = 1;
L1norm = max(sum(abs(X),1)); Linfnorm = max(sum(abs(X),2));

if lFlag
    % Upper bound for largest eigenvalue of Hessian matrix
    L = 2*min([L1norm*Linfnorm; size(X,1)*Linfnorm*Linfnorm; size(X,2)*L1norm*L1norm; size(X,1)*size(X,2)*max(abs(X(:)))]);
else
    % Lower bound for largest eigenvalue of Hessian matrix
    L = 2*max(L1norm*L1norm/size(X,1),Linfnorm*Linfnorm/size(X,2));
end
% Initial function value
funcVal = cat(1, funcVal, norm(X*(P(:) + Q(:)) - y)^2 + lambda1*L1infnorm(P) + lambda2*L11norm(Q));
%fun(1) = norm(X*(P(:) + Q(:)) - y)^2 + lambda1*L1infnorm(P) + lambda2*L11norm(Q);

%count = 0;
for iter = 1 : opts.maxIter
    P_old = P; Q_old = Q;
    t_old = t_new;
    gradvec = 2*(XtX*(Pn(:)+Qn(:)) - Xty);
    gradmat = reshape(gradvec,dimension,task_num);
    % If we estimate the upper bound of Lipschitz constant, no line search
    % is needed.
    if lFlag
        P = proximalL1infnorm(Pn - gradmat/L, lambda1/L);
        Q = proximalL11norm(Qn - gradmat/L, lambda2/L);
    else
        % line search
        for inneriter = 1:20
            P = proximalL1infnorm(Pn - gradmat/L, lambda1/L);
            Q = proximalL11norm(Qn - gradmat/L, lambda2/L);
            dP = P - Pn;  dQ = Q - Qn;
            if 2*((dP(:) + dQ(:))'*XtX*(dP(:) + dQ(:))) <= L*sum(sum((dP.*dP + dQ.*dQ)))
                break;
            else
                L = L*2;
            end
        end
    end
    funcVal = cat(1, funcVal, norm(X*(P(:) + Q(:)) - y)^2 + lambda1*L1infnorm(P) + lambda2*L11norm(Q));
    %fun(iter+1) = norm(X*(P(:) + Q(:)) - y)^2 + lambda1*L1infnorm(P) + lambda2*L11norm(Q);

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    %     % stopping condition
    %     if abs(fun(iter) - fun(iter+1))/fun(iter+1) < tol
    %         count = count + 1;
    %     else
    %         count = 0;
    %     end
    %     if count >= 3
    %         break;
    %     end

    % Update the coefficient
    t_new = (1+sqrt(1+4*t_old^2))/2;
    Pn = P + (t_old-1)/t_new*(P - P_old);
    Qn = Q + (t_old-1)/t_new*(Q - Q_old);

end

W = P + Q;

end


function anat = makeFlatAnat(view)
%
% function anat = makeFlatAnat(view)
%
% djh, 8/1/99  Culled this stuff out of loadAnat.
% djh, 8/18/99 Modified to use curvature if available.

disp('Interpolating flat images...');

% Initialize to zeros
imSize = view.ui.imSize;
anat = NaN*ones([imSize,2]);

% Loop through hemispheres and make the images with myGriddata
for h=1:2

    % Load gLocs to get 2D coords and Z
    if h==1
        [gLocs2d,gLocs3d,curvature] = loadGLocs('left',view.leftPath);

    else
        [gLocs2d,gLocs3d,curvature] = loadGLocs('right',view.rightPath);

    end
    if ~isempty(gLocs2d)
        y = gLocs2d(1,:);
        x = gLocs2d(2,:);

        % If curvature is nonempty, then use it.  Otherwise use z coord

        if ~isempty(curvature)
            z = curvature;
        else
            z = gLocs3d(3,:);
        end

        % Remove NaN coords
        allFinite = isfinite(x) & isfinite(y);

        x = x(allFinite);
        y = y(allFinite);

        % Deal with new curvature coding: only l1 curvatures are now
        % encoded to make the flat maps look nicer. The following code
        % checks to see if we are building an anatomy from the old or the
        % new version of the flat files.
        if (length(z)<length(x))
            x=x(1:length(z));
            y=y(1:length(z));
        else
            z = z(allFinite);
        end

        % call griddata
        yi = [1:imSize(1)]';
        xi = [1:imSize(2)];
        warning off;

        if (checkML7)
            disp('Matlab 7 or higher detected..');
%             anat(:,:,h) = griddata(x,y,z,xi,yi,'linear',{'QJ'});
            anat(:,:,h) = griddata(x,y,z,xi,yi,'linear');
        else
            anat(:,:,h) = griddata(x,y,z,xi,yi,'linear');
        end

        warning backtrace;
    end
end


classdef math_model_cpf_acpi < mp.math_model_cpf_acp & mp.mm_shared_pfcpf_acpi
%MP.MATH_MODEL_CPF_ACPI  MATPOWER mathematical model for continuation power flow (CPF) problem.
%   ?
%
%   MP.MATH_MODEL_CPF_ACPI ... power flow ...
%
%   Properties
%       ? - ?
%
%   Methods
%       ?

%   MATPOWER
%   Copyright (c) 2021-2022, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%     properties
%     end

    methods
        function tag = form_tag(obj)
            tag = 'acpi';
        end

        function name = form_name(obj)
            name = 'AC-polar-current';
        end

        function obj = add_node_balance_constraints(obj, nm, dm, mpopt)
            %% power balance constraints
            ad = obj.aux_data;
            npvq = ad.npv+ad.npq;
            fcn = @(x)node_balance_equations_cpf(obj, x, nm);
            obj.add_nln_constraint({'Irmis', 'Iimis'}, [npvq;npvq], 1, fcn, []);
        end
    end     %% methods
end         %% classdef


function scansV = getScanAt(scanNum, xV, yV, zV, planC)
%"getScanAt"
%   Returns the scan values at the points defined by vectors xV, yV, zV, in the
%   requested scanNum.  Uses linear interpolation with zeros returned for
%   x,y,z points that are out of the scan bounds.
%
%   Any transM values are NOT included.  If the transformed scan is wanted,
%   pass xV,yV,zV through applyTransM before calling getScansAt.
%
%   If planC isn't specified, the global planC is used.
%
%LM: 02 May 03, JOD, first version.  Based on getDVH.m.
%    24 Feb 05, JRA, Modified header, now using some subfunctions.
%                    Also updated for new finterp3.
%
%Usage:
%   scansV = getScanAt(scanNum, xV, yV, zV, planC)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

indexS = planC{end};

[xVS, yVS, zVS] = getScanXYZVals(planC{indexS.scan}(scanNum));

%Use scan access function in case of remote variables.
% sA = getScanArray(planC{indexS.scan}(scanNum));

sA = planC{indexS.scan}(scanNum).scanArray;

%Interpolate to values in xV/yV/zV from the scanarray, 0 if out of bounds.
tol = 1e6*eps;
if numel(size(sA)) > 2
    scansV = finterp3(xV, yV, zV, sA, [xVS(1)-tol xVS(2)-xVS(1) xVS(end)+tol], [yVS(1)+tol yVS(2)-yVS(1) yVS(end)-tol], zVS, 0);
else
    scansV = finterp2(xVS,yVS,single(sA),xV,yV,0);
end
% scansV = interp3(xVS, yVS, zVS, sA , xV, yV, zV);


function catOBJs(input_files, output_file)
  % Concatenate meshes obj files into one mesh
  %
  % catOBJs(input_files, output_file)
  %
  % Inputs:
  %   input_files  cell of paths to input files
  % Output:
  %   output_file  path to output OBJ
  %
  [V,F] = load_mesh(input_files{1});
  for i = 2:numel(input_files)
    [Vi,Fi] = load_mesh(input_files{i});
    [V, F] = cat_meshes(V,F,Vi,Fi);
  end
  writeOBJ(output_file,V,F);
end


function ExecFlag=DoGradChk

global VSeq
global VCtl

ExecFlag = 1;
GradTolerance = abs(VCtl.MaxGrad * 0.005);
SRTolerance = abs(VCtl.MaxSlewRate * 0.005);
% check Gz
Flags = VSeq.flagsLine(2,:);
t = VSeq.tsLine(Flags~=0);
dt =  diff(t);
dG =  diff(VSeq.GzAmpLine);

SlewRate = dG./dt;

if ~isempty(find((abs(SlewRate)-VCtl.MaxSlewRate)>SRTolerance, 1))

    disp(['GzSS Max SlewRate ' num2str(max(abs(SlewRate)))]);

    errordlg('GzSS exceeds maximum allowed gradient slew rate!');
    ExecFlag = 0;
end

if ~isempty(find((abs(VSeq.GzAmpLine)-VCtl.MaxGrad)>GradTolerance, 1))

    disp(['GzSS Max Gradient ' num2str(max(abs(VSeq.GzAmpLine)))]);

    errordlg('GzSS exceeds maximum allowed gradient amplitude!');
    ExecFlag = 0;
end

% check Gy
Flags = VSeq.flagsLine(3,:);
t = VSeq.tsLine(Flags~=0);
dt =  diff(t);
dG =  diff(VSeq.GyAmpLine);

SlewRate = dG./dt;

if ~isempty(find((abs(SlewRate)-VCtl.MaxSlewRate)>SRTolerance, 1))

    disp(['GyP Max SlewRate ' num2str(max(abs(SlewRate)))]);

    errordlg('GyP exceeds maximum allowed gradient slew rate!');
    ExecFlag = 0;
end

if ~isempty(find((abs(VSeq.GyAmpLine)-VCtl.MaxGrad)>GradTolerance, 1))

    disp(['GyP Max Gradient ' num2str(max(abs(VSeq.GyAmpLine)))]);

    errordlg('GyP exceeds maximum allowed gradient amplitude!');
    ExecFlag = 0;
end

% check Gx
Flags = VSeq.flagsLine(4,:);
t = VSeq.tsLine(Flags~=0);
dt =  diff(t);
dG =  diff(VSeq.GxAmpLine);

SlewRate = dG./dt;

if ~isempty(find((abs(SlewRate)-VCtl.MaxSlewRate)>SRTolerance, 1))

    disp(['GxR Max SlewRate ' num2str(max(abs(SlewRate)))]);

    errordlg('GxR exceeds maximum allowed gradient slew rate!');
    ExecFlag = 0;

end

if ~isempty(find((abs(VSeq.GxAmpLine)-VCtl.MaxGrad)>GradTolerance, 1))

    disp(['GxR Max Gradient ' num2str(max(abs(VSeq.GxAmpLine)))]);

    errordlg('GxR exceeds maximum allowed gradient amplitude!');
    ExecFlag = 0;
end

end


function [pot, MEU] = marginal_nodes(engine, d)

C = length(cliques);
%clpot = init_clpot(limid, cliques, d, clq_ass_to_node);
clpot = init_clpot(limid, cliques, [], clq_ass_to_node);

% collect to root
if 1
  % HUGIN
  seppot = cell(C, C);    % separators are implicitely initialized to 1s
  for n=postorder{di}(1:end-1)
    for p=parents(rooted_jtree{di}, n)
      %clpot{p} = divide_by_pot(clpot{n}, seppot{p,n}); % dividing by 1 is redundant
      seppot{p,n} = marginalize_pot(clpot{n}, separator{p,n});
      clpot{p} = multiply_by_pot(clpot{p}, seppot{p,n});
    end
  end
else
  % Shafer-Shenoy
  msg = cell(C,C);
  for n=postorder{di}(1:end-1)
    for c=children(rooted_jtree{di}, n)
      clpot{n} = multiply_by_pot(clpot{n}, msg{c,n});
    end
    p = parents(rooted_jtree{di}, n);
    %msg{n,p} = marginalize_pot(clpot{n}, cliques{p});
    msg{n,p} = marginalize_pot(clpot{n}, separator{n,p});
  end
  root = clq_ass_to_node(d);
  n=postorder{di}(end);
  assert(n == root);
  for c=children(rooted_jtree{di}, n)
    clpot{n} = multiply_by_pot(clpot{n}, msg{c,n});
  end
end

fam = family(limid.dag, d);
pot = marginalize_pot(clpot{root}, fam);

%%%%%%%
jpot = compute_joint_pot_limid(limid);
pot2 = marginalize_pot(jpot, fam);
assert(approxeq_pot(pot, pot2))
%%%%%%

[policy, score] = extract_policy(pot);

e = limid.equiv_class(d);
limid.CPD{e} = set_params(limid.CPD{e}, 'policy', policy);


function ADEM_cost_SHC
% This demo illustrates the use of priors on the motion of hidden states as
% polices. It simulates exploration and exploitation using radial basis
% function attractors and auto-vitiative (self-destroying) attractors
% as the basis of the prior. These dynamics enforce exploration, under
% active inference. In turn, this exploration enables perceptual learning
% to associate attractors with changes in physiological states (cf,
% rewards). This can be exploited to by formal priors to ensure regions of
% physiological state-space are avoided.
% We look at this scheme using simulated pathology; first, we simulate a
% (neurodegenerative) reduction in log-precision (cf Parkinson's disease) on
% the motion of physical states.  This results in active inference with
% a loss of precise volitional movement and subsequent failure to optimise
% physiological states. Second, we look at the effects of precision on
% learning by increasing log-precision (cf, Addition) on the motion of
% physiological states. This results in a failure to learn and, again,
% subsequent failure to optimise physiological states.
%__________________________________________________________________________
% Copyright (C) 2010 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: ADEM_cost_SHC.m 7679 2019-10-24 15:54:07Z spm $

% switch for demo
%--------------------------------------------------------------------------
DEMO = 1;

% location and radius of attractors (A)
%--------------------------------------------------------------------------
global A
A.x  = [1 -1 -1  1;
        2 -2  2 -2];
A.d  = 1/4;              % s.d. of Gaussian radial basis function
A.u  = 1/8;              % threshold to induce rapid decay of x.a
A.q  = [1 2];            % indices of locations that increase x.q

% generative process
%==========================================================================

% level 1
%--------------------------------------------------------------------------
G(1).x.x = [0;0];
G(1).x.v = [1;0];
G(1).x.q = [1;1/2];

G(1).f   = inline('spm_cost_SHC_fxa(x,v,a,P)','x','v','a','P');
G(1).g   = inline('[x.x; x.v; x.q]','x','v','a','P');
G(1).V   = exp(16);                          % error precision
G(1).W   = exp(16);                          % error precision
G(1).U   = exp(4);                           % action precision

% level 2
%--------------------------------------------------------------------------
G(2).a   = sparse(2,1);                      % action
G(2).v   = 0;                                % inputs
G(2).V   = exp(16);
G        = spm_ADEM_M_set(G);

% generative model
%==========================================================================

% level 1
%--------------------------------------------------------------------------
M(1).x.x = G(1).x.x;
M(1).x.v = G(1).x.v;
M(1).x.q = G(1).x.q;
M(1).x.a = randn(4,1)/8;

M(1).f   = inline('spm_cost_SHC_fx(x,v,P)','x','v','P');
M(1).g   = inline('[x.x; x.v; x.q]','x','v','P');
M(1).pE  = speye(4,2);

M(1).V   = exp(8);
M(1).W   = diag(exp([[1 1 1 1]*4 [1 1]*6 [1 1 1 1]*4]));

% level 2 (no exogenous inputs in this simulation)
%--------------------------------------------------------------------------
M(2).v   = 0;
M(2).V   = exp(16);

% Integrate: active inference
%==========================================================================
M(1).E.nE = 1;
M(1).E.n  = 4;

N      = 128;
DEM.U  = sparse(N,1);
DEM.C  = sparse(N,1);
DEM.G  = G;
DEM.M  = M;

if DEMO
    load DEM_addiction
else
    DEM = spm_ADEM(DEM);
end

% show behavior
%==========================================================================

% overview
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 1');
spm_DEM_qU(DEM.qU)

subplot(2,2,3)
spm_cost_SHC_path(DEM.pU,A)

% a closer look at physiology
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 2'); clf

subplot(2,1,1)
t    = 1:N;
plot(t,DEM.qU.x{1}(5:6,:)',t,t*0 + A.u,':'), hold on
plot(t,DEM.qU.x{1}(7:10,:)','-.'),           hold off
xlabel('time','Fontsize',14)
title('internal states','FontSize',16)
axis square, box off, set(gca,'XLim',[1 N])

subplot(2,2,3)
plot(t,DEM.qU.x{1}(5:6,:)',t,t*0 + A.u,':')
xlabel('time','Fontsize',14)
title('physiological states','FontSize',16)
axis square, box off, set(gca,'XLim',[1 N])

subplot(2,2,4)
plot(DEM.pU.x{1}(5,:),DEM.pU.x{1}(6,:)),               hold on
plot([A.u 1],[A.u A.u],'-.r',[A.u A.u],[A.u 1],'-.r'), hold off
xlabel('time','Fontsize',14)
title('physiological states','FontSize',16)
axis square, box off, axis([-.1 1.2 -.1 1.2])

% look at the effect of precision on inference (cf Parkinson's disease)
%==========================================================================

% initialize states
%--------------------------------------------------------------------------
DEM = spm_ADEM_update(DEM);

% simulate exposure with different log-precisions on physical motion
%--------------------------------------------------------------------------
WP    = [4 2 0];
if ~DEMO
    for i = 1:length(WP)
        DEM_P{i}        = DEM;
        W               = [[1 1 1 1]*WP(i) [1 1]*6 [1 1 1 1]*4];
        DEM_P{i}.M(1).W = diag(exp(W));
        DEM_P{i}        = spm_ADEM(DEM_P{i});
    end
end

% Graphics - path and physiology
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 3'); clf
for i = 1:3

    % path
    %----------------------------------------------------------------------
    spm_figure('GetWin','Figure 3');
    subplot(3,2,i*2 - 1)
    spm_cost_SHC_path(DEM_P{i}.pU,A)

    % and physiology
    %----------------------------------------------------------------------
    subplot(3,2,i*2)
    plot(t,DEM_P{i}.pU.x{1}(5:6,:)',t,t*0 + A.u,':'), hold on
    plot(t,sum(DEM_P{i}.pU.x{1}(1:2,:).^2)/8,'m:'),    hold off
    xlabel('time','Fontsize',12)
    title(sprintf('%s (%1.0f)','physiological states',WP(i)),'FontSize',16)
    axis square, box off, set(gca,'XLim',[1 N])
    drawnow

end

% Graphics - action and prediction error
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 4'); clf
for i = 1:3

    % path
    %----------------------------------------------------------------------
    subplot(3,2,2*i - 1)
    plot(t,DEM_P{i}.qU.a{2})
    xlabel('time','FontSize',12)
    title('action','FontSize',16)
    axis square, box off, set(gca,'XLim',[1 N])
    if i == 1; a = axis; end, axis(a)

    % and prediction error
    %----------------------------------------------------------------------
    subplot(3,2,2*i)
    plot(t,DEM_P{i}.qU.z{1}(1:4,:))
    xlabel('time','FontSize',12)
    title('sensory error','FontSize',16)
    axis square, box off, set(gca,'XLim',[1 N])
    if i == 1; aa = axis; end, axis(aa)
    drawnow
end

% Learning (at different levels of precision on physiological motion)
%==========================================================================

% Switch attractor for second physiological state and log-precision levels
%--------------------------------------------------------------------------
spm_figure('GetWin','DEM'); clf
A.q  = [1 3];
WA   = [4 8 12];

if ~DEMO

    for i = 1:length(WA)

        % Enable learning
        %------------------------------------------------------------------
        A.u              = 0;
        DEM_L{i}         = DEM;
        DEM_L{i}.M(1).pC = exp(8);
        W                = [[1 1 1 1]*4 [1 1]*WA(i) [1 1 1 1]*4];
        DEM_L{i}.M(1).W  = diag(exp(W));
        DEM_L{i}         = spm_ADEM(DEM_L{i});

        % replace prior with posterior and re-expose
        %------------------------------------------------------------------
        A.u              = 1/8;
        DEM_D{i}         = DEM_L{i};
        DEM_D{i}.M(1).pE = DEM_D{i}.qP.P{1};
        DEM_D{i}.M(1).pC = [];
        DEM_D{i}         = spm_ADEM(DEM_D{i});

    end

    % save DEM structures for future demonstrations
    %----------------------------------------------------------------------
    save DEM_addiction DEM DEM_P DEM_L DEM_D

end

% Graphics - optimal learning
%--------------------------------------------------------------------------
spm_figure('GetWin','DEM'); clf
spm_DEM_qU(DEM.qU,DEM.pU)

spm_figure('GetWin','Figure 5'); clf
spm_DEM_qP(DEM_L{1}.qP)

subplot(2,2,3)
spm_cost_SHC_path(DEM.pU,A)
title('Before','Fontsize',16)
subplot(2,2,4)
spm_cost_SHC_path(DEM_D{1}.pU,A)
title('After','Fontsize',16)
drawnow

% Comparison of learning over levels of log-precisions (cf. Addiction)
%==========================================================================
spm_figure('GetWin','Figure 6'); clf

for i = 1:3

    % path
    %----------------------------------------------------------------------
    subplot(3,2,i*2 - 1)
    spm_cost_SHC_path(DEM_D{i}.pU,A)
    title('After','FontSize',16)

    % and physiology
    %----------------------------------------------------------------------
    subplot(3,2,i*2)
    spm_plot_ci(DEM_L{i}.qP.P{1}(:),DEM_L{i}.qP.C)
    xlabel('parameter','FontSize',12)
    title(sprintf('%s (%1.0f)','learning',WA(i)),'FontSize',16)
    axis square, box off

end
drawnow

% and physiology
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 7'); clf
for i = 1:3

    % predicted motion of second physiological state
    %----------------------------------------------------------------------
    for j = 1:N
        x    = DEM_L{i}.qU.x{1}(:,j);
        f    = spm_cost_SHC_fx(spm_unvec(x,M(1).x),M(2).v,M(1).pE);
        p(j) = DEM_L{i}.qU.w{1}(6,j) + f.q(2);
        x    = DEM_L{i}.pU.x{1}(:,j);
        f    = spm_cost_SHC_fxa(spm_unvec(x,G(1).x),M(2).v,G(2).a,G(1).pE);
        q(j) = f.q(2);
    end
    subplot(3,2,2*i - 1)
    plot(t,p,t,q,'b:')
    xlabel('time','FontSize',12)
    title(sprintf('%s (%1.0f)','predicted motion',WA(i)),'FontSize',16)
    axis square, box off, set(gca,'XLim',[1 N])
    if i == 1; a = axis; end, axis(a)

    subplot(3,2,2*i)
    plot(t,DEM_L{i}.qU.w{1}(6,:))
    xlabel('time','FontSize',12)
    title('prediction error','FontSize',16)
    axis square, box off, set(gca,'XLim',[1 N])
    if i == 1; aa = axis; end, axis(aa)

end


function [X,XX,Y,T] = loadfile(file,T,options)
% load the file and does some optional preprocessing

if iscell(file) % T needs to be cell too
    T = cell2mat(T);
    for j = 1:length(file)
        if ischar(file{j})
            fsub = file{j};
            loadfile_sub;
        else
            X = file{j};
        end
        if j==1
            Xc = zeros(sum(T),size(X,2));
            acc = 0;
        end
        Xc((1:size(X,1)) + acc, :) = X; acc = acc + size(X,1);
    end
    X = Xc; clear Xc
else
    if ischar(file)
        fsub = file;
        loadfile_sub;
    else
        X = file;
    end
end

% Standardise data and control for ackward trials
if isfield(options,'standardise')
    X = standardisedata(X,T,options.standardise);
end
% Filtering
if isfield(options,'filter') && ~isempty(options.filter)
    X = filterdata(X,T,options.Fs,options.filter);
end
% Detrend data
if isfield(options,'detrend') && options.detrend
    X = detrenddata(X,T);
end
% Leakage correction
if isfield(options,'leakagecorr') && options.leakagecorr ~= 0
    X = leakcorr(X,T,options.leakagecorr);
end
% Hilbert envelope
if isfield(options,'onpower') && options.onpower
    X = rawsignal2power(X,T);
end
% Leading Phase Eigenvectors
if isfield(options,'leida') && options.leida
    X = leadingPhEigenvector(X,T);
end
% PCA transform (before embedded, i.e. drawing only from space corr)
if isfield(options,'As') && ~isempty(options.As)
    X = bsxfun(@minus,X,mean(X)); % must center
    X = X * options.As;
end
% Embedding
if isfield(options,'embeddedlags') && length(options.embeddedlags)>1
    [X,T] = embeddata(X,T,options.embeddedlags);
end
% PCA transform
if isfield(options,'A') && ~isempty(options.A)
    X = bsxfun(@minus,X,mean(X)); % must center
    X = X * options.A;
    % Standardise principal components and control for ackward trials
    X = standardisedata(X,T,options.standardise_pc);
end
% Downsampling
if isfield(options,'downsample') && options.downsample > 0
    [X,T] = downsampledata(X,T,options.downsample,options.Fs);
end

if isfield(options,'B'), B = options.B;
else B = []; end
if isfield(options,'V'), V = options.V;
else V = []; end
if nargout>=2
    XX = formautoregr(X,T,options.orders,options.order,options.zeromean,0,B,V);
end
if nargout>=3
    Y = zeros(sum(T)-length(T)*options.order,size(X,2));
    for in=1:length(T)
        t0 = sum(T(1:in-1));
        t = sum(T(1:in-1)) - (in-1)*options.order;
        Y(t+1:t+T(in)-options.order,:) = X(t0+options.order+1:t0+T(in),:);
    end
end
end


function b = p01_b ( m )

%*****************************************************************************80
%
%% P01_B returns the right hand side B for problem 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of equations.
%
%    Output, real B(M,1), the right hand side.
%
  b = [ 1.0; 2.3; 4.6; 3.1; 1.2 ];

  return
end


% mean ROI tcs across all sessions
%
% kgs 1/2007
clear all
% for PC:
expDir = ('y:\RAID\projects\Kids\fmri\adolescents');
% for linux:
%expDir='/biac1/kgs/projects/Kids/fmri/adolescents';

sessions = { 'ar_12yo_011908' 'is_16yo_121607' 'kwl_14yo_011208' 'dw_14yo_102707'};
roi=      {'lPPA_IOvsOJ_p3' 'lPPA_IOvsOJ_p3' 'lPPA_IOvsOJ_p3' 'lPPA_IOvsOJ_p3'};

%% data type:
dt =    [4 3 4 3]; % motion corrected both within and between
%  scan number
scan = [7 8 8 8]; %
nsbj=length(sessions);

% get some basic params from first session e.g. eventParams
cd( fullfile(expDir, sessions{1}) );
fprintf('*** Session 1 *** \n', sessions{1});
% initialize hidden inplane
hI = initHiddenInplane(dt(1), scan(1));

% params is a struct in which i set the following fields the following fields:
params.dataType=dt;
params.scan=scan;
params.studyDir=expDir;
params.viewType='INPLANE';

% enforce all analysis params to be the same
params.eventParams  = er_getParams(hI);

params.eventParams.ampType='difference'; % params.eventParams.ampType can be one of the following:
                                        % 'difference' or 'betas' or 'relamps' or 'deconvolved';
                                        % this field specifies which amptype will be plotted in the meanamps plot
                                        % and which amps will be calculated in the tc.allMeanAmps and tc.meanAmps
params.eventParams.peakPeriod=[4:12];

params.methodFlag=4; % method to combine Tcs across subjects; Across-sessions analysis ("random effects")
                     % Note that the function tc_acrossSessions
                     % first generates the subjectTcs array which is a
                     % structure array of individual subjects tc
                     % and then calls tc_combineTcs to calculate the across
                     % subject average; Check tc_combineTcs to learn about
                     % methods to combine data across subjects

params.openUI=1; % 1 to open TC UI with average time course across subjects
                 % will open a left panel with mean amps according to params.eventParams.ampType field
                 % and a right panel with the event trigged average time
                 % course (except if
                 % params.eventParams.ampType='deconvolved' in that case
                 % will show the mean deconvolved time course

% set glm and analysis params to be the same across sessions
params.eventParams.detrend = 1; % high-pass filter (low-pass is blurring, below)
params.eventParams.inhomoCorrection = 1; % divide by mean
params.eventParams.temporalNormalization = 0; % no matching 1st temporal frame

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% set HRF function to be the GLM SPM HRF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
params.eventParams.glmHRF = 3; %
% number of events per block
params.eventParams.eventsPerBlock = 6; % see above

params.sessionPlots={'betas' 'meanAmps' 'meantcs'}; % single subject plots - see all options in tc_sessionPlots
params.savePlotsPath='Images';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% loop across sessions
% returns mean tc across subject in the tc variable
% and structure array containing single subject tcs in subjectTcs
% calls tc_combineTcs and tc_sessionPlots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[tc subjectTcs] = tc_acrossSessions(sessions, roi, params);


function llsq_test ( )

%*****************************************************************************80
%
%% LLSQ_TEST tests the LLSQ library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 March 2012
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LLSQ_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the LLSQ library.\n' );

  llsq_test01 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LLSQ_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function a = r8vec_sort_bubble_d ( n, a )

%*****************************************************************************80
%
%% R8VEC_SORT_BUBBLE_D descending sorts an R8VEC using bubble sort.
%
%  Discussion:
%
%    Bubble sort is simple to program, but inefficient.  It should not
%    be used for large arrays.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real A(N), an unsorted array.
%
%    Output, real A(N), the array has been sorted.
%
  for i = 1 : n-1
    for j = i+1 : n
      if ( a(i) < a(j) )
        t    = a(i);
        a(i) = a(j);
        a(j) = t;
      end
    end
  end

  return
end


classdef ParamsMonitor < ParamsMonitor_Interface

    properties (Access = private)
        frame
        figures
        nFigs
        nRows
        nCols

        dualVariable

        problemID
        cost
        nCostFuncs
        constraint
        nConstraints
        refreshInterval
        convergenceVars
        iteration
        hasFinished
        iRefresh
        iStep
        nStep

        namingManager
    end

    methods (Access = public)

        function obj = ParamsMonitor(cParams)
            obj.init(cParams);
            drawnow
        end

        function refresh(obj,it,hasFinished,iStep,nStep)
            obj.updateParams(it,hasFinished,iStep,nStep);
            obj.updateFigures();
            if obj.shallRefresh()
                obj.refreshFigures();
                obj.refreshFrameTitle();
            end
        end

        function f = saveFigure(obj)
            f = obj.frame;
            fNameFig = fullfile('Output',obj.problemID,'Monitoring.fig');
            fNamePng = fullfile('Output',obj.problemID,'Monitoring.png');
            saveas(f,fNameFig)
            saveas(f,fNamePng)
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.problemID = cParams.problemID;
            obj.refreshInterval = cParams.refreshInterval;
            obj.nCostFuncs      = length(cParams.costFuncNames);
            obj.nConstraints    = length(cParams.constraintFuncs);

            obj.dualVariable     = cParams.dualVariable;
            obj.cost             = cParams.cost;
            obj.constraint       = cParams.constraint;
            obj.convergenceVars  = cParams.convergenceVars;
            obj.createNamingManager(cParams);
            obj.createFigures();
        end

        function createFigures(obj)
            obj.figures = {};
            obj.initCostFigures();
            obj.initConstraintFigures();
            obj.initConvergenceVarsFigures();
            obj.initShapeFunctionsVariablesFigures();
            obj.displayFigures();
        end

        function initCostFigures(obj)
            obj.initFigure('Cost');
            for i = 1:obj.nCostFuncs
                title = obj.namingManager.getCostFuncFigureTitle(i);
                obj.initFigure(title);
            end
        end

        function initConstraintFigures(obj)
            for i = 1:obj.nConstraints
                obj.initConstraintFigure(i);
                obj.initLambdaFigure(i);
            end
        end

        function initConstraintFigure(obj,i)
            title = obj.namingManager.getConstraintFigureTitle(i);
            obj.initFigure(title);
        end

        function initLambdaFigure(obj,i)
            title = obj.namingManager.getLambdaFigureTitle(i);
            obj.initFigure(title);
        end

        function initConvergenceVarsFigures(obj)
            for i = 1:obj.convergenceVars.nVar
                title = obj.namingManager.getConvVarNames(i);
                obj.initFigure(title);
            end
        end

        function initShapeFunctionsVariablesFigures(obj)
            obj.initShapeFunctionsVariablesFromFunctional(obj.cost);
            obj.initShapeFunctionsVariablesFromFunctional(obj.constraint);
        end

        function initShapeFunctionsVariablesFromFunctional(obj,functional)
            nShapes = numel(functional.shapeFunctions);
            for i = 1:nShapes
                shape  = functional.shapeFunctions{i};
                titles = shape.getTitlesToPlot();
                for iv = 1:numel(titles)
                    title = titles{iv};
                    obj.initFigure(title);
                end
            end
        end

        function obj = initFigure(obj,title)
            chartType = obj.getChartType(title);
            newFig = DisplayFactory.create(chartType,title);
            obj.appendFigure(newFig);
        end

        function appendFigure(obj,fig)
            obj.figures{end+1} = fig;
        end

        function displayFigures(obj)
            obj.createFrame();
            obj.computeDistribution();
            for i = 1:obj.nFigs
                obj.figures{i}.show(obj.nRows,obj.nCols,i,[0.06 0.03]);
            end
        end

        function createFrame(obj)
            obj.frame = figure;
            drawnow;
            obj.frame.WindowState = "maximized";
        end

        function computeDistribution(obj)
            if obj.nFigs <= 4
                obj.nRows = 1;
            else
                obj.nRows = 2;
            end
            obj.nCols = round(obj.nFigs/obj.nRows);
        end

        function updateParams(obj,it,hasFinished,iStep,nStep)
            obj.iteration = it;
            obj.hasFinished = hasFinished;
            obj.iRefresh = 1;
            obj.iStep = iStep;
            obj.nStep = nStep;
        end

        function updateFigures(obj)
            obj.updateCost();
            obj.updateConstraints();
            obj.updateConvergenceVars();
            obj.updateShapeFunctionsVar();
        end

        function updateCost(obj)
            obj.updateFigure(obj.cost.value);
            for i = 1:obj.nCostFuncs
                shape = obj.cost.shapeFunctions{i};
                value = shape.value;
                obj.updateFigure(value);
            end
        end

        function updateConstraints(obj)
            for i = 1:obj.nConstraints
                shape = obj.constraint.shapeFunctions{i};
                value = shape.value;
                obj.updateFigure(value);
                if ~isempty(obj.dualVariable.value)
                    obj.updateFigure(obj.dualVariable.value(i))
                end
            end
        end

        function updateConvergenceVars(obj)
            for i = 1:obj.convergenceVars.nVar
                if ~isempty(obj.convergenceVars.values(i))
                    obj.updateFigure(obj.convergenceVars.values(i));
                end
            end
        end

        function updateShapeFunctionsVar(obj)
            obj.updateShapeFunctionsVarFromFunctional(obj.cost);
            obj.updateShapeFunctionsVarFromFunctional(obj.constraint);
        end

        function updateShapeFunctionsVarFromFunctional(obj,functional)
            nShapes = numel(functional.shapeFunctions);
            for i = 1:nShapes
                shape  = functional.shapeFunctions{i};
                values = shape.getVariablesToPlot();
                for iv = 1:numel(values)
                    value = values{iv};
                    obj.updateFigure(value);
                end
            end
        end

        function updateFigure(obj,value)
            obj.figures{obj.iRefresh}.updateParams(obj.iteration,value,obj);
        end

        function refreshFigures(obj)
            for iFig = 1:obj.nFigs
                obj.figures{iFig}.refresh();
            end
        end

        function refreshFrameTitle(obj)
            title = obj.namingManager.getFrameTitle(obj.iStep,obj.nStep,obj.iteration);
            set(obj.frame,'NumberTitle','off','Name',title)
        end

        function itShall = shallRefresh(obj)
            itShall = (obj.isTimeToRefresh() || obj.hasFinished) && obj.iteration > 0;
        end

        function itIs = isTimeToRefresh(obj)
            itIs = mod(obj.iteration,obj.refreshInterval) == 0;
        end

        function createNamingManager(obj,cParams)
           s.costFuncNames   = cParams.costFuncNames;
           s.costWeights     = cParams.costWeights;
           s.constraintFuncs = cParams.constraintFuncs;
           s.optimizerNames   = cParams.optimizerNames;
           obj.namingManager = NamingManager(s);
        end

    end

    methods (Access = private, Static)

        function type = getChartType(title)
            switch title
                case {'Line Search','outit','Line Search trials'}
                    type = 'bar';
                case {'kktnorm','Norm L2','inf_{pr}','inf_{du}','C - C not scaled'}
                    type = 'log';
                otherwise
                    type = 'plot';
            end
        end

    end

    methods (Access = ?Display_Abstract)

        function increaseRefreshIterator(obj)
            obj.iRefresh = obj.iRefresh + 1;
        end

    end

    methods

        function n = get.nFigs(obj)
            n = length(obj.figures);
        end

    end

end


function afthelp() % autogenerated function wrapper
%In order to analyse the parameters in the "Modified Omori Law" for aftershock sequences, especially the p-value, please take into account the following:
 % turned into function by Celso G Reyes 2017

ZG=ZmapGlobal.Data; % used by get_zmap_globals

%1. The routines are available from the "Cumulative Number" window
%   or from "Seismicity Map Window" - for mapping of parameters.
%   Thus, the file timeplot.m and
%   update(mainmap()).m have options to call the relevant routines. Besides,
%   the cross-sectional mapping is available from "Cross-section Window".
%2. Before starting analysing the data, please execute the following
%   command in "Matlab Command Window": set(0, 'recursionlimit', 510).
%   It can be any other
%   number greater or equal than 502 instead of 510. The reason for this
%   command is that the routine for determining the parameters in the
%   Omori formula, ploop_c_and_p_calcs.m, works by recursion and the default of
%   Matlab has a maximum of 500 recursive calls.
%3. The file opened for analysis should contain the mainshock. Please
%   be sure to open the "Cumulative Number Window" at least ones before
%   starting any analysis.
%   This is because the mainshock information is contained in the matrix
%   "ZG.maepi", which is not defined until you open the "Cumulative Number
%   Window". After this,
%   please eliminate the mainshock, by filtering the catalog in magnitude.
%4. There are several files which permit the analysis of the parameters in
%   Omori formula. You can modify them on your convenience:
%   - src/declus/pvalcat.m - starts computation of p, c and k and their
%     standard deviations for the catalog displayed in the "Cumulative
%     Number Window".
%     You can choose the threshold magnitude, the starting time (in days)
%     after the mainshock and if the value of c is fixed or not. If you
%     want a fix c, you can
%     choose its value.
%   - src/declus/pvalcat2.m - determines p, c and K for different
%     magnitude thresholds and different starting times and presents them
%     as a map. You have to choose
%     the minimum and maximum threshold magnitude, the minimum and maximum
%     starting time and a step for each of them.
%   - src/bpvalgrid.m - computes the spatial (map) distribution of p, b
%     and Mc values for an aftershock sequence. Interactively you can choose
%     several parameters.
%     You can save your grid of values and use it at a later time.
%   - src/view_bpva.m - plots map of b, p, Mc values, computed before with
%     bpvalgrid.m.
%   - src/pcrossnew - compute the spatial (cross-section) distribution of p,
%     b and Mc values. Interactively you can choose several parameters.
%   - src/view_bpvs.m - plots map of b, p, Mc values, computed before with
%     pcrossnew.m.
%   - src/declus/mypval2m.m - interface between the programmes pvalcat.m,
%     pvalcat2.m or bpvalgrid.m on one hand and ploop_c_and_p_calcs.m
%     other hand.
%   - src/declus/ploop2.m - this is the routine which computes the p,c and
%     K values in the "Modified Omori Law".
%   - src/declus/ploop3.m - the same as above, but c is fixed (0 or any
%     other value can be chosen). However, if c = 0, ts should not be 0,
%     because when the time
%     approaches 0 the frequency tends to infinity. Please check the routine
%     mypval2m.m to verify ts, the minimum time in the aftershock sequence.
%   - src/adju2.m - filters the results displayed on the map (for spatial
%     variation of parameters) in function of different parameters, including
%     the standard error
%     in p. It is called from src/view_bpva.m.
%   - src/timabs.m - computes the number of minutes from a reference date.
%   - src/afthlp - opens this file (afthelp.m).
%   - afthelp.m - file with explanations regarding the analysis of aftershock
%     sequences (this file).
%5.  Please read the comments in the above mentioned files for further instructions.

% Bogdan Enescu, 5/2001.

%   References:

%  Ogata Y., Estimation of the parameters in the Modified Omori Formula for aftershock sequencies by the maximum likelihood procedure, J. Phys. Earth, 31, 115-
%            -124, 1983.
%  Wiemer, S. and Katsumata, K., Spatial variability of seismicity parameters in aftershock zones, J. Geophys. Res., 104, B6, 13135-13151, 1999.
%  Utsu, T, Ogata, Y., and Matsu'ura, R.S., The centenary of the Omori Formula for a Decay Law of Aftershock Activity., J. Phys. Earth, 43, 1-33, 1995.
%  Reasenberg, P.A., Lucile M. Jones, Earthquake hazard after a mainshock in California, Science, 243, 1173-1176, 1989.
%  Reasenberg, P.A., Lucile M. Jones, California Aftershock Hazard Forecasts, Science, 247, 345-346, 1990.
%  Reasenberg, P.A., Lucile M. Jones, Earthquake Aftershocks: Update, Science, 265, 1251-1252, 1994.
%  Ogata, Y., Seismicity analysis through point-process modelling: a review, Pure Appl. Geophys., 155, 471-507, 1999.

end


classdef CoherentOrientationSelector < handle

    properties (Access = private)
       mesh
       orientation
    end

    methods (Access = public)

        function obj = CoherentOrientationSelector(cParams)
            obj.init(cParams);
        end

        function isCoherent = isOrientationCoherent(obj)
            nnode   = obj.mesh.nnodeElem;
            nElem   = obj.mesh.nelem;
            isCoh   = false(1,nnode,nElem);
            orient  = obj.orientation.fValues;
            bN1 = squeeze(orient(:,1,:));
            for iNode = 1:nnode
                bNi = squeeze(orient(:,iNode,:));
                bN1bNI= dot(bN1,bNi);
                isCoh(1,iNode,:) = (bN1bNI)>0;
            end
            s.fValues  = isCoh;
            s.mesh     = obj.mesh;
            isCoherent = P1DiscontinuousFunction(s);
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.mesh        = cParams.mesh;
            obj.orientation = cParams.orientation;
        end

    end

end


function [IPPEPoses,refinedPoses] = perspectiveIPPE(U,Q,hEstMethod,opts)
%perspectiveIPPE: The solution to Perspective IPPE with point correspondences computed
%between points in world coordinates on the plane z=0, and normalised points in the
%camera's image.
%
%Inputs:
%
%U: 2xN or 3xN matrix holding the model points in world coordinates. If U
%is 2xN then the points are defined in world coordinates on the plane z=0
%
%Q: 2xN matrix holding the points in the image. These are in normalised
%pixel coordinates. That is, the effects of the camera's intrinsic matrix
%and lens distortion are corrected, so that the Q projects with a perfect
%pinhole model.
%
%hEstMethod: the homography estimation method, either 'Harker' (default) or 'DLT'. If
%'Harker' then the method of Harker and O'Leary is used, from the paper
%"Computation of Homographies" in the 2005 British Computer Vision
%Conference. Otherwise the Direct Linear Transform is used, from Peter Kovesi's implementation at http://www.csse.uwa.edu.au/~pk.
%If hEstMethod=[] then the default is used.
%
%opts is an optional datastructure holding the fields:
%setup IPPE options:
%opts.withPoseRefinement = true; %If this is set, we also refine the camera pose from IPPE by gradient-based optimisation (Levenberg–Marquardt). This is designed to give the statistically optimal pose assuming zero mean I.I.D nose for the feature's 2D positions, but requires initialisation (from IPPE). Note that it about two orders of magnitude slower than IPPE.
%opts.displayTiming = true; %set this to true if you want to see how long IPPE takes in Matlab.
%
%Outputs:
%IPPEPoses: structure containing the two pose solutions from IPPE.
%   IPPEPoses.R1 is the first pose's rotation
%   IPPEPoses.t1 is the first pose's translation

%   IPPEPoses.R2 is the second pose's rotation
%   IPPEPoses.t2 is the second pose's translation

%   IPPEPoses.reprojError1 is the first pose's reprojection error
%   IPPEPoses.reprojError2 is the second pose's reprojection error
%
%refinedPoses: structure containing the two refined pose solutions using
%Levenberg–Marquardt (same format as IPPEPoses). This is only outputted if opts.withPoseRefinement = true
%
%Note that the poses are transforms from world
%coordinates to camera coordinates. If you
%want to get the camera's pose in world coordinates you would get it with
%M1 = inv([[R1,t1];[0,0,0,1]]) and M2 = inv([[R2,t2];[0,0,0,1]])
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of the IPPE package for very fast plane-based pose
% estimation from a single perspective image, from the paper "Infinitesimal Plane-based Pose Estimation"
% by Toby Collins and Adrien Bartoli, published in the International Journal of Computer Vision, September 2014.
% A copy of the author's pre-print version can be found here: http://isit.u-clermont1.fr/~ab/Publications/Collins_Bartoli_IJCV14.pdf
%
% Feel free to contact Toby (toby.collins@gmail.com) if you have any
% questions about the paper and IPPE.
%
% This package is free and covered by the BSD licence without any warranty. We hope you find this code useful and please cite our paper in your work:
% (c) Toby Collins 2015
%
%@article{
%year={2014},
%issn={0920-5691},
%journal={International Journal of Computer Vision},
%volume={109},
%number={3},
%doi={10.1007/s11263-014-0725-5},
%title={Infinitesimal Plane-Based Pose Estimation},
%url={http://dx.doi.org/10.1007/s11263-014-0725-5},
%publisher={Springer US},
%keywords={Plane; Pose; SfM; PnP; Homography},
%author={Collins, Toby and Bartoli, Adrien},
%pages={252-286},
%language={English}
%}

opts = parseOpts(opts);

measureTiming = opts.measureTiming;
refineSolutions = opts.withPoseRefinement;

U = double(U);
Q = double(Q);

%set defaults for hEstMethod:
if isempty(hEstMethod)
    hEstMethod = 'Harker';
else
    if ~(strcmp(hEstMethod,'Harker')||strcmp(hEstMethod,'DLT'))
        error('You must specify a valid homography estimation method in perspectiveIPPE. The options are Harker or DLT');
    end
end

%argument sanity check:
assert(size(U,1)==2|size(U,1)==3);
assert(size(U,2)==size(Q,2));
assert(size(Q,1) == 2);

%n is the number of points:
n = size(U,2);
modelDims = size(U,1);

Uuncentred = U;

if modelDims==2
   %we zero-center the model points:
    Pbar = [mean(U,2);0];
    U(1,:) = U(1,:)-Pbar(1);
    U(2,:) = U(2,:)-Pbar(2);
else
    %we rotate the model points onto the plane z=0 and zero center them:
    Pbar = mean(U,2);
    MCenter = eye(4);
    MCenter(1:3,end) = -Pbar;
    U_ = MCenter(1:3,:)*[U;ones(1,size(U,2))];
    %compute the rotation that rotates the model points onto the plane z=0:
    [modelRotation,sigs,~] = svd(U_*U_'); modelRotation=modelRotation';
    if det(modelRotation)<0
        modelRotation(3,:)  = -modelRotation(3,:); %this ensures modelRotation is a member of SO3
    end
    if (sigs(3,3)/sigs(2,2)>1e-5)
        error('IPPE requires the model points to be planar!');
    end

    modelRotation(4,4) = 1;
    Mcorrective = modelRotation*MCenter;
    U = Mcorrective(1:2,:)*[U;ones(1,size(U,2))];

end

if measureTiming
    tic;
end
%compute the model-to-image homography:
switch hEstMethod
    case 'DLT'
        H = homography2d([U;ones(1,n)],[Q;ones(1,n)]);
    case 'Harker'
        H = homographyHarker([U;ones(1,n)],[Q;ones(1,n)]);
end

%[ px, py, 1, 0, 0, 0, -px*qx, -py*qx]h = qx
%[ 0, 0, 0, px, py, 1, -px*qy, -py*qy]h = qy

%Compute the Jacobian J of the homography at (0,0):
H = H./H(3,3);
J(1,1) = H(1,1)-H(3,1)*H(1,3);
J(1,2) = H(1,2)-H(3,2)*H(1,3);
J(2,1) = H(2,1)-H(3,1)*H(2,3);
J(2,2) = H(2,2)-H(3,2)*H(2,3);

%Compute the two rotation solutions:
v = [H(1,3);H(2,3)];
[R1,R2] = IPPE_dec(v,J);

%Compute the two translation solutions:
t1_ = estT(R1,U,Q);
t2_ = estT(R2,U,Q);

if modelDims==2
    t1 = [R1,t1_]*[-Pbar;1];
    t2 = [R2,t2_]*[-Pbar;1];
else
    M1 = [R1,t1_];
    M1(4,4) = 1;
    M2 = [R2,t2_];
    M2(4,4) = 1;
    M1 = M1*Mcorrective;
    M2 = M2*Mcorrective;
    R1 = M1(1:3,1:3);
    R2 = M2(1:3,1:3);
    t1 = M1(1:3,end);
    t2 = M2(1:3,end);
end

[reprojErr1,reprojErr2] = computeReprojErrs(R1,R2,t1,t2,Uuncentred,Q);
%sort solutions:
if reprojErr1>reprojErr2
    [R1,R2,t1,t2,reprojErr1,reprojErr2] = swapSolutions(R1,R2,t1,t2,reprojErr1,reprojErr2);
end
IPPEPoses.R1 = R1;
IPPEPoses.t1 = t1;
IPPEPoses.R2 = R2;
IPPEPoses.t2 = t2;
IPPEPoses.reprojError1 = reprojErr1;
IPPEPoses.reprojError2 = reprojErr2;

%%with timing?
if measureTiming
    disp('Computing time to estimate homography for IPPE. Warning this slows things down, so turn it off when you are not benchmarking!');
    tic;
    %average timing over 100 runs:
    for i=1:100
        switch hEstMethod
            case 'DLT'
                H = homography2d([U;ones(1,n)],[Q;ones(1,n)]);
            case 'Harker'
                H = homographyHarker([U;ones(1,n)],[Q;ones(1,n)]);
        end
    end

    tm = toc;
    disp(['Time to estimate homography: ' num2str(1000*tm/100) 'ms']);

    disp('Computing time to solve IPPE given homography...');
    tic;
    %average timing over 100 runs:
    for i=1:100
        %Compute the Jacobian J of the homography at (0,0):
        H = H./H(3,3);
        J(1,1) = H(1,1)-H(3,1)*H(1,3);
        J(1,2) = H(1,2)-H(3,2)*H(1,3);
        J(2,1) = H(2,1)-H(3,1)*H(2,3);
        J(2,2) = H(2,2)-H(3,2)*H(2,3);
        %Compute the two rotation solutions:
        v = [H(1,3);H(2,3)];
        [R1,R2] = IPPE_dec(v,J);
        %Compute the two translation solutions:
        t1_ = estT(R1,U,Q);
        t2_ = estT(R2,U,Q);
        t1 = [R1,t1_]*[-Pbar;1];
        t2 = [R2,t2_]*[-Pbar;1];
    end
    tm = toc;
    disp(['Time to solve IPPE given homography: ' num2str(1000*tm/100) 'ms']);
end

if modelDims==2
    Uuncentred(3,:) = 0;
end

if refineSolutions
    poseEst= pnpLMRefinement(Uuncentred,Q,R1,t1,'MLE');
    R1 = poseEst.R;
    t1 = poseEst.t;

    poseEst= pnpLMRefinement(Uuncentred,Q,R2,t2,'MLE');
    R2 = poseEst.R;
    t2 = poseEst.t;

    [reprojErr1,reprojErr2] = computeReprojErrs(R1,R2,t1,t2,Uuncentred,Q);
    %Sort the solutions:
    if reprojErr1>reprojErr2
        [R1,R2,t1,t2,reprojErr1,reprojErr2] = swapSolutions(R1,R2,t1,t2,reprojErr1,reprojErr2);
    end
    refinedPoses.R1 = R1;
    refinedPoses.t1 = t1;
    refinedPoses.R2 = R2;
    refinedPoses.t2 = t2;
    refinedPoses.reprojError1 = reprojErr1;
    refinedPoses.reprojError2 = reprojErr2;

    %%with timing?
    if measureTiming
        disp('Computing time to refine IPPE solutions with Levenberg Marquardt...');
        tic;
        %average timing over 100 runs:
        for i=1:100
            pnpLMRefinement(Uuncentred,Q,IPPEPoses.R1,IPPEPoses.t1,'MLE');
            pnpLMRefinement(Uuncentred,Q,IPPEPoses.R2,IPPEPoses.t2,'MLE');
        end
        tm = toc;
        disp(['Time to refine IPPE solutions with Levenberg Marquardt: ' num2str(1000*tm/100) 'ms']);
    end

end

function [reprojErr1,reprojErr2] = computeReprojErrs(R1,R2,t1,t2,U,Q)
%computeReprojErrs: Computes the reprojection errors for the two solutions
%generated by IPPE.

%transform model points to camera coordinates and project them onto the
%image:
if size(U,1)==2
    PCam1 = R1(:,1:2)*U;
    PCam2 = R2(:,1:2)*U(1:2,:);
else
    PCam1 = R1*U;
    PCam2 = R2*U;
end

PCam1(1,:) = PCam1(1,:) + t1(1);
PCam1(2,:) = PCam1(2,:) + t1(2);
PCam1(3,:) = PCam1(3,:) + t1(3);

PCam2(1,:) = PCam2(1,:) + t2(1);
PCam2(2,:) = PCam2(2,:) + t2(2);
PCam2(3,:) = PCam2(3,:) + t2(3);

Qest_1 = PCam1./[PCam1(3,:);PCam1(3,:);PCam1(3,:)];
Qest_2 = PCam2./[PCam2(3,:);PCam2(3,:);PCam2(3,:)];

%compute reprojection errors:
reprojErr1 = norm(Qest_1(1:2,:)-Q);
reprojErr2 = norm(Qest_2(1:2,:)-Q);

function t = estT(R,psPlane,Q)
%Computes the least squares estimate of translation given the rotation solution.
if size(psPlane,1)==2
    psPlane(3,:) =0;
end
%qq = homoMult(Kinv,pp')';
Ps = R*psPlane;

numPts = size(psPlane,2);
Ax = zeros(numPts,3);
bx = zeros(numPts,1);

Ay = zeros(numPts,3);
by = zeros(numPts,1);

Ax(:,1) = 1;
Ax(:,3) = -Q(1,:);
bx(:) = Q(1,:).*Ps(3,:) -  Ps(1,:);

Ay(:,2) = 1;
Ay(:,3) = -Q(2,:);
by(:) = Q(2,:).*Ps(3,:) -  Ps(2,:);

A = [Ax;Ay];
b = [bx;by];

AtA = A'*A;
Atb = A'*b;

Ainv = IPPE_inv33(AtA);
t = Ainv*Atb;

function [R1,R2,t1,t2,reprojErr1,reprojErr2] = swapSolutions(R1_,R2_,t1_,t2_,reprojErr1_,reprojErr2_)
%swaps the solutions:
R1 = R2_;
t1 = t2_;
reprojErr1 = reprojErr2_;

R2 = R1_;
t2 = t1_;
reprojErr2 = reprojErr1_;

function Ainv = IPPE_inv33(A)
%computes the inverse of a 3x3 matrix, assuming it is full-rank.
a11 = A(1,1);
a12 = A(1,2);
a13 = A(1,3);

a21 = A(2,1);
a22 = A(2,2);
a23 = A(2,3);

a31 = A(3,1);
a32 = A(3,2);
a33 = A(3,3);

Ainv = [[ a22*a33 - a23*a32, a13*a32 - a12*a33, a12*a23 - a13*a22];
    [ a23*a31 - a21*a33, a11*a33 - a13*a31, a13*a21 - a11*a23];
    [ a21*a32 - a22*a31, a12*a31 - a11*a32, a11*a22 - a12*a21]];
Ainv = Ainv./(a11*a22*a33 - a11*a23*a32 - a12*a21*a33 + a12*a23*a31 + a13*a21*a32 - a13*a22*a31);

function opts = parseOpts(opts)
if ~isfield(opts,'measureTiming')
    opts.measureTiming = false;
end
if ~isfield(opts,'withPoseRefinement')
    opts.withPoseRefinement = true;
end


function [s,it]=display(d,tab,it)

if nargin>1
    [s,it]=display(d.algorithm,tab,it);
    return;
end

disp(d.algorithm.name);
s=(['X = ' num2str(size(d.X,1)) 'x' num2str(size(d.X,2))  ]);
% if size(d.X,1)<=10 & size(d.X,2)<=10  & size(d.Y,1)<=10 & size(d.Y,2)<=10
%     if size(d.X,2)==1 & size(d.Y,2)==1
%         if ~isstruct(d.X(1,1))
%             disp('     X     Y'); disp([d.X d.Y]);
%         end
%     else
%         if ~isempty(d.X) & prod(size(d.X))>1  disp('X:'); disp(d.X); end
%         if ~isempty(d.Y) & prod(size(d.X))>1  disp('Y:'); disp(d.Y); end
%     end
%  else
     disp(['data dimensions:'])
    disp([s '  Y = ' num2str(size(d.Y,1)) 'x' num2str(size(d.Y,2)) ]);
% end


filename='Cantileverbeam_Hexahedra_Linear_Structured_SYM';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'holes';
cost = {'compliance'};
weights = [1];
constraint = {'volume'};
optimizer = 'HAMILTON-JACOBI';
incrementFactor = 1;
designVariable = 'LevelSet';
filterType = 'P1';
constraint_case = 'INEQUALITY';
line_search_initiator = 'INCREASING LAST STEP';

nsteps = 10;
Vfrac_final = 0.15;
Perimeter_target=3.5;
optimality_final =1e-4;
constr_final =1e-4;

BCscale_factor = 0.3;
rotation_per_it = 20;
HJiter0 = 1;
e2 = 50;
N_holes = [8 4 4];
R_holes = 0.2;
phase_holes = [0 0 0];

Vfrac_initial = 0.7;
optimality_initial = 5e-2;
constr_initial = 1e-3;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;

TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;

TOL.nu_plus = 0.3;
TOL.nu_minus = 0.3;


function h = som_barplane(varargin)

%SOM_BARPLANE Visualize the map prototype vectors as bar charts
%
% h = som_barplane(lattice, msize, data, [color], [scaling], [gap], [pos])
% h = som_barplane(topol, data, [color], [scaling], [gap], [pos])
%
%  som_barplane('hexa',[5 5], rand(25,4), jet(4))
%  som_barplane(sM, sM.codebook,'none')
%
%  Input and output argumetns ([]'s are optional):
%   lattice   (string) grid 'hexa' or 'rect'
%   msize     (vector) size 1x2, defines the map grid size msize, M=prod(msize)
%             (matrix) size Mx2, gives explicit coordinates for each node:
%                      in this case the first argument does not matter.
%   topol     (struct) map or topology struct
%   data      (matrix) size Mxd, each row defines heights of the bars
%   [color]   (matrix) size dx3, of RGB triples. The rows define colors
%                      for each bar in a node. Default is hsv(d). A ColorSpec or
%             (string) A ColorSpec or 'none' gives each bar the same color.
%   [scaling] (string) 'none', 'unitwise' or 'varwise'. The scaling
%                      mode for the values. Default is 'varwise'.
%   [gap]     (scalar) Defines the gap between bars, limits: 0 <= gap <= 1
%                      where 0=no gap, 1=bars are thin lines. Default is 0.25.
%   [pos]     (vector) 1x2 vector defines the position of origin.
%                      Default is [1 1].
%
%   h         (scalar) the object handle to the PATCH object
%
% Axis are set as in SOM_CPLANE.
%
% For more help, try 'type som_barplane' or check out online documentation.
% See also SOM_CPLANE, SOM_PLOTPLANE, SOM_PIEPLANE.

%%%%%%%%%%%%% DETAILED DESCRIPTION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% som_barplane
%
% PURPOSE
%
% Visualizes the map prototype vectors as bar charts.
%
% SYNTAX
%
%  h = som_barplane(topol, data)
%  h = som_barplane(lattice, msize, data)
%  h = som_barplane(..., color)
%  h = som_barplane(..., color, scaling)
%  h = som_barplane(..., color, scaling, gap)
%  h = som_barplane(..., color, scaling, gap, pos)
%
% DESCRIPTION
%
% Visualizes the map prototype vectors as bar charts.
%
% REQUIRED INPUT ARGUMENTS
%
% lattice     The basic shape of the map units
%    (string) 'hexa' or 'rect' positions the bar charts according to
%             hexagonal or rectangular map lattice
%
% msize       The size of the map grid
%    (vector) [n1 n2] vector defines the map size (height: n1 units widht: n2
%             units, total: M=n1xn2 units). The units will be placed to their
%             topological locations in order to form a uniform hexagonal or
%             rectangular grid.
%    (matrix) Mx2 matrix defines arbitary coordinates for the N units. In
%             this case the argument 'lattice' has no effect
%
% topol    Topology of the map grid
%
%   (struct) map or topology struct from which the topology is taken
%
% data        The data to use when constructing the bar charts.
%             Typically, the map codebook or some of its components.
%    (matrix) Mxd matrix. A row defines heights of the bars.
%
% OPTIONAL INPUT ARGUMENTS
%
% Note: if unspecified or given an empty value ('' or []), default
% values are used for optional input arguments.
%
% color       The color of the bars in each pie
%    (ColorSpec) or (string) 'none' gives the same color for each slice.
%    (matrix) dx3 matrix assigns an RGB color determined by the dth row of
%             the matrix to the dth bar (variable) in each bar plot.
%             Default value is hsv(d).
%
% scaling     How to scale the values
%    (string) 'none', 'unitwise' or 'varwise'. This determines the
%             scaling of codebook values when drawing the bars.
%
%             'none' don't scale at all. The bars are not limited
%              to remain inside he units' area: That is, if value of
%              some variable exceeds [-.625,.625] for 'rect' (and
%              in "worst case" [-.5,-.5] for 'hexa') the bars may
%              overlap other units.
%
%              Base line (zero value line)
%              - is in the middle of the unit if data (codebook) contains both
%                negative and positive values (or is completely zero).
%              - is in the top the unit if data (codebook) contains only
%                non-positive values (everything <=0).
%              - is in the bottom the unit if data (codebook) contains only
%                non-negative values (everything >=0).
%
%             'varwise' scales values so that each variable is scaled separately
%              so that when it gets its overall maximum value, the
%              corresponding bar gets maximum range and for minimum value
%              it gets the minimum range. Baseline: see scaling 'none'
%              This is the default.
%
%             'unitwise' scales values in each unit individually so that the
%              bars for variables having minimum and maximum values have minimum
%              and maximum range inside each unit, respectively.
%              In this case the zero value line may move depending on the values.
%

% gap         The gap between bars
%    (scalar) 0: no gap: bars are glued together
%             ... default value is 0.25
%             1: maximum gap: bars are thin lines
%
% pos         Position of origin
%    (vector) size 1x2. This is meant for drawing the plane in arbitrary
%             location in a figure. Note the operation: if this argument is
%             given, the axis limits setting part in the routine is skipped and
%             the limits setting will be left to be done by MATLAB's defaults.
%             Default is [1 1].
%
% OUTPUT ARGUMENTS
%
%  h (scalar) handle to the created patch object
%
% OBJECT TAGS
%
%  One object handle is returned: field Tag is set to 'planeBar'
%
% FEATURES
%
%  - The colors are fixed: changing colormap in the figure (see help
%    colormap) will not change the coloring of the bars.
%
% EXAMPLES
%
% %%% Create the data and make a map
%
% data=rand(100,5); map=som_make(data);
%
% %%% Create a 'jet' colormap that has as many rows as the data has variables
%
% colors=jet(5);
%
% %%% Draw bars
%
% som_barplane(map.topol.lattice, map.topol.msize, map.codebook, colors);
% or som_barplane(map.topol, map.codebook, colors);
% or som_barplane(map, map.codebook, colors);
%
% %%% Draw the bars so that the gap between the bars is bigger and all
%     bars are black
%
% som_barplane(map, map.codebook, 'k', '', 0.6);
%
% SEE ALSO
%
% som_cplane     Visualize a 2D component plane, u-matrix or color plane
% som_plotplane  Visualize the map prototype vectors as line graphs
% som_pieplane   Visualize the map prototype vectors as pie charts

% Copyright (c) 1999-2000 by the SOM toolbox programming team.
% http://www.cis.hut.fi/projects/somtoolbox/

% Version 2.0beta Juha P 110599, Johan 140799, juuso 151199 140300 070600

%%% Check & Init arguments %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nargin, lattice, msize, data, color, scaling, gap, pos] = vis_planeGetArgs(varargin{:});
error(nargchk(3, 7, nargin))   % check that no. of input args is correct

% Check pos

if nargin < 7 || isempty(pos)
  pos=NaN;                            % default value for pos (no translation)
elseif ~vis_valuetype(pos,{'1x2'})
  error('Position of origin has to be given as an 1x2 vector');
end

% Check gap

if nargin < 6 || isempty(gap),
  gap=0.25;                           % default value for gap
elseif ~vis_valuetype(gap, {'1x1'}),
  error('Gap value must be scalar.');
elseif ~(gap >= 0 && gap<=1)
  error('Gap value must be in interval [0,1].')
end

% Check scaling

if nargin < 5 || isempty(scaling),
  scaling='varwise';
elseif ~vis_valuetype(scaling,{'string'}) || ...
      ~any(strcmp(scaling,{'none','unitwise','varwise'})),
  error('scaling sholud be ''none'', ''unitwise'' or ''varwise''.');
end

% Check msize

if ~vis_valuetype(msize,{'1x2','nx2'}),
  error('msize has to be 1x2 grid size vector or a Nx2 coordinate matrix.');
end

% Check data

if ~isnumeric(data),
  error('Data matrix has to be numeric.');
elseif length(size((data)))>2
  error('Data matrix has too many dimensions!');
else
  d=size(data,2);
  N=size(data,1);
end

s=.8;                                 % patch size scaling factor

switch scaling,
case 'none'
  % no scaling: don't scale
  % Check data max and min values
  positive=any(data(:)>0); negative=any(data(:)<0);
  if (positive && negative) || (~positive && ~negative),
    % Data contains both negative and positive values (or is
    % completely zero) baseline to centre
    zeroline='zero';
  elseif positive && ~negative
    % Data contains only positive values: baseline to bottom
    zeroline='bottom';
  elseif ~positive && negative
    % Data contains only negative values: baseline to top
    zeroline='top';
  end
case 'unitwise'
  % scale the variables so that the bar for variable with the maximum
  % value in the unit spans to the upper edge of the unit
  % and the bar for the variable with minimum value spans to the lower edge,
  % respectively.
  zeroline='moving';
 case 'varwise'
  % Check data max and min values
  positive=any(data(:)>0); negative=any(data(:)<0);
  if (positive && negative) || (~positive && ~negative),
    % Data contains both negative and positive values (or is
    % completely zero) baseline to
    % centre, scale data so that it doesn't overflow
    data=data./repmat(max(abs([max(data); min(data)])),N,1)*.5;
    zeroline='zero';
  elseif positive && ~negative
    % Data contains only positive values: baseline to
    % bottom, scale data so that it doesn't overflow
    data=data./repmat(max(abs([max(data); min(data)])),N,1)*.5;
    zeroline='bottom';
  elseif ~positive && negative
    % Data contains only negative values: baseline to
    % top, scale data so that it doesn't overflow
    zeroline='top';
    data=data./repmat(max(abs([max(data); min(data)])),N,1)*.5;
  end
otherwise
  error('Unknown scaling mode?');
end

for i=1:N,                            % calculate patch coordinates for
  v=data(i,:);
  [nx,ny]=vis_barpatch(v,gap,zeroline); % bars
  barx(:,(1+(i-1)*d):(i*d))=s*nx;
  bary(:,(1+(i-1)*d):(i*d))=s*ny;
end
l=size(barx,1);

if size(msize,1) == 1,
  xdim=msize(2);
  ydim=msize(1);
  if xdim*ydim~=N
    error('Data matrix has wrong size.');
  else
    y=reshape(repmat(1:ydim,d,1),1,d*ydim); y=repmat(repmat(y,l,1),1,xdim);
    x=reshape(repmat(1:xdim,l*ydim*d,1),l,N*d);
  end
else
  x=reshape(repmat(msize(:,1),1,l*d)',l,d*N);
  y=reshape(repmat(msize(:,2),1,l*d)',l,d*N);
  if N ~= size(msize,1),
    error('Data matrix has wrong size.');
  else
    lattice='rect';
    if isnan(pos),
      pos=[0 0];
    end
  end
end

% Check lattice

if ~ischar(lattice)
  error('Invalid lattice.');
end

switch lattice
case {'hexa','rect'}
  pos=pos-1;
otherwise
  error([ 'Lattice' lattice ' not implemented!']);
end

% Check color
% C_FLAG is for color 'none'

if nargin < 4 || isempty(color)
  color=hsv(d);                       % default n hsv colors
end
if ~vis_valuetype(color, {[d 3],'nx3rgb'},'all') && ...
  ~vis_valuetype(color,{'colorstyle','1x3rgb'})
error('The color matrix has wrong size or has invalid values.');
elseif ischar(color) && strcmp(color,'none')
  C_FLAG=1;
  color='w';
else
  C_FLAG=0;
end

%% Action %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Making lattice.
% Command view([0 90]) shows the map in 2D properly oriented

switch lattice
  case 'hexa'
   t=find(rem(y(1,:),2));                        % move even rows by .5
   x(:,t)=x(:,t)-.5;
   x=x+barx+.5;
   y=y+bary;
  case 'rect'
   x=x+barx;
   y=y+bary;
end

% NB: The coordinates in hexa are not uniform in order to get even
% y-coordinates for the nodes. This is handled by setting _axis scaling_
% so that the hexa-nodes look like uniform hexagonals. See
% vis_PlaneAxisProperties

if ~isnan(pos)
  x=x+pos(1);y=y+pos(2);               % move upper left corner
end                                    % to pos

%% Set axes properties

ax=newplot;                            % get current axis
vis_PlaneAxisProperties(ax,lattice, msize, pos);

%% Rearrange dx3 color matrix

if ~ischar(color) && size(color,1)~=1,
  color=reshape(repmat(color,N,1),[1 N*d 3]);
end

%% Draw the plane!

if isnumeric(color),
  % explicit color settings by RGB-triplets won't work with
  % patch in 'painters' mode, unless there only a single triplet
  si = size(color);
  if length(si)~=2 || any(si==[1 3]), set(gcf,'renderer','zbuffer'); end
end

h_=patch(x,y,color);

if C_FLAG
  set(h_,'FaceColor','none');
end

set(h_,'Tag','planeBar');              % tag the object

%%% Build output %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargout>0, h=h_; end                % Set h only if
                                       % there really is output

%%% Subfunctions %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xcoord,ycoord]=vis_barpatch(y,gap,zeroline)

x = length(y);
d = gap/(2*(x-1)+2);
step= -.5:1/x:.5;

miny=min(y);
maxy=max(y);

switch(zeroline)
 case 'moving'
  if miny < 0
    if maxy > 0
      zl = .5 - (abs(miny)/(maxy-miny));    %reverse mode
      y= .5 - ((y-miny*ones(1,x))./(maxy-miny));
    else
      zl = -.5;
      y=-.5+abs(y./miny);
    end
  else
    zl = .5;    %reverse mode
    y=.5-y./maxy;
  end
 case 'moveNotScale'
  if miny < 0
    if maxy > 0
      zl = 0.5+miny;
      y = zl - y;
    else
      zl=-.5;
      y=-.5+abs(y);
    end
  else
    zl=.5;
    y =.5-y;
  end
 case 'zero'
  zl=0; y=zl-y;
 case 'top'
  zl=-.5; y=zl-2*y;
 case 'bottom'
  zl=.5; y=zl-2*y;
end

for i=1:x
  xcoord(:,i) = [d+step(i);d+step(i);step(i+1)-d;step(i+1)-d;d+step(i)];
  ycoord(:,i) = [zl;y(i);y(i);zl;zl];
end


function varargout = lbp(varargin)
% VL_LBP  Local Binary Patterns
%   F = VL_LBP(IM, CELLSIZE) computes the Local Binary Pattern (LBP)
%   features for image I.
%
%   IM is divided in cells of size CELLSIZE. F is a three-dimensional
%   array containing one histograms of quantized LBP features per
%   cell. The witdh of F is FLOOR(WIDTH/CELLSIZE), where WIDTH is the
%   width of the image. The same for the height. The third dimension
%   is 58.
%
%   See also: VL_HELP().
[varargout{1:nargout}] = vl_lbp(varargin{:});


% SP_TO_VTK: Export to VTK format for plotting.
%
%  sp_to_vtk (u, space, geometry, npts, filename, fieldname, [options], [lambda_lame, mu_lame])
%  sp_to_vtk (u, space, geometry, pts, filename, fieldname, [options], [lambda_lame, mu_lame])
%
% INPUT:
%
%     u:           vector of dof weights
%     space:       object representing the space of discrete functions (see sp_vector)
%     geometry:    geometry structure (see geo_load)
%     npts:        number of points along each parametric direction where to evaluate
%     pts:         cell array with the coordinates along each parametric direction of the points where to evaluate
%     filename:    name of the output file.
%     fieldnames:  how to name the saved variables in the vtk file
%     options:     cell array with the fields to plot
%                   accepted options are 'value' (default), 'gradient',
%                   and for vectors also 'curl', 'divergence', 'stress'
%     lambda_lame: function handle to the first Lame coefficient (only needed to compute 'stress')
%     mu_lame:     function handle for the second Lame coefficient (only needed to compute 'stress')
%
% OUTPUT:
%
%    none
%
% Copyright (C) 2009, 2010 Carlo de Falco
% Copyright (C) 2011, 2012, 2015 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

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

function sp_to_vtk (u, space, geometry, npts, filename, fieldname, varargin)

  [eu, F] = sp_eval (u, space, geometry, npts, varargin{:});

  msh_to_vtk (F, eu, filename, fieldname);

end


function pot = mpot(members, sizes, logp, mu, Sigma)
% MPOT Make a moment Gaussian potential.
% pot = mpot(members, sizes, logp, mu, Sigma)
%
% All params default to 0 if omitted.

n = sum(sizes);
if nargin < 3, logp = 0; end
if nargin < 4, mu = zeros(n,1); end
if nargin < 5, Sigma = zeros(n,n); end

pot.domain = members;
pot.sizes = sizes;
pot.logp = logp;
pot.mu = mu;
pot.Sigma = Sigma;zeros(n,n);
pot = class(pot, 'mpot');


function [p1,p2,dp1,dp2] = autoGen_cartPoleKinematics(x,q,dx,dq,l,empty)
%AUTOGEN_CARTPOLEKINEMATICS
%    [P1,P2,DP1,DP2] = AUTOGEN_CARTPOLEKINEMATICS(X,Q,DX,DQ,L,EMPTY)

%    This function was generated by the Symbolic Math Toolbox version 6.3.
%    30-Nov-2015 20:13:07

p1 = [x;empty];
if nargout > 1
    t2 = cos(q);
    t3 = sin(q);
    p2 = [x+l.*t3;-l.*t2];
end
if nargout > 2
    dp1 = [dx;empty];
end
if nargout > 3
    dp2 = [dx+dq.*l.*t2;dq.*l.*t3];
end


function tapas_hgf_ar1_binary_mab_plotTraj(r)
% Plots the estimated or generated trajectories for the binary HGF perceptual model for multi-armed
% bandit situations.
%
% Usage example:  est = tapas_fitModel(responses, inputs); tapas_hgf_binary_plotTraj(est);
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 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/>.

% Optional plotting of standard deviations (true or false)
plotsd = true;

% Optional plotting of responses (true or false)
ploty = true;

% Set up display
scrsz = get(0,'screenSize');
outerpos = [0.2*scrsz(3),0.2*scrsz(4),0.8*scrsz(3),0.8*scrsz(4)];
figure(...
    'OuterPosition', outerpos,...
    'Name', 'HGF trajectories');

% Set up colors
colors = [1 0 0; 0.67 0 1; 0 0.67 1; 0.67 1 0];

% Number of bandits
b = r.c_prc.n_bandits;

% Number of trials
n = size(r.u,1);

% Time axis
t = ones(1,n);

ts = cumsum(t);
ts = [0, ts];

% Number of levels
try
    l = r.c_prc.n_levels;
catch
    l = (length(r.p_prc.p)+1)/5;
end

% Upper levels
for j = 1:l-2

    % Subplots
    subplot(l,1,j);

    if plotsd == true
        upperprior = r.p_prc.mu_0(l-j+1) +sqrt(r.p_prc.sa_0(l-j+1));
        lowerprior = r.p_prc.mu_0(l-j+1) -sqrt(r.p_prc.sa_0(l-j+1));
        upper = [upperprior; r.traj.mu(:,l-j+1,1)+sqrt(r.traj.sa(:,l-j+1,1))];
        lower = [lowerprior; r.traj.mu(:,l-j+1,1)-sqrt(r.traj.sa(:,l-j+1,1))];

        plot(0, upperprior, 'ob', 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'ob', 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             'b', 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
    plot(ts, [r.p_prc.mu_0(l-j+1); r.traj.mu(:,l-j+1,1)], 'b', 'LineWidth', 2);
    hold all;
    plot(0, r.p_prc.mu_0(l-j+1), 'ob', 'LineWidth', 2); % prior
    xlim([0 ts(end)]);
    title(['Posterior expectation of x_' num2str(l-j+1)], 'FontWeight', 'bold');
    ylabel(['\mu_', num2str(l-j+1)]);
end

% Second level
subplot(l,1,l-1)

if plotsd == true
    for j=1:b
        upperprior = r.p_prc.mu_0(2) +sqrt(r.p_prc.sa_0(2));
        lowerprior = r.p_prc.mu_0(2) -sqrt(r.p_prc.sa_0(2));
        upper = [upperprior; r.traj.mu(:,2,j)+sqrt(r.traj.sa(:,2,j))];
        lower = [lowerprior; r.traj.mu(:,2,j)-sqrt(r.traj.sa(:,2,j))];

        plot(0, upperprior, 'o', 'Color', colors(j,:), 'LineWidth', 1);
        hold all;
        plot(0, lowerprior, 'o', 'Color', colors(j,:), 'LineWidth', 1);
        fill([ts, fliplr(ts)], [(upper)', fliplr((lower)')], ...
             colors(j,:), 'EdgeAlpha', 0, 'FaceAlpha', 0.15);
    end
end
for j=1:b
    plot(ts, [r.p_prc.mu_0(1); r.traj.mu(:,2,j)], 'Color', colors(j,:), 'LineWidth', 2);
    hold all;
    plot(0, r.p_prc.mu_0(1), 'o', 'Color', colors(j,:), 'LineWidth', 2); % prior
end
xlim([0 ts(end)]);
title('Posterior expectations of x_2', 'FontWeight', 'bold');
ylabel('\mu_2');

% Input level
subplot(l,1,l);

for j=1:b
    plot(ts, [tapas_sgm(r.p_prc.mu_0(2), 1); tapas_sgm(r.traj.mu(:,2,j), 1)], 'Color', colors(j,:), 'LineWidth', 2);
    hold all;
    plot(0, tapas_sgm(r.p_prc.mu_0(2), 1), 'o', 'Color', colors(j,:), 'LineWidth', 2); % prior
end
plot(ts(2:end), r.u(:,1), '.', 'Color', [0 0 0]); % inputs
plot(ts(2:end), r.traj.wt(:,1), 'k') % implied learning rate
if (ploty == true) && ~isempty(find(strcmp(fieldnames(r),'y'))) && ~isempty(r.y)
    y = r.y(:,1);
    if ~isempty(find(strcmp(fieldnames(r),'irr')))
        y(r.irr) = NaN; % weed out irregular responses
        plot(ts(r.irr)+1,  1.08.*ones([1 length(r.irr)]), 'x', 'Color', [1 0.7 0], 'Markersize', 11, 'LineWidth', 2); % irregular responses
    end
    for j=1:b
        plot(find(y==j), 1.08*ones([1 length(find(y==j))]), '.', 'Color', colors(j,:)); % responses
    end
    title(['Response y, input u (black dots), learning rate (fine black line), and posterior expectation of reward s(\mu_2) ', ...
           '(colour coded) for \phi=', num2str(r.p_prc.phi(2:end)), ', m=', num2str(r.p_prc.m(2:end)), ', \kappa=', ...
           num2str(r.p_prc.ka(2:end)), ', \omega=', num2str(r.p_prc.om(2:end))], ...
      'FontWeight', 'bold');
    ylabel('y, u, s(\mu_2)');
    axis([0 ts(end) -0.15 1.15]);
else
    title(['Input u (black dots), learning rate (fine black line), and posterior expectation of input s(\mu_2) ', ...
           '(red) for \rho=', num2str(r.p_prc.rho(2:end)), ', \kappa=', ...
           num2str(r.p_prc.ka(2:end)), ', \omega=', num2str(r.p_prc.om(2:end))], ...
      'FontWeight', 'bold');
    ylabel('u, s(\mu_2)');
    axis([0 ts(end) -0.1 1.1]);
end
plot(ts(2:end), 0.5, 'k');
xlabel('Trial number');
hold off;


function [f,nxe_train,w] = train_qfuser(scores,scrQ,key,quiet,w0,cstepHessian,niters,obj_func,prior)
% Trains a linear fusion of scores with quality measures starting
% from the linear fusion of the only the scores.
% Inputs:
%   scores: A matrix of dev scores where the number of rows is the
%     number of systems to be fused and the number of columns
%     is the number of scores.
%   scrQ: An object of type Quality containing the quality measures
%     for the dev trials.
%   key: A Key object indicating target and non-target trials.
%   quiet: Boolean.  If true, the training algorithm outputs fewer
%     messages describing its progress.
%   w0: The fusion weights from the linear (without quality) fusion
%     step.
%   cstepHessian: Boolean.  If true, the training algorithm will
%     calculate the Hessian matrix using complex step
%     differentiation which should make training faster.
%   niters: The maximum number of training iterations.
%   obj_func: The objective function for the fusion training.  If [],
%     cllr objective is used.
%   prior: The effective target prior.
% Outputs:
%   f: A function handle for the trained fusion function (of type qfuser_backoff).
%   nxe_train: The normalized cross-entropy of the solution found
%     when training the fusion weights.
%   w: The fusion weights found by the trainer.

assert(nargin==9)
assert(isa(key,'Key'))

trials = key.tar(:)' | key.non(:)';
n1 = size(scores,2);
n2 = sum(trials);
assert(n1==n2,'# score trials = %i, but # key trials = %i',n1,n2)

% dump bad trials
validscores = scrQ.scoremask(trials);
if sum(trials)-sum(validscores)>0
    log_warning('Quality fusion training has %i missing trials.\n',sum(trials)-sum(validscores));
end
scores = scores(:,validscores);

% remove models and segs without quality measures from key (and the
% corresponding trials)
key.modelset = key.modelset(scrQ.hasmodel);
key.segset = key.segset(scrQ.hasseg);
key.tar = key.tar(scrQ.hasmodel,scrQ.hasseg);
key.non = key.non(scrQ.hasmodel,scrQ.hasseg);
trials = key.tar(:)' | key.non(:)';

% also remove them from scrQ
scrQ.modelset = scrQ.modelset(scrQ.hasmodel);
scrQ.segset = scrQ.segset(scrQ.hasseg);
scrQ.scoremask = scrQ.scoremask(scrQ.hasmodel,scrQ.hasseg);
scrQ.modelQ = scrQ.modelQ(:,scrQ.hasmodel);
scrQ.segQ = scrQ.segQ(:,scrQ.hasseg);

scrQ.hasmodel = scrQ.hasmodel(scrQ.hasmodel);
scrQ.hasseg = scrQ.hasseg(scrQ.hasseg);

assert(size(scores,2)==sum(trials));

% set up function that must be trained
[fusion,w0] = qfuser_linear([],scores,scrQ,key,w0);

% Create vector that classifies scores as target/non-target
tar = key.tar(trials);
tar = tar(:)';
non = key.non(trials);
non = non(:)';
classf = zeros(1,size(scores,2));
classf(tar) = 1;
classf(non) = -1;
% rest is zero and will be ignored

% train the fusion weights
[w,nxe_train] = train_binary_classifier(fusion,classf,w0,obj_func,prior,[],0,niters,[],[],quiet,cstepHessian);

% create function handle to do fusion using trained weights
f = @(scores,scrQ,key,backoff_scores) qfuser_backoff(w,scores,scrQ,key,backoff_scores);


function cond = bodewig_condition ( )

%*****************************************************************************80
%
%% BODEWIG_CONDITION returns the L1 condition of the BODEWIG matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real COND, the L1 condition.
%
  cond = 10.436619718309862;

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Max manipulability index ALONG A LINE.
% Use stomp like to optimize along a surface/line
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [qq, manips]=path_planning_SCO_SAWYER
close all;
global robot
global parameters
global hfigures

%STOMP PARAMETERS
%conversion from cost to Prob factor
%lambda is a constant to smooth the probability function
%with respect to obstacles and manipulability
parameters.lambda_obstacles = 2;
parameters.lambda_manip = 3;
%cost function starts at this distance
%must be below 0.3 for the 4 DOF robot
parameters.epsilon_distance = 0.1;
%height of the obstacle
%parameters.yo = 2.5;

%parameters.noise_sigma_null_space = 0.01;
parameters.alpha=0.05;
parameters.step_time=0.1;
%Error in XYZ to stop inverse kinematics
parameters.epsilonXYZ=0.01;
%Error in Quaternion to stop inverse kinematics.
parameters.epsilonQ=0.01;
parameters.stop_iterations=500;

%number of waypoints. More waypoints means greater precision in the
%trajectory
parameters.N = 15;
%number of particles. Set the number of hypotheses in the algorithm
parameters.K = 5;
%select the rows of J that should be taken into account when computing
%manipulability
% this considers the ellipsoid in linear speed and angular speed
%parameters.sel_J = [1 2 3 4 5 6];
% this one only considers the ellipsoid in linear speed and discards the
% angular speed component
parameters.sel_J = [1 2 3];
parameters.obstacles = [];

parameters.animate = 0;
close all
hfigures.hpaths = figure;
hfigures.hcosts = figure;
hfigures.hee = figure;
hfigures.htheta = figure;
hfigures.hbest_costs = figure;
hfigures.hdtheta = figure;

%LINE 1
x1 = -0.5;
y1 = 0.3; %m
z1 = 0;
x2 = 0;
y2 = 0.4; %m
z2 = 0;
p0 = [x1 y1 z1]';
pf = [x2 y2 z2]';
p3 = [0 0 0]';

%normal
n = compute_plane([p3 pf p0]);
parameters.obstacles{1}.n = n;
parameters.obstacles{1}.p = p0;
%points defining surface. %define obstacles using point and normal
parameters.obstacles{1}.points = [p3 pf p0];
%define trajectory from plane
parameters.trajectory{1}.line = [p0 pf];
parameters.trajectory{1}.T0 = build_T_from_obstacle(parameters.obstacles{1}, p0);

%LINE 2
x1 = 0;
y1 = 0.4; %m
z1 = 0;
x2 = 0.4;
y2 = 0.4; %m
z2 = 0.4;
p0 = [x1 y1 z1]';
pf = [x2 y2 z2]';
p3 = [0 0 0]';
%normal
n = compute_plane([p3 pf p0]);
parameters.obstacles{2}.n = n;
parameters.obstacles{2}.p = p0;
parameters.obstacles{2}.points = [p3 pf p0];
%define trajectory from plane
parameters.trajectory{2}.line = [p0 pf];
parameters.trajectory{2}.T0 = build_T_from_obstacle(parameters.obstacles{2}, p0);

%
% %LINE 3
x1 = 0.4;
y1 = 0.4; %m
z1 = 0.4;
x2 = 0.4;
y2 = 0.0; %m
z2 = 0.0; %m
p0 = [x1 y1 z1]';
pf = [x2 y2 z2]';
p3 = [0 0 0]';
%normal
n = compute_plane([p3 pf p0]);
parameters.obstacles{3}.n = n;
parameters.obstacles{3}.p = p0;
parameters.obstacles{3}.points = [p3 pf p0];
%define trajectory from plane
parameters.trajectory{3}.line = [p0 pf];
parameters.trajectory{3}.T0 = build_T_from_obstacle(parameters.obstacles{3}, p0);

%LAUNCH SCO given the stored parameters
pk = SCO_null_space(robot);


function fg = dtiXformFiberCoords(fg, xform, coordspace)
%Applies the coordinate xform to all the fiber coordinates
%
%  fg = dtiXformFiberCoords(fg, xform)
%
% Typically fibers are stored in ACPC space.  Sometimes we want the fibers
% in image space or MNI space.  This routine efficiently transforms the
% coordinates from the space they are in the new space defined by the
% homogeneous transform (4x4) in xform.
%
% The code is optimized to transform without an expensive loop, unless it
% seems that memory is tight. In that case, it just does a (slow) loop.
%
% Example:
%  xform = dtiGet(dtiH,'acpc2img xform');
%  fgImg = dtiXformFiberCoords(fg,xform,'img');
%
%  xform = dtiGet(dtiH,'img2acpc xform');
%  fgAcpc = dtiXformFiberCoords(fg,xform,'acpc');
%
% HISTORY:
% 2005.01.25 RFD: wrote it.
% 2006.05.17 RFD: added fallback to slow, memory-efficient loop when it
% seems that space is tight.
% 2006.11.21 RFD: now allow just the fibers cell array as input, so that
% this routine is useful even when you don't have a proper 'fg' struct.
% 2009.01.11 RFD: modified to use a hybrid routine that is a good compromise
% between speed and memory usage.
%
% (c) Stanford VISTA Team

% We take either a fibergroup struct or just the 'fibers' field (a cell
% array of fibers).
if(iscell(fg))
    wasCell = true;
    tmp = fg;
    clear fg;
    fg.fibers = tmp;
else
    wasCell = false;
end

% Operate on 1000 fiber coordinates at a time?
stepSize = 1000;
n = numel(fg.fibers);
fiberLen = cellfun('size',fg.fibers,2);
for jj = 1:stepSize:n
    sInd = jj;
    eInd = min(jj+stepSize-1,n);

    fc = mrAnatXformCoords(xform, horzcat(fg.fibers{sInd:eInd}),0);

    if(~isempty(fc))
        fiberCoord = 1;
        for ii=sInd:eInd
            fg.fibers{ii} = fc(:,(fiberCoord:fiberCoord+fiberLen(ii)-1));
            fiberCoord = fiberCoord + fiberLen(ii);
        end
    end
end

% This was AR's idea, and it was a good one.  But it creates a problem
% because the fg structures have different slots so we can't store them in
% dtiH arrays.  We get an error about unmatched structures.  If the
% dtiH.fiberGroups was a cell array, we would be OK.  But it is an array of
% structs.  Sigh.
if ~notDefined('coordspace')
    fg = fgSet(fg, 'coordspace', coordspace);
else
    fg = fgSet(fg, 'coordspace', []);
end

% Put it back as you found it
if(wasCell)
    fg = fg.fibers;
end

return;


function nearest_interp_1d_test ( )

%*****************************************************************************80
%
%% NEAREST_INTERP_1D_TEST tests the NEAREST_INTERP_1D library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 October 2012
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../r8lib' )
  addpath ( '../test_interp' )

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'NEAREST_INTERP_1D_TEST:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the NEAREST_INTERP_1D library.\n' );
  fprintf ( 1, '  The R8LIB library is needed.\n' );
  fprintf ( 1, '  The test needs the TEST_INTERP library.\n' );

  prob_num = p00_prob_num ( );

  ni = 11;
  for prob = 1 : prob_num
    nearest_interp_1d_test01 ( prob, ni );
  end

  for prob = 1 : prob_num
    nearest_interp_1d_test02 ( prob );
  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'NEAREST_INTERP_1D_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  rmpath ( '../r8lib' )
  rmpath ( '../test_interp' )

  return
end


function [estimate] = estimate_completion_time(sequences, varargin)
% estimate_completion_time Estimates completion time for a tracker
%
% Estimates time for a pass over a given set of sequences given some basic performance characteristics.
%
% Input:
% - sequences: Cell array of sequence structures.
% - varargin[FPS]: Average speed of a tracker (frames per second).
% - varargin[Failures]: Average number of failures per sequence.
% - varargin[Repeats]: Required number of repetitions for each sequence.
%
% Output:
% - estimate: Completion time estimate given in number of seconds

failures = 8;
fps = 0.5;
repeats = 1;

args = varargin;
for j=1:2:length(args)
    switch varargin{j}
        case 'fps', fps = max(0, args{j+1});
        case 'failures', failures = max(0, args{j+1});
        case 'repeats', repeats = max(1, args{j+1});
        otherwise, error(['unrecognized argument ' args{j}]);
    end
end

total_frames = 0;

for i = 1:length(sequences)
    total_frames = total_frames + sequences{i}.length;
end;

average_frames = total_frames / length(sequences);

lens = linspace(0, 1, failures + 1);
reruns = sum(lens);

estimate = (average_frames * reruns * repeats) * length(sequences) / fps;


% Codes for CVPR-15 work `Face Alignment by Coarse-to-Fine Shape Searching'
% Any question please contact Shizhan Zhu: zhshzhutah2@gmail.com
% Released on July 25, 2015

function [images,T] = testingsetGeneration(img_root,nameList,bbox,...
    priorModel,priorsInfo,mean_simple_face,target_simple_face)

assert(~xor(nargout >= 3,exist('data','var')));
m = length(nameList);
assert(size(bbox,1)==m);
p = zeros(m,8);
p(:,1:4) = repmat(bbox(:,1),1,4) + ...
    repmat(mean_simple_face(:,1:4),m,1) .* repmat(bbox(:,2)-bbox(:,1),1,4);
p(:,5:8) = repmat(bbox(:,3),1,4) + ...
    repmat(mean_simple_face(:,5:8),m,1) .* repmat(bbox(:,4)-bbox(:,3),1,4);
Tb = getTransToSpecific(p,target_simple_face * priorsInfo.win_size);
ori_images = cell(m,1);
images = cell(m,1);
for i = 1:m
    img = imread([img_root nameList{i}]);
    img = im2uint8(img);
    if size(img,3)>1, img = rgb2gray(img);end;
    ori_images{i} = img;
    images{i} = imtransform(img,Tb(i),'XData',[1 priorsInfo.win_size],...
        'YData',[1,priorsInfo.win_size]);
end;

rotationCenter = priorsInfo.win_size * priorsInfo.rotationCenter;
extractWindow = priorsInfo.win_size * priorsInfo.extractWindow;
assert(extractWindow(4)-extractWindow(3) == extractWindow(2)-extractWindow(1));
extractCellSize = (extractWindow(4)-extractWindow(3)) / priorsInfo.extractCellNum;
predicted = zeros(m,2);
F = zeros(m,priorsInfo.extractCellNum*priorsInfo.extractCellNum*31);
for i = 1:m
    F(i,:) = reshape(...
        vl_hog(single(images{i}(extractWindow(3):extractWindow(4),extractWindow(1):extractWindow(2))),extractCellSize),...
        [1,priorsInfo.extractCellNum*priorsInfo.extractCellNum*31]);
end;
for i = 1:2
    predicted(:,i) = predict(priorModel.B{i},F);
end;

predicted_vote = mean(predicted,2);
ind = find(predicted(:,1) .* predicted(:,2) < 0);
predicted_vote(ind) = 0;
ind = find(predicted(:,1) > priorsInfo.predictedVoteThre & predicted(:,2) > priorsInfo.predictedVoteThre);
predicted_vote(ind) = max(predicted(ind,:),[],2);
ind = find(predicted(:,1) < -priorsInfo.predictedVoteThre & predicted(:,2) < -priorsInfo.predictedVoteThre);
predicted_vote(ind) = min(predicted(ind,:),[],2);

Tr = getTransViaRotateGivenCenter(-predicted_vote,rotationCenter,priorsInfo.rotatorLength);
T = getTransViaMerge(priorsInfo.win_size,Tb,Tr);
for i = 1:m
    images{i} = imtransform(ori_images{i},T(i),'XData',[1 priorsInfo.win_size],...
        'YData',[1 priorsInfo.win_size]);
end;

end


function msm_to_mm_test08 ( )

%*****************************************************************************80
%
%% MSM_TO_MM_TEST08 tests MSM_TO_MM_ARRAY_REAL_GENERAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MSM_TO_MM_TEST08\n' );
  fprintf ( 1, '  Convert an MSM to MM array real general format.\n' );

  output_filename = 'msm_to_mm_test08.mm';

  a = r8mat_indicator ( 5, 3 );
%
%  Have MSM_TO_MM write the matrix to a file.
%
  msm_to_mm ( output_filename, a, 'array', 'real', 'general' );

  return
end


function x = p04_x ( n )

%*****************************************************************************80
%
%% P04_X returns the least squares solution X for problem 4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Output, real X(N,1), the least squares solution.
%
  x = [ 1.0; 2.0; 3.0; 4.0; 5.0 ];

  return
end