% IN ORDER TO SIMULATE THE PROGRAM:
%   A) FIRST, LOAD A ROBOT
%       robot = load_robot('abb','irb140');
%   B) NEXT, LOAD SOME EQUIPMENT.
%       robot.equipment{1} = load_robot('equipment','tables/table_small');
%       OR
%       robot.equipment{1} = load_robot('equipment','bumper_cutting');
%   C) NOW, LOAD AN END TOOL
%       robot.tool= load_robot('equipment','end_tools/parallel_gripper_0');
%   D) FINALLY, LOAD A PIECE TO GRAB BY THE ROBOT
%       robot.piece=load_robot('equipment','cylinders/cylinder_tiny');
%
%   E) IF NECESSARY, CHANGE THE POSITION AND ORIENTATION OF THE ROBOT'S
%   BASE
%       robot.piece.T0= [1 0 0 -0.35;
%                        0 1 0 -0.55;
%                        0 0 1 0.2;
%                        0 0 0 1];
%
% during the simulation, call simulation_open_tool; to open the tool and
% simulation_close_tool; to close it.
% To grip the piece, call simulation_grip_piece; and
% simulation_release_piece to release it.
% The call to each function must be correct, thus, typically the correct
% sequence is:

% simulation_open_tool;
% approach the piece to grab.
% simulation_close_tool;
% simulation_grip_piece; --> the piece will be drawn with the robot
% move to a different place
% simulation_open_tool;
% simulation_release_piece

function basic_robot_program

global RT_tp1 RT_tp2 RT_tp3 RT_tp4 TD_gripper

TD_gripper=[1,[[0,0,0.125],[1,0,0,0]],[0.1,[0,0,0.100],[1,0,0,0],0,0,0]];
RT_tp1=[[0.541,0.1171,0.713],[0.727812,-0.115363,0.676004,-0.000468],[0,0,-1,1],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
RT_tp2=[[0.515,-0.200,0.712],[0.7071,0,0.7071,0],[-1,-2,1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
RT_tp3=[[0.515,0,0.912],[0.7071,0,0.7071,0],[0,-1,0,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];
RT_tp4=[[0.515,0,0.512],[0.7071,0,0.7071,0],[0,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];

main
end

function main

global RT_tp1 RT_tp2 RT_tp3 RT_tp4 TD_gripper

MoveJ(RT_tp1, 'vmax' , 'fine' , TD_gripper, 'wobj0');
MoveJ(RT_tp2, 'vmax' , 'fine' , TD_gripper, 'wobj0');
MoveJ(RT_tp3,'vmax','fine', TD_gripper, 'wobj0');
MoveL(RT_tp4,'vmax','fine', TD_gripper, 'wobj0');
end


function [mesh,elt2fce] = mshFace(mesh)
%+========================================================================+
%|                                                                        |
%|                 OPENMSH - LIBRARY FOR MESH MANAGEMENT                  |
%|           openMsh is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : mshFace.m                                     |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Submesh to face mesh                          |
%|  `---'  |                                                              |
%+========================================================================+

% Particles mesh
if (size(mesh.elt,2) == 1)
    error('mshFace.m : unavailable case')

% Edge mesh
elseif (size(mesh.elt,2) == 2)
    error('mshFace.m : unavailable case')

% Triangular mesh
elseif (size(mesh.elt,2) == 3)
    fce2vtx = mesh.elt;
    elt2fce = (1:size(mesh.elt,1))';
    col     = mesh.col;

% Tetrahedral mesh
elseif (size(mesh.elt,2) == 4)
    % All faces
    fce2vtx = [ mesh.elt(:,[2,3,4]) ; mesh.elt(:,[3,4,1]) ; ...
        mesh.elt(:,[4,1,2]) ; mesh.elt(:,[1,2,3]) ];

    % Faces unicity
    tmp           = sort(fce2vtx,2);
    [~,I,elt2fce] = unique(tmp,'rows');

    % Final faces
    fce2vtx = fce2vtx(I,:);
    elt2fce = reshape(elt2fce,size(mesh.elt,1),4);

    % Colours
    col             = zeros(size(fce2vtx,1),1);
    tmp             = mesh.col * ones(1,4);
    col(elt2fce(:)) = tmp(:);

    % Identify boundary
    bnd      = ( accumarray(elt2fce(:),1).*col ~= accumarray(elt2fce(:),tmp(:)) );
    col(bnd) = -1;

% Unknown type
else
    error('mshFace.m : unavailable case')
end

% Mesh format
mesh = msh(mesh.vtx,fce2vtx,col);
end


function tests = test_spm_dcm_post_hoc
% Unit Tests for spm_dcm_post_hoc
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% $Id: test_spm_dcm_post_hoc.m 6492 2015-06-26 14:27:40Z guillaume $

tests = functiontests(localfunctions);

%--------------------------------------------------------------------------
function data_path = get_data_path()
data_path = fullfile( spm('Dir'), 'tests', ...
    'data', 'test_spm_dcm_post_hoc');

%--------------------------------------------------------------------------
function setup(testCase)
data_path = get_data_path();

% Expected outputs
artefacts = {fullfile(data_path,'DCM_BPA.mat');
             fullfile(data_path,'DCM_opt_fwd_bwd_simulated.mat')};

% Delete if exist
for i = 1:length(artefacts)
    spm_unlink(artefacts{i});
end

% Initialize SPM
spm('defaults','fmri');
spm_get_defaults('cmdline',true);

%--------------------------------------------------------------------------
function test_on_simulated_attention_data(testCase)
import matlab.unittest.constraints.*

data_path = get_data_path();

% Load model matching the generative model
DCM_fwd = load(fullfile(data_path, 'DCM_fwd_simulated.mat'));
DCM_fwd = DCM_fwd.DCM;

% Load model with extra connection
DCM_fwd_bwd = load(fullfile(data_path, 'DCM_fwd_bwd_simulated.mat'));
DCM_fwd_bwd = DCM_fwd_bwd.DCM;

% Run post-hoc
DCM_BPA = spm_dcm_post_hoc(fullfile(data_path, 'DCM_fwd_bwd_simulated.mat'));
DCM_opt = load(fullfile(data_path,'DCM_opt_fwd_bwd_simulated.mat'));
DCM_opt = DCM_opt.DCM;

% Check artefacts were created
testCase.assertEqual( exist(fullfile(data_path,'DCM_BPA.mat'),'file'), 2 );
testCase.assertEqual( exist(fullfile(data_path,'DCM_opt_fwd_bwd_simulated.mat'),'file'), 2 );

% Check reduced priors match the generative model
pC_fwd = full(diag(DCM_fwd.M.pC));
pC_opt = full(diag(DCM_opt.M.pC));
testCase.assertTrue(all(pC_fwd == pC_opt));

% % Plot for visual inspection
% spm_figure('Getwin','BMC'); clf
%
% subplot(2,2,1); spm_plot_ci(DCM_fwd.Ep,DCM_fwd.Cp);
% title('Fwd (generative) model'); xlabel('Parameter')
%
% subplot(2,2,2); spm_plot_ci(DCM_fwd_bwd.Ep,DCM_fwd_bwd.Cp);
% title('Bwd (over-complex) model'); xlabel('Parameter')
%
% subplot(2,2,3); spm_plot_ci(DCM_BPA.Ep,DCM_BPA.Cp);
% title('BPA'); xlabel('Parameter')
%
% subplot(2,2,4); spm_plot_ci(DCM_opt.Ep,DCM_opt.Cp);
% title('Optimal model for subject'); xlabel('Parameter');


function varargout = bbApply( action, varargin )
% Functions for manipulating bounding boxes (bb).
%
% A bounding box (bb) is also known as a position vector or a rectangle
% object. It is a four element vector with the fields: [x y w h]. A set of
% n bbs can be stores as an [nx4] array, most funcitons below can handle
% either a single or multiple bbs. In addtion, typically [nxm] inputs with
% m>4 are ok (with the additional columns ignored/copied to the output).
%
% bbApply contains a number of utility functions for working with bbs. The
% format for accessing the various utility functions is:
%  outputs = bbApply( 'action', inputs );
% The list of functions and help for each is given below. Also, help on
% individual subfunctions can be accessed by: "help bbApply>action".
%
% Compute area of bbs.
%   bb = bbApply( 'area', bb )
% Shift center of bbs.
%   bb = bbApply( 'shift', bb, xdel, ydel )
% Get center of bbs.
%   cen = bbApply( 'getCenter', bb )
% Get bb at intersection of bb1 and bb2 (may be empty).
%   bb = bbApply( 'intersect', bb1, bb2 )
% Get bb that is union of bb1 and bb2 (smallest bb containing both).
%   bb = bbApply( 'union', bb1, bb2 )
% Resize the bbs (without moving their centers).
%   bb = bbApply( 'resize', bb, hr, wr, [ar] )
% Fix bb aspect ratios (without moving the bb centers).
%   bbr = bbApply( 'squarify', bb, flag, [ar] )
% Draw single or multiple bbs to image (calls rectangle()).
%   hs = bbApply( 'draw', bb, [col], [lw], [ls], [prop], [ids] )
% Embed single or multiple bbs directly into image.
%  I = bbApply( 'embed', I, bb, [varargin] )
% Crop image regions from I encompassed by bbs.
%   [patches, bbs] = bbApply('crop',I,bb,[padEl],[dims])
% Convert bb relative to absolute coordinates and vice-versa.
%   bb = bbApply( 'convert', bb, bbRef, isAbs )
% Randomly generate bbs that fall in a specified region.
%   bbs =  bbApply( 'random', pRandom )
% Convert weighted mask to bbs.
%   bbs = bbApply('frMask',M,bbw,bbh,[thr])
% Create weighted mask encoding bb centers (or extent).
%   M = bbApply('toMask',bbs,w,h,[fill],[bgrd])
%
% USAGE
%  varargout = bbApply( action, varargin );
%
% INPUTS
%  action     - string specifying action
%  varargin   - depends on action, see above
%
% OUTPUTS
%  varargout  - depends on action, see above
%
% EXAMPLE
%
% See also bbApply>area bbApply>shift bbApply>getCenter bbApply>intersect
% bbApply>union bbApply>resize bbApply>squarify bbApply>draw bbApply>crop
% bbApply>convert bbApply>random bbApply>frMask bbApply>toMask
%
% Piotr's Computer Vision Matlab Toolbox      Version 3.30
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

%#ok<*DEFNU>
varargout = cell(1,max(1,nargout));
[varargout{:}] = feval(action,varargin{:});
end

function a = area( bb )
% Compute area of bbs.
%
% USAGE
%  bb = bbApply( 'area', bb )
%
% INPUTS
%  bb     - [nx4] original bbs
%
% OUTPUTS
%  a      - [nx1] area of each bb
%
% EXAMPLE
%  a = bbApply('area', [0 0 10 10])
%
% See also bbApply
a=prod(bb(:,3:4),2);
end

function bb = shift( bb, xdel, ydel )
% Shift center of bbs.
%
% USAGE
%  bb = bbApply( 'shift', bb, xdel, ydel )
%
% INPUTS
%  bb     - [nx4] original bbs
%  xdel   - amount to shift x coord of each bb left
%  ydel   - amount to shift y coord of each bb up
%
% OUTPUTS
%  bb     - [nx4] shifted bbs
%
% EXAMPLE
%  bb = bbApply('shift', [0 0 10 10], 1, 2)
%
% See also bbApply
bb(:,1)=bb(:,1)-xdel; bb(:,2)=bb(:,2)-ydel;
end

function cen = getCenter( bb )
% Get center of bbs.
%
% USAGE
%  cen = bbApply( 'getCenter', bb )
%
% INPUTS
%  bb     - [nx4] original bbs
%
% OUTPUTS
%  cen    - [nx1] centers of bbs
%
% EXAMPLE
%  cen = bbApply('getCenter', [0 0 10 10])
%
% See also bbApply
cen=bb(:,1:2)+bb(:,3:4)/2;
end

function bb = intersect( bb1, bb2 )
% Get bb at intersection of bb1 and bb2 (may be empty).
%
% USAGE
%  bb = bbApply( 'intersect', bb1, bb2 )
%
% INPUTS
%  bb1    - [nx4] first set of bbs
%  bb2    - [nx4] second set of bbs
%
% OUTPUTS
%  bb     - [nx4] intersection of bbs
%
% EXAMPLE
%  bb = bbApply('intersect', [0 0 10 10], [5 5 10 10])
%
% See also bbApply bbApply>union
n1=size(bb1,1); n2=size(bb2,1);
if(n1==0 || n2==0), bb=zeros(0,4); return, end
if(n1==1 && n2>1), bb1=repmat(bb1,n2,1); n1=n2; end
if(n2==1 && n1>1), bb2=repmat(bb2,n1,1); n2=n1; end
assert(n1==n2);
lcsE=min(bb1(:,1:2)+bb1(:,3:4),bb2(:,1:2)+bb2(:,3:4));
lcsS=max(bb1(:,1:2),bb2(:,1:2)); empty=any(lcsE<lcsS,2);
bb=[lcsS lcsE-lcsS]; bb(empty,:)=0;
end

function bb = union( bb1, bb2 )
% Get bb that is union of bb1 and bb2 (smallest bb containing both).
%
% USAGE
%  bb = bbApply( 'union', bb1, bb2 )
%
% INPUTS
%  bb1    - [nx4] first set of bbs
%  bb2    - [nx4] second set of bbs
%
% OUTPUTS
%  bb     - [nx4] intersection of bbs
%
% EXAMPLE
%  bb = bbApply('union', [0 0 10 10], [5 5 10 10])
%
% See also bbApply bbApply>intersect
n1=size(bb1,1); n2=size(bb2,1);
if(n1==0 || n2==0), bb=zeros(0,4); return, end
if(n1==1 && n2>1), bb1=repmat(bb1,n2,1); n1=n2; end
if(n2==1 && n1>1), bb2=repmat(bb2,n1,1); n2=n1; end
assert(n1==n2);
lcsE=max(bb1(:,1:2)+bb1(:,3:4),bb2(:,1:2)+bb2(:,3:4));
lcsS=min(bb1(:,1:2),bb2(:,1:2));
bb=[lcsS lcsE-lcsS];
end

function bb = resize( bb, hr, wr, ar )
% Resize the bbs (without moving their centers).
%
% If wr>0 or hr>0, the w/h of each bb is adjusted in the following order:
%  if(hr~=0), h=h*hr; end
%  if(wr~=0), w=w*wr; end
%  if(hr==0), h=w/ar; end
%  if(wr==0), w=h*ar; end
% Only one of hr/wr may be set to 0, and then only if ar>0. If, however,
% hr=wr=0 and ar>0 then resizes bbs such that areas and centers are
% preserved but aspect ratio becomes ar.
%
% USAGE
%  bb = bbApply( 'resize', bb, hr, wr, [ar] )
%
% INPUTS
%  bb     - [nx4] original bbs
%  hr     - ratio by which to multiply height (or 0)
%  wr     - ratio by which to multiply width (or 0)
%  ar     - [0] target aspect ratio (used only if hr=0 or wr=0)
%
% OUTPUT
%  bb    - [nx4] the output resized bbs
%
% EXAMPLE
%  bb = bbApply('resize',[0 0 1 1],1.2,0,.5) % h'=1.2*h; w'=h'/2;
%
% See also bbApply, bbApply>squarify
if(nargin<4), ar=0; end; assert(size(bb,2)>=4);
assert((hr>0&&wr>0)||ar>0);
% preserve area and center, set aspect ratio
if(hr==0 && wr==0), a=sqrt(bb(:,3).*bb(:,4)); ar=sqrt(ar);
  d=a*ar-bb(:,3); bb(:,1)=bb(:,1)-d/2; bb(:,3)=bb(:,3)+d;
  d=a/ar-bb(:,4); bb(:,2)=bb(:,2)-d/2; bb(:,4)=bb(:,4)+d; return;
end
% possibly adjust h/w based on hr/wr
if(hr~=0), d=(hr-1)*bb(:,4); bb(:,2)=bb(:,2)-d/2; bb(:,4)=bb(:,4)+d; end
if(wr~=0), d=(wr-1)*bb(:,3); bb(:,1)=bb(:,1)-d/2; bb(:,3)=bb(:,3)+d; end
% possibly adjust h/w based on ar and NEW h/w
if(~hr), d=bb(:,3)/ar-bb(:,4); bb(:,2)=bb(:,2)-d/2; bb(:,4)=bb(:,4)+d; end
if(~wr), d=bb(:,4)*ar-bb(:,3); bb(:,1)=bb(:,1)-d/2; bb(:,3)=bb(:,3)+d; end
end

function bbr = squarify( bb, flag, ar )
% Fix bb aspect ratios (without moving the bb centers).
%
% The w or h of each bb is adjusted so that w/h=ar.
% The parameter flag controls whether w or h should change:
%  flag==0: expand bb to given ar
%  flag==1: shrink bb to given ar
%  flag==2: use original w, alter h
%  flag==3: use original h, alter w
%  flag==4: preserve area, alter w and h
% If ar==1 (the default), always converts bb to a square, hence the name.
%
% USAGE
%  bbr = bbApply( 'squarify', bb, flag, [ar] )
%
% INPUTS
%  bb     - [nx4] original bbs
%  flag   - controls whether w or h should change
%  ar     - [1] desired aspect ratio
%
% OUTPUT
%  bbr    - the output 'squarified' bbs
%
% EXAMPLE
%  bbr = bbApply('squarify',[0 0 1 2],0)
%
% See also bbApply, bbApply>resize
if(nargin<3 || isempty(ar)), ar=1; end; bbr=bb;
if(flag==4), bbr=resize(bb,0,0,ar); return; end
for i=1:size(bb,1), p=bb(i,1:4);
  usew = (flag==0 && p(3)>p(4)*ar) || (flag==1 && p(3)<p(4)*ar) || flag==2;
  if(usew), p=resize(p,0,1,ar); else p=resize(p,1,0,ar); end; bbr(i,1:4)=p;
end
end

function hs = draw( bb, col, lw, ls, prop, ids )
% Draw single or multiple bbs to image (calls rectangle()).
%
% To draw bbs aligned with pixel boundaries, subtract .5 from the x and y
% coordinates (since pixel centers are located at integer locations).
%
% USAGE
%  hs = bbApply( 'draw', bb, [col], [lw], [ls], [prop], [ids] )
%
% INPUTS
%  bb     - [nx4] standard bbs or [nx5] weighted bbs
%  col    - ['g'] color or [kx1] array of colors
%  lw     - [2] LineWidth for rectangle
%  ls     - ['-'] LineStyle for rectangle
%  prop   - [] other properties for rectangle
%  ids    - [ones(1,n)] id in [1,k] for each bb into colors array
%
% OUTPUT
%  hs     - [nx1] handles to drawn rectangles (and labels)
%
% EXAMPLE
%  im(rand(3)); bbApply('draw',[1.5 1.5 1 1 .5],'g');
%
% See also bbApply, bbApply>embed, rectangle
[n,m]=size(bb); if(n==0), hs=[]; return; end
if(nargin<2 || isempty(col)), col=[]; end
if(nargin<3 || isempty(lw)), lw=2; end
if(nargin<4 || isempty(ls)), ls='-'; end
if(nargin<5 || isempty(prop)), prop={}; end
if(nargin<6 || isempty(ids)), ids=ones(1,n); end
% prepare display properties
prop=['LineWidth' lw 'LineStyle' ls prop 'EdgeColor'];
tProp={'FontSize',10,'color','w','FontWeight','bold',...
  'VerticalAlignment','bottom'}; k=max(ids);
if(isempty(col)), if(k==1), col='g'; else col=hsv(k); end; end
if(size(col,1)<k), ids=ones(1,n); end; hs=zeros(1,n);
% draw rectangles and optionally labels
for b=1:n, hs(b)=rectangle('Position',bb(b,1:4),prop{:},col(ids(b),:)); end
if(m==4), return; end; hs=[hs zeros(1,n)]; bb=double(bb);
for b=1:n, hs(b+n)=text(bb(b,1),bb(b,2),num2str(bb(b,5),4),tProp{:}); end
end

function I = embed( I, bb, varargin )
% Embed single or multiple bbs directly into image.
%
% USAGE
%  I = bbApply( 'embed', I, bb, varargin )
%
% INPUTS
%  I      - input image
%  bb     - [nx4] or [nx5] input bbs
%  varargin   - additional params (struct or name/value pairs)
%    .col       - [0 255 0] color for rectangle or nx3 array of colors
%    .lw        - [3] width for rectangle in pixels
%    .fh        - [35] font height (if displaying weight), may be 0
%    .fcol      - [255 0 0] font color or nx3 array of colors
%
% OUTPUT
%  I      - output image
%
% EXAMPLE
%  I=imResample(imread('cameraman.tif'),2); bb=[200 70 70 90 0.25];
%  J=bbApply('embed',I,bb,'col',[0 0 255],'lw',8,'fh',30); figure(1); im(J)
%  K=bbApply('embed',J,bb,'col',[0 255 0],'lw',2,'fh',30); figure(2); im(K)
%
% See also bbApply, bbApply>draw, char2img

% get additional parameters
dfs={'col',[0 255 0],'lw',3,'fh',35,'fcol',[255 0 0]};
[col,lw,fh,fcol]=getPrmDflt(varargin,dfs,1);
n=size(bb,1); bb(:,1:4)=round(bb(:,1:4));
if(size(col,1)==1), col=col(ones(1,n),:); end
if(size(fcol,1)==1), fcol=fcol(ones(1,n),:); end
if( ismatrix(I) ), I=I(:,:,[1 1 1]); end
% embed each bb
x0=bb(:,1); x1=x0+bb(:,3)-1; y0=bb(:,2); y1=y0+bb(:,4)-1;
j0=floor((lw-1)/2); j1=ceil((lw-1)/2); h=size(I,1); w=size(I,2);
x00=max(1,x0-j0); x01=min(x0+j1,w); x10=max(1,x1-j0); x11=min(x1+j1,w);
y00=max(1,y0-j0); y01=min(y0+j1,h); y10=max(1,y1-j0); y11=min(y1+j1,h);
for b=1:n
  for c=1:3, I([y00(b):y01(b) y10(b):y11(b)],x00(b):x11(b),c)=col(b,c); end
  for c=1:3, I(y00(b):y11(b),[x00(b):x01(b) x10(b):x11(b)],c)=col(b,c); end
end
% embed text displaying bb score (inside upper-left bb corner)
if(size(bb,2)<5 || fh==0), return; end
bb(:,1:4)=intersect(bb(:,1:4),[1 1 w h]);
for b=1:n
  M=char2img(sprintf('%.4g',bb(b,5)),fh); M=M{1}==0; [h,w]=size(M);
  y0=bb(b,2); y1=y0+h-1; x0=bb(b,1); x1=x0+w-1;
  if( x0>=1 && y0>=1 && x1<=size(I,2) && y1<=size(I,1))
    Ir=I(y0:y1,x0:x1,1); Ig=I(y0:y1,x0:x1,2); Ib=I(y0:y1,x0:x1,3);
    Ir(M)=fcol(b,1); Ig(M)=fcol(b,2); Ib(M)=fcol(b,3);
    I(y0:y1,x0:x1,:)=cat(3,Ir,Ig,Ib);
  end
end
end

function [patches, bbs] = crop( I, bbs, padEl, dims )
% Crop image regions from I encompassed by bbs.
%
% The only subtlety is that a pixel centered at location (i,j) would have a
% bb of [j-1/2,i-1/2,1,1].  The -1/2 is because pixels are located at
% integer locations. This is a Matlab convention, to confirm use:
%  im(rand(3)); bbApply('draw',[1.5 1.5 1 1],'g')
% If bb contains all integer entries cropping is straightforward. If
% entries are not integers, x=round(x+.499) is used, eg 1.2 actually goes
% to 2 (since it is closer to 1.5 then .5), and likewise for y.
%
% If ~isempty(padEl), image is padded so can extract full bb region (no
% actual padding is done, this is fast). Otherwise bb is intersected with
% the image bb prior to cropping. If padEl is a string ('circular',
% 'replicate', or 'symmetric'), uses padarray to do actual padding (slow).
%
% USAGE
%  [patches, bbs] = bbApply('crop',I,bb,[padEl],[dims])
%
% INPUTS
%  I        - image from which to crop patches
%  bbs      - bbs that indicate regions to crop
%  padEl    - [0] value to pad I or [] to indicate no padding (see above)
%  dims     - [] if specified resize each cropped patch to [w h]
%
% OUTPUTS
%  patches  - [1xn] cell of cropped image regions
%  bbs      - actual integer-valued bbs used to crop
%
% EXAMPLE
%  I=imread('cameraman.tif'); bb=[-10 -10 100 100];
%  p1=bbApply('crop',I,bb); p2=bbApply('crop',I,bb,'replicate');
%  figure(1); im(I); figure(2); im(p1{1}); figure(3); im(p2{1});
%
% See also bbApply, ARRAYCROP, PADARRAY, IMRESAMPLE

% get padEl, bound bb to visible region if empty
if( nargin<3 ), padEl=0; end; h=size(I,1); w=size(I,2);
if( nargin<4 ), dims=[]; end;
if(isempty(padEl)), bbs=intersect([.5 .5 w h],bbs); end
% crop each patch in turn
n=size(bbs,1); patches=cell(1,n);
for i=1:n, [patches{i},bbs(i,1:4)]=crop1(bbs(i,1:4)); end

  function [patch, bb] = crop1( bb )
    % crop single patch (use arrayCrop only if necessary)
    lcsS=round(bb([2 1])+.5-.001); lcsE=lcsS+round(bb([4 3]))-1;
    if( any(lcsS<1) || lcsE(1)>h || lcsE(2)>w )
      if( ischar(padEl) )
        pt=max(0,1-lcsS(1)); pb=max(0,lcsE(1)-h);
        pl=max(0,1-lcsS(2)); pr=max(0,lcsE(2)-w);
        lcsS1=max(1,lcsS); lcsE1=min(lcsE,[h w]);
        patch = I(lcsS1(1):lcsE1(1),lcsS1(2):lcsE1(2),:);
        patch = padarray(patch,[pt pl],padEl,'pre');
        patch = padarray(patch,[pb pr],padEl,'post');
      else
        if(ndims(I)==3); lcsS=[lcsS 1]; lcsE=[lcsE 3]; end
        patch = arrayCrop(I,lcsS,lcsE,padEl);
      end
    else
      patch = I(lcsS(1):lcsE(1),lcsS(2):lcsE(2),:);
    end
    bb = [lcsS([2 1]) lcsE([2 1])-lcsS([2 1])+1];
    if(~isempty(dims)), patch=imResample(patch,[dims(2),dims(1)]); end
  end
end

function bb = convert( bb, bbRef, isAbs )
% Convert bb relative to absolute coordinates and vice-versa.
%
% If isAbs==1, bb is assumed to be given in absolute coords, and the output
% is given in coords relative to bbRef. Otherwise, if isAbs==0, bb is
% assumed to be given in coords relative to bbRef and the output is given
% in absolute coords.
%
% USAGE
%  bb = bbApply( 'convert', bb, bbRef, isAbs )
%
% INPUTS
%  bb     - original bb, either in abs or rel coords
%  bbRef  - reference bb
%  isAbs  - 1: bb is in abs coords, 0: bb is in rel coords
%
% OUTPUTS
%  bb     - converted bb
%
% EXAMPLE
%  bbRef=[5 5 15 15]; bba=[10 10 5 5];
%  bbr = bbApply( 'convert', bba, bbRef, 1 )
%  bba2 = bbApply( 'convert', bbr, bbRef, 0 )
%
% See also bbApply
if( isAbs )
  bb(1:2)=bb(1:2)-bbRef(1:2);
  bb=bb./bbRef([3 4 3 4]);
else
  bb=bb.*bbRef([3 4 3 4]);
  bb(1:2)=bb(1:2)+bbRef(1:2);
end
end

function bbs = random( varargin )
% Randomly generate bbs that fall in a specified region.
%
% The vector dims defines the region in which bbs are generated. Specify
% dims=[height width] to generate bbs=[x y w h] such that: 1<=x<=width,
% 1<=y<=height, x+w-1<=width, y+h-1<=height. The biggest bb generated can
% be bb=[1 1 width height]. If dims is a three element vector the third
% coordinate is the depth, in this case bbs=[x y w h d] where 1<=d<=depth.
%
% A number of constraints can be specified that control the size and other
% characteristics of the generated bbs. Note that if incompatible
% constraints are specified (e.g. if the maximum width and height are both
% 5 while the minimum area is 100) no bbs will be generated. More
% generally, if fewer than n bbs are generated a warning is displayed.
%
% USAGE
%  bbs =  bbApply( 'random', pRandom )
%
% INPUTS
%  pRandom    - parameters (struct or name/value pairs)
%   .n          - ['REQ'] number of bbs to generate
%   .dims       - ['REQ'] region in which to generate bbs [height,width]
%   .wRng       - [1 inf] range for width of bbs (or scalar value)
%   .hRng       - [1 inf] range for height of bbs (or scalar value)
%   .aRng       - [1 inf] range for area of bbs
%   .arRng      - [0 inf] range for aspect ratio (width/height) of bbs
%   .unique     - [1] if true generate unique bbs
%   .maxOverlap - [1] max overlap (intersection/union) between bbs
%   .maxIter    - [100] max iterations to go w/o changes before giving up
%   .show       - [0] if true show sample generated bbs
%
% OUTPUTS
%  bbs        - [nx4] array of randomly generated integer bbs
%
% EXAMPLE
%  bbs=bbApply('random','n',50,'dims',[20 20],'arRng',[.5 .5],'show',1);
%
% See also bbApply

% get parameters
rng=[1 inf]; dfs={ 'n','REQ', 'dims','REQ', 'wRng',rng, 'hRng',rng, ...
  'aRng',rng, 'arRng',[0 inf], 'unique',1, 'maxOverlap',1, ...
  'maxIter',100, 'show',0 };
[n,dims,wRng,hRng,aRng,arRng,uniqueOnly,maxOverlap,maxIter,show] ...
  = getPrmDflt(varargin,dfs,1);
if(length(hRng)==1), hRng=[hRng hRng]; end
if(length(wRng)==1), wRng=[wRng wRng]; end
if(length(dims)==3), d=5; else d=4; end

% generate random bbs satisfying constraints
bbs=zeros(0,d); ids=zeros(0,1); n1=min(n*10,1000);
M=max(dims)+1; M=M.^(0:d-1); iter=0; k=0;
tid=ticStatus('generating random bbs',1,2);
while( k<n && iter<maxIter )
  ys=1+floor(rand(2,n1)*dims(1)); ys0=min(ys); ys1=max(ys); hs=ys1-ys0+1;
  xs=1+floor(rand(2,n1)*dims(2)); xs0=min(xs); xs1=max(xs); ws=xs1-xs0+1;
  if(d==5), ds=1+floor(rand(1,n1)*dims(3)); else ds=zeros(0,n1); end
  if(arRng(1)==arRng(2)), ws=hs.*arRng(1); end
  ars=ws./hs; ws=round(ws); xs1=xs0+ws-1; as=ws.*hs;
  kp = ys0>0 & xs0>0 & ys1<=dims(1) & xs1<=dims(2) & ...
    hs>=hRng(1) & hs<=hRng(2) & ws>=wRng(1) & ws<=wRng(2) & ...
    as>=aRng(1) & as<=aRng(2) & ars>=arRng(1) & ars<=arRng(2);
  bbs1=[xs0' ys0' ws' hs' ds']; bbs1=bbs1(kp,:);
  k0=k; bbs=[bbs; bbs1]; k=size(bbs,1); %#ok<AGROW>
  if( maxOverlap<1 && k ), bbs=bbs(1:k0,:);
    for j=1:size(bbs1,1), bbs0=bbs; bb=bbs1(j,:);
      if(d==5), bbs=bbs(bbs(:,5)==bb(5),:); end
      if(isempty(bbs)), bbs=[bbs0; bb]; continue; end
      ws1=min(bbs(:,1)+bbs(:,3),bb(1)+bb(3))-max(bbs(:,1),bb(1));
      hs1=min(bbs(:,2)+bbs(:,4),bb(2)+bb(4))-max(bbs(:,2),bb(2));
      o=max(0,ws1).*max(0,hs1); o=o./(bbs(:,3).*bbs(:,4)+bb(3).*bb(4)-o);
      if(max(o)<=maxOverlap), bbs=[bbs0; bb]; else bbs=bbs0; end
    end
  elseif( uniqueOnly && k )
    ids=[ids; sum(bbs1.*M(ones(1,size(bbs1,1)),:),2)]; %#ok<AGROW>
    [ids,o]=sort(ids); bbs=bbs(o,:); kp=[ids(1:end-1)~=ids(2:end); true];
    bbs=bbs(kp,:); ids=ids(kp,:);
  end
  k=size(bbs,1); if(k0==k), iter=iter+1; else iter=0; end
  if(k>n), bbs=bbs(randSample(k,n),:); k=n; end;
  tocStatus(tid,max(k/n,iter/maxIter));
end
if( k<n ), warning('only generated %i of %i bbs',k,n); n=k; end %#ok<WNTAG>

% optionally display a few bbs
if( show )
  k=8; figure(show); im(zeros(dims)); cs=uniqueColors(1,k,0,0);
  if(n>k), bbs1=bbs(randsample(n,k),:); else bbs1=bbs; end
  bbs1(:,1:2)=bbs1(:,1:2)-.5;
  for i=1:min(k,n), rectangle('Position',bbs1(i,:),...
      'EdgeColor',cs(i,:),'LineStyle','--'); end
end

end

function bbs = frMask( M, bbw, bbh, thr )
% Convert weighted mask to bbs.
%
% Pixels in mask above given threshold (thr) indicate bb centers.
%
% USAGE
%  bbs = bbApply('frMask',M,bbw,bbh,[thr])
%
% INPUTS
%  M      - mask
%  bbw    - bb target width
%  bbh    - bb target height
%  thr    - [0] mask threshold
%
% OUTPUTS
%  bbs    - bounding boxes
%
% EXAMPLE
%  w=20; h=10; bbw=5; bbh=8; M=double(rand(h,w)); M(M<.95)=0;
%  bbs=bbApply('frMask',M,bbw,bbh); M2=bbApply('toMask',bbs,w,h);
%  sum(abs(M(:)-M2(:)))
%
% See also bbApply, bbApply>toMask
if(nargin<4), thr=0; end
ids=find(M>thr); ids=ids(:); h=size(M,1);
if(isempty(ids)), bbs=zeros(0,5); return; end
xs=floor((ids-1)/h); ys=ids-xs*h; xs=xs+1;
bbs=[xs-floor(bbw/2) ys-floor(bbh/2)];
bbs(:,3)=bbw; bbs(:,4)=bbh; bbs(:,5)=M(ids);
end

function M = toMask( bbs, w, h, fill, bgrd )
% Create weighted mask encoding bb centers (or extent).
%
% USAGE
%  M = bbApply('toMask',bbs,w,h,[fill],[bgrd])
%
% INPUTS
%  bbs    - bounding boxes
%  w      - mask target width
%  h      - mask target height
%  fill   - [0] if 1 encodes extent of bbs
%  bgrd   - [0] default value for background pixels
%
% OUTPUTS
%  M      - hxw mask
%
% EXAMPLE
%
% See also bbApply, bbApply>frMask
if(nargin<4||isempty(fill)), fill=0; end
if(nargin<5||isempty(bgrd)), bgrd=0; end
if(size(bbs,2)==4), bbs(:,5)=1; end
M=zeros(h,w); B=true(h,w); n=size(bbs,1);
if( fill==0 )
  p=floor(getCenter(bbs)); p=sub2ind([h w],p(:,2),p(:,1));
  for i=1:n, M(p(i))=M(p(i))+bbs(i,5); end
  if(bgrd~=0), B(p)=0; end
else
  bbs=[intersect(round(bbs),[1 1 w h]) bbs(:,5)]; n=size(bbs,1);
  x0=bbs(:,1); x1=x0+bbs(:,3)-1; y0=bbs(:,2); y1=y0+bbs(:,4)-1;
  for i=1:n, y=y0(i):y1(i); x=x0(i):x1(i);
    M(y,x)=M(y,x)+bbs(i,5); B(y,x)=0; end
end
if(bgrd~=0), M(B)=bgrd; end
end


%% Test Problems for SEDUMI Interface
clc
clear

%Check SEDUMI is available
if(~optiSolver('sedumi',0))
    fprintf(2,'\n\nSEDUMI IS NOT AVAILABLE, NO SEDUMI TESTS CAN BE RUN\n\n');
    return;
end

files = {'sdp_truss1.mat','sdp_truss2.mat','sdp_truss3.mat','sdp_truss4.mat','sdp_hinf01.mat',...
         'sdp_hinf02.mat','sdp_hinf03.mat','sdp_hinf04.mat','sdp_arch0.mat','sdp_arch2.mat',...
         'sdp_arch4.mat','sdp_qap05.mat','sdp_qap06.mat','sdp_qap07.mat','sdp_qap08.mat',...
         'sdp_mcp100.mat','sdp_mcp124-1.mat','sdp_mcp124-2.mat','sdp_mcp124-3.mat','sdp_mcp124-4.mat',...
         'sdp_mcp250-1.mat','sdp_mcp250-2.mat','sdp_mcp250-3.mat','sdp_mcp250-4.mat'};

len = length(files);
cfval = zeros(len,1);
sfval = zeros(len,1);
optsC = optiset('solver','csdp','maxtime',15);
optsS = optiset('solver','sedumi','maxtime',15);

fprintf('SEDUMI SDP Checking\n');
%For each problem
for i = 1:len
    %Read In Problem
    prob = sdpRead(files{i});
    %Solve
    fprintf('Running Problem %s....',files{i});
    [~,cfval(i)] = solve(opti(prob,optsC));
    [~,sfval(i)] = solve(opti(prob,optsS));
    fprintf(' Done\n');
end

%Display fvals
display([cfval sfval])
if(norm(cfval-sfval) > 1e-1)
    error('Failed SeDuMi Checking');
else
    fprintf('Passed SeDuMi Checking\n');
end

% %% Complex Checking
% % Create a positive (semi)definite matrix:
% R=randn(5)+j*randn(5); R=R*R';
% % Make sure it's Hermitian:
% R=(R+R')/2;
%
% % Primal formulation
% clear At b c K
% c=vec(eye(5));
% At=vec(R);
% b=1;
% K.s=5;
% K.scomplex=1;
%
% [x,y,info]=sedumi(At,b,c,K);
%
% % Normed eigenvector:
% e=x(1:5)/norm(x(1:5));
% % Verify the eigenvalue
% [real(e'*R*e), max(eig(R))]


function test_suite=test_map_pca
% tests for cosmo_map_pca
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #
    try % assignment of 'localfunctions' is necessary in Matlab >= 2016
        test_functions=localfunctions();
    catch % no problem; early Matlab versions can use initTestSuite fine
    end
    initTestSuite;

function test_map_pca_exceptions
    aet=@(varargin)assertExceptionThrown(@()...
                        cosmo_map_pca(varargin{:}),'');

    ds=cosmo_synthetic_dataset();
    pca_params=struct();
    pca_params.mu=[0.0864 0.9248 -0.2001 1.2522 1.0349 0.0568];
    pca_params.coef=[ 0.0014    0.7569    0.3369   -0.0540    0.3234;...
                      0.4052   -0.5164    0.4105   -0.3355   -0.0290;...
                      0.3133    0.0275    0.6618    0.0901    0.1745;...
                     -0.4294   -0.2190   -0.0420   -0.5306    0.6797;...
                      0.7090    0.0436   -0.5070    0.0744    0.4815;...
                     -0.2251   -0.3313    0.1456    0.7677    0.4127];
    pca_params.retain=true(1,5);

    % test mutually exclusive parameters
    aet(ds,'pca_params',pca_params,'pca_explained_count',3);
    aet(ds,'pca_explained_ratio',.5,'pca_explained_count',3);
    aet(ds,'pca_explained_ratio',.5,'pca_params',pca_params);

    % wrong size for coef
    bad_pca_params=pca_params;
    bad_pca_params.coef=1;
    aet(ds,'pca_params',bad_pca_params);

    % missing field
    bad_pca_params=pca_params;
    bad_pca_params=rmfield(bad_pca_params,'mu');
    aet(ds,'pca_params',bad_pca_params);

    % bad explained_count
    aet(ds,'pca_explained_count',-1);
    aet(ds,'pca_explained_count',1.5);
    aet(ds,'pca_explained_count',7);

    % bad explained ratio
    aet(ds,'pca_explained_ratio',-1);
    aet(ds,'pca_explained_ratio',-.001);
    aet(ds,'pca_explained_ratio',1.001);

function test_map_pca_max_feature_count_exceptions()
    default_max_feature_count=1000;

    nsamples=20;
    for max_feature_count=[NaN, ...
                            round(rand()*10+10), ...
                            default_max_feature_count]
        for delta=(-1:1)
            opt=struct();

            if isnan(max_feature_count)
                nfeatures=default_max_feature_count+delta;
            else
                nfeatures=max_feature_count+delta;
                opt.max_feature_count=max_feature_count;
            end

            data=randn(nsamples,nfeatures);

            expect_exception=delta>0;

            for use_struct=[false,true]
                if use_struct
                    ds=struct();
                    ds.samples=data;
                else
                    ds=data;
                end

                func_handle=@()cosmo_map_pca(ds,opt);

                if expect_exception
                    assertExceptionThrown(func_handle);
                else
                    % should not raise an exception
                    func_handle();
                end
            end
        end
    end

function test_map_pca_basics
    nfeatures=ceil(rand()*10+10);
    nsamples_train=ceil(rand()*10+10)+nfeatures;
    nsamples_test=ceil(rand()*10+10)+nfeatures;

    train_samples=randn(nsamples_train,nfeatures);
    test_samples=randn(nsamples_test,nfeatures);

    for count=[1 ceil(nfeatures/2) nfeatures ceil(rand()*nfeatures)]
        helper_test_map_pca_with_count(train_samples,...
                                                test_samples,count);
    end

    for ratio=[.1 .5 1 rand()]
        helper_test_map_pca_with_ratio(train_samples,...
                                                test_samples,ratio);
    end

    helper_test_map_pca_with_no_arguments(train_samples,...
                                                test_samples);

function helper_test_map_pca_with_ratio(train_samples,...
                                                test_samples,ratio)
    ratio_opt=struct();
    ratio_opt.pca_explained_ratio=ratio;

    % check that ratio and count give the same output
    [unused,param]=cosmo_pca(train_samples);
    count=find(cumsum(param.explained)>=ratio*100,1);
    if isempty(count)
        count=numel(param.explained);
    end

    count_opt=struct();
    count_opt.pca_explained_count=count;

    did_throw=true;
    try
        [samples_ratio,params_ratio]=cosmo_map_pca(train_samples,ratio_opt);
        did_throw=false;
    end

    if did_throw
        cosmo_map_pca(train_samples,count_opt)
        assertExceptionThrown(@()...
                cosmo_map_pca(train_samples,count_opt));
        assertExceptionThrown(@()...
                helper_test_dataset_samples_correspondence(train_samples,...
                                            ratio_opt));

    else
        [samples_count,params_count]=cosmo_map_pca(train_samples,...
                                            count_opt);

        assert_almost_equal(samples_ratio,samples_count);
        assert_almost_equal(params_ratio,params_count);

        % verify that raw samples and dataset structures give the same
        % result
        helper_test_dataset_samples_correspondence(train_samples,...
                                                    ratio_opt);
        helper_test_dataset_samples_correspondence(test_samples,...
                                                'pca_params',params_ratio);
    end

    % verify correct result by delegating to count
    if ~did_throw
        helper_test_map_pca_with_count(train_samples,test_samples,count);
    else
        assertExceptionThrown(@()...
            helper_test_map_pca_with_count(train_samples,test_samples,...
                                            count));
    end

function helper_test_map_pca_with_count(train_samples,test_samples,count)
    assert(numel(count)==1);
    opt=struct();
    opt.pca_explained_count=count;

    % do PCA in two ways; we assume that cosmo_pca works correctly, and
    % compare its result to cosmo_map_pca
    [ys,params]=cosmo_map_pca(train_samples,opt);
    [pca_samples,pca_params]=cosmo_pca(train_samples,count);

    assert_almost_equal(ys,pca_samples);

    % verify when using samples
    helper_test_dataset_samples_correspondence(train_samples, ...
                                    'pca_explained_count',count);

    % verify params
    expected_params=struct();
    expected_params.mu=pca_params.mu;
    expected_params.coef=pca_params.coef;
    nfeatures=size(train_samples,2);
    expected_params.retain=(1:nfeatures)<=count;
    assert_almost_equal(expected_params,params);

    [zs,params2]=cosmo_map_pca(test_samples,'pca_params',params);
    assertEqual(params2,params);

    expected_samples=bsxfun(@minus,test_samples,params.mu)*params.coef;
    assert_almost_equal(zs,expected_samples);

    helper_test_dataset_samples_correspondence(test_samples,...
                                'pca_params',params);

function helper_test_map_pca_with_no_arguments(train_samples,test_samples)
    [ys,params]=cosmo_map_pca(train_samples);

    count=size(train_samples,2);
    [ys_count,params_count]=cosmo_map_pca(train_samples,...
                                    'pca_explained_count',count);
    assert_almost_equal(ys,ys_count);
    assert_almost_equal(params,params_count);

    helper_test_dataset_samples_correspondence(train_samples);
    helper_test_dataset_samples_correspondence(test_samples,...
                                            'pca_params',params);

function helper_test_dataset_samples_correspondence(samples, varargin)
    ds=struct();
    ds.samples=samples;

    [result_ds,param_ds]=cosmo_map_pca(ds,varargin{:});
    [result,param]=cosmo_map_pca(samples,varargin{:});

    % verify that output is the same
    expected_ds=struct();
    expected_ds.samples=result;

    expected_ds.fa.comp=1:size(result,2);
    expected_ds.a.fdim.labels={'comp'};
    expected_ds.a.fdim.values={1:size(samples,2)};

    assert_almost_equal(expected_ds,result_ds);

    % parameters must be identical
    assert_almost_equal(param_ds,param);

function assert_almost_equal(x,y,msg,tol)
    % helper that supports structs recursively
    if nargin<4
        tol=1e-5;
    end
    if nargin<3
        msg='';
    end

    if isstruct(x)
        assertTrue(isstruct(y),[msg ' - x is a struct but y is not']);

        fns=fieldnames(x);
        assertEqual(sort(fns),sort(fieldnames(y)),...
                                    [msg ' - fieldname mismatch'])

        n=numel(fns);
        for k=1:n
            fn=fns{k};

            assert_almost_equal(x.(fn),y.(fn),...
                            sprintf('%s - mismatch for field %s',msg,fn))
        end

    elseif iscell(x) && iscell(y)
        assertEqual(size(x),size(y),[msg ' - cell size mismatch']);
        for k=1:numel(x)
            assert_almost_equal(x{k},y{k},...
                        sprintf(' - cell element %d different',k));
        end
    elseif ischar(x) && ischar(y)
        assertEqual(x,y,sprintf('%s - %s ~= %s',msg,x,y));
    elseif islogical(x) && islogical(y)
        assertEqual(x,y,sprintf('%s - boolean array mismatch',msg));
    elseif isnumeric(x) && isnumeric(y)
        if isempty(x) && isempty(y)
            return;
        end
        assertElementsAlmostEqual(x,y,'absolute',tol,[msg ' - different']);
    else
        error('unsupported data type');
    end

function test_pca_map_ratio_unity()
    sizes=[4,10;...
           10,4;...
           4,4];

    for k=size(sizes,1);
        sz=sizes(k,:);
        samples=randn(sz);
        [xs,params]=cosmo_map_pca(samples,'pca_explained_ratio',1);

        assertEqual(params.retain,true(1,sz(end)-1));
    end


function used = getvariables(F)

F = flatten(F);
if length(F.clauses) == 0
    used = [];
    return
end

if isa(F.clauses{1},'cell')
    F = flatten(F);
end

used = recursivegetvariables(F,1,length(F.clauses));

function used = recursivegetvariables(F,startindex,endindex)

if endindex-startindex>50
    newstart = startindex;
    mid = ceil((startindex + endindex)/2);
    newend = endindex;
    used1 = recursivegetvariables(F,newstart,mid);
    used2 = recursivegetvariables(F,mid+1,newend);
    used = uniquestripped([used1 used2]);
else
    used = [];
    if startindex <= length(F.clauses)

        if F.clauses{startindex}.type == 56
            used = [];
            for j = 1:length(F.clauses{startindex}.data)
                used = [used getvariables(F.clauses{startindex}.data{j})];
            end
        else
            used = getvariables(F.clauses{startindex}.data);
        end

        for i = startindex+1:endindex

            %Fivars = getvariables(F.clauses{i}.data);
            if F.clauses{i}.type == 56
                % Meta constraint such as implies. This object is just holding
                % the data involved
                Fivars = [];
                for j = 1:length(F.clauses{i}.data)
                    Fivars = [Fivars getvariables(F.clauses{i}.data{j})];
                end
            else
                Fivars = getvariables(F.clauses{i}.data);
            end

            if ~isequal(used,Fivars(:)')
                used = [used Fivars(:)'];
            end
        end
        used = uniquestripped(used);
    end
end


%--- help for generic/stoch_simul ---
%
%  stoch_simul -- attempts to emulate dynare's stoch_simul
%
%  ::
%
%     oo_=stoch_simul(obj)
%
%     oo_=stoch_simul(obj,var_list)
%
%     oo_=stoch_simul(obj,var_list,varargin)
%
%  Args:
%
%     obj (dsge | rise ): model object
%
%     var_list (empty | char | cellstr ): List of variables for which to run
%       stoch_simul
%
%     varargin : Pairwise list of extra arguments
%
%  Returns:
%     :
%
%     - **oo_** [struct]: structure containing
%        - **irfs** [struct]: structure containing impulse responses in time
%          series format
%        - **simulations** [struct]: structure containing impulse responses
%          in time series format
%        - **vcov** [matrix]: (simulated) variance-covariance
%        - **skewness** [vector]: skewness measure
%        - **kurtosis** [vector]: kurtosis measure
%        - **variance** [vector]: (simulated) variances for the endogenous
%          variables
%        - **stdev** [vector]: (simulated) standard deviations for the endogenous
%          variables
%        - **corrcoef** [matrix]: (simulated) correlation coefficients for
%          the endogenous variables
%
%


%SerialLink.ikine Numerical inverse kinematics
%
% Q = R.ikine(T) are the joint coordinates (1xN) corresponding to the robot
% end-effector pose T (4x4) which is a homogenenous transform.
%
% Q = R.ikine(T, Q0, OPTIONS) specifies the initial estimate of the joint
% coordinates.
%
% This method can be used for robots with 6 or more degrees of freedom.
%
% Underactuated robots::
%
% For the case where the manipulator has fewer than 6 DOF the solution
% space has more dimensions than can be spanned by the manipulator joint
% coordinates.
%
% Q = R.ikine(T, Q0, M, OPTIONS) similar to above but where M is a mask
% vector (1x6) which specifies the Cartesian DOF (in the wrist coordinate
% frame) that will be ignored in reaching a solution.  The mask vector
% has six elements that correspond to translation in X, Y and Z, and rotation
% about X, Y and Z respectively.  The value should be 0 (for ignore) or 1.
% The number of non-zero elements should equal the number of manipulator DOF.
%
% For example when using a 3 DOF manipulator rotation orientation might be
% unimportant in which case  M = [1 1 1 0 0 0].
%
% For robots with 4 or 5 DOF this method is very difficult to use since
% orientation is specified by T in world coordinates and the achievable
% orientations are a function of the tool position.
%
% Trajectory operation::
%
% In all cases if T is 4x4xM it is taken as a homogeneous transform sequence
% and R.ikine() returns the joint coordinates corresponding to each of the
% transforms in the sequence.  Q is MxN where N is the number of robot joints.
% The initial estimate of Q for each time step is taken as the solution
% from the previous time step.
%
% Options::
% 'pinv'         use pseudo-inverse instead of Jacobian transpose (default)
% 'ilimit',L     set the maximum iteration count (default 1000)
% 'tol',T        set the tolerance on error norm (default 1e-6)
% 'alpha',A      set step size gain (default 1)
% 'varstep'      enable variable step size if pinv is false
% 'verbose'      show number of iterations for each point
% 'verbose=2'    show state at each iteration
% 'plot'         plot iteration state versus time
%
% References::
% - Robotics, Vision & Control, Section 8.4,
%   P. Corke, Springer 2011.
%
% Notes::
% - Solution is computed iteratively.
% - Solution is sensitive to choice of initial gain.  The variable
%   step size logic (enabled by default) does its best to find a balance
%   between speed of convergence and divergence.
% - Some experimentation might be required to find the right values of
%   tol, ilimit and alpha.
% - The pinv option leads to much faster convergence (default)
% - The tolerance is computed on the norm of the error between current
%   and desired tool pose.  This norm is computed from distances
%   and angles without any kind of weighting.
% - The inverse kinematic solution is generally not unique, and
%   depends on the initial guess Q0 (defaults to 0).
% - The default value of Q0 is zero which is a poor choice for most
%   manipulators (eg. puma560, twolink) since it corresponds to a kinematic
%   singularity.
% - Such a solution is completely general, though much less efficient
%   than specific inverse kinematic solutions derived symbolically, like
%   ikine6s or ikine3.
% - This approach allows a solution to be obtained at a singularity, but
%   the joint angles within the null space are arbitrarily assigned.
% - Joint offsets, if defined, are added to the inverse kinematics to
%   generate Q.
% - Joint limits are not considered in this solution.
%
% See also SerialLink.ikcon, SerialLink.ikunc, SerialLink.fkine, SerialLink.ikine6s.

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

function [qt,histout] = ikine(robot, tr, varargin)

    %  set default parameters for solution
    opt.ilimit = 1000;
    opt.tol = 1e-6;
    opt.alpha = 0.9;
    opt.plot = false;
    opt.pinv = true;
    opt.varstep = false;

    [opt,args] = tb_optparse(opt, varargin);

    n = robot.n;

    % robot.ikine(tr, q)
    if ~isempty(args)
        q = args{1};
        if isempty(q)
            q = zeros(1, n);
        else
            q = q(:)';
        end
    else
        q = zeros(1, n);
    end

    % robot.ikine(tr, q, m)
    if length(args) > 1
        m = args{2};
        m = m(:);
        if numel(m) ~= 6
            error('RTB:ikine:badarg', 'Mask matrix should have 6 elements');
        end
        if numel(find(m)) > robot.n
            error('RTB:ikine:badarg', 'Number of robot DOF must be >= the same number of 1s in the mask matrix')
        end
    else
        if n < 6
            error('RTB:ikine:badarg', 'For a manipulator with fewer than 6DOF a mask matrix argument must be specified');
        end
        m = ones(6, 1);
    end
    % make this a logical array so we can index with it
    m = logical(m);

    npoints = size(tr,3);    % number of points
    qt = zeros(npoints, n);  % preallocate space for results
    tcount = 0;              % total iteration count

    if ~ishomog(tr)
        error('RTB:ikine:badarg', 'T is not a homog xform');
    end

    J0 = jacob0(robot, q);
    J0 = J0(m, :);
    if cond(J0) > 100
        warning('RTB:ikine:singular', 'Initial joint configuration results in a (near-)singular configuration, this may slow convergence');
    end

    history = [];
    failed = false;
    e = zeros(6,1);
    revolutes = [robot.links.sigma] == 0;

    for i=1:npoints
        T = tr(:,:,i);

        nm = Inf;
        % initialize state for the ikine loop
        eprev = [Inf Inf Inf Inf Inf Inf];
        save.e = [Inf Inf Inf Inf Inf Inf];
        save.q = [];
        count = 0;

        while true
            % update the count and test against iteration limit
            count = count + 1;
            if count > opt.ilimit
                warning('ikine: iteration limit %d exceeded (row %d), final err %f', ...
                    opt.ilimit, i, nm);
                failed = true;
                %q = NaN*ones(1,n);
                break
            end

            % compute the error
            Tq = robot.fkine(q');

            e(1:3) = transl(T - Tq);
            Rq = t2r(Tq);
            [th,n] = tr2angvec(Rq'*t2r(T));
            e(4:6) = th*n;

            % optionally adjust the step size
            if opt.varstep
                % test against last best error, only consider the DOF of
                % interest
                if norm(e(m)) < norm(save.e(m))
                    % error reduced,
                    % let's save current state of solution and rack up the step size
                    save.q = q;
                    save.e = e;
                    opt.alpha = opt.alpha * (2.0^(1.0/8));
                    if opt.verbose > 1
                        fprintf('step %d: raise alpha to %f\n', count, opt.alpha);
                    end
                else
                    % rats!  error got worse,
                    % restore to last good solution and reduce step size
                    q = save.q;
                    e = save.e;
                    opt.alpha = opt.alpha * 0.5;
                    if opt.verbose > 1
                        fprintf('step %d: drop alpha to %f\n', count, opt.alpha);
                    end
                end
            end

            % compute the Jacobian
            J = jacob0(robot, q);

            % compute change in joint angles to reduce the error,
            % based on the square sub-Jacobian
            if opt.pinv
                %pinv( J(m,:) )
                dq = opt.alpha * pinv( J(m,:) ) * e(m);
            else
                dq = J(m,:)' * e(m);
                dq = opt.alpha * dq;
            end

            % diagnostic stuff
            if opt.verbose > 1
                fprintf('%d/%d: |e| = %f\n', i, count, nm);
                fprintf('       e  = '); disp(e');
                fprintf('       dq = '); disp(dq');
            end
            if opt.plot
                h.q = q';
                h.dq = dq;
                h.e = e;
                h.ne = nm;
                h.alpha = opt.alpha;
                history = [history; h]; %#ok<*AGROW>
            end

            % update the estimated solution
            q = q + dq';

            % wrap angles for revolute joints
            k = (q > pi) & revolutes;
            q(k) = q(k) - 2*pi;

            k = (q < -pi) & revolutes;
            q(k) = q(k) + 2*pi;

            nm = norm(e(m));

            if norm(e(m)) > 2*norm(eprev(m))
                warning('RTB:ikine:diverged', 'solution diverging at step %d, try reducing alpha', count);
            end
            eprev = e;

            if nm <= opt.tol
                break
            end

        end  % end ikine solution for tr(:,:,i)
        qt(i,:) = q';
        tcount = tcount + count;
        if opt.verbose
            fprintf('%d iterations\n', count);
        end
    end

    if opt.verbose && npoints > 1
        fprintf('TOTAL %d iterations\n', tcount);
    end

    % plot evolution of variables
    if opt.plot
        figure(1);
        subplot(511)
        plot([history.q]');
        ylabel('q');
        grid

        subplot(512)
        plot([history.dq]');
        ylabel('dq');
        grid

        subplot(513)
        plot([history.e]');
        ylabel('e');
        grid

        subplot(514)
        semilogy([history.ne]);
        ylabel('|e|');
        grid

        subplot(515)
        plot([history.alpha]);
        xlabel('iteration');
        ylabel('\alpha');
        grid

        if nargout > 1
            histout = history;
        end
    end
end


function f = conj(f)
%CONJ   Complex conjugate of a SEPARABLEAPPROX.
%   CONJ(F) returns the complex conjugate of F.  For a complex F, CONJ(F) =
%   REAL(F) - i*IMAG(F).
%
% See also REAL, IMAG.

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

% Check for empty SEPARABLEAPPROX.
if ( isempty( f ) )
   return
end

% TODO: Write down the formulas for conj, instead of calling the
% constructor.

f = compose( f, @conj );

end


function A = import_data(fname)
%IMPORT_DATA Import tensor-related data to a file.
%
%   A = IMPORT_DATA(FNAME) imports an object A from the file named FNAME.
%   The supported data types and formatting of the file are explained in
%   EXPORT_DATA.
%
%   See also TENSOR, EXPORT_DATA
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2015) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%% Open file
fid = fopen(fname,'r');
if (fid == -1)
    error('Cannot open file %s',fname);
end

%% Get the type of object
type = import_type(fid);

%% Import the object

if strcmpi(type,'tensor')

    sz = import_size(fid);
    data = import_array(fid, prod(sz));
    A = tensor(data, sz);

elseif strcmpi(type,'sptensor')

    sz = import_size(fid);
    nz = import_nnz(fid);
    [subs, vals] = import_sparse_array(fid, length(sz), nz);
    A = sptensor(subs, vals, sz);

elseif strcmpi(type,'matrix') || strcmpi(type,'matrix')

    sz = import_size(fid);
    data = import_array(fid, prod(sz));
    A = reshape(data, sz);

elseif strcmpi(type,'ktensor')

    sz = import_size(fid);
    r = import_rank(fid);
    lambda = import_array(fid, r);
    U = {};
    for n = 1:length(sz)
        line = fgets(fid);
        fac_type = import_type(fid);
        fac_sz = import_size(fid);
        fac_data = import_array(fid, prod(fac_sz));
        % row wise reshape
        fac = reshape(fac_data, fliplr(fac_sz))';
        U{n} = fac;
    end
    A = ktensor(lambda,U);

else

    error('Invalid data type for export');

end

%% Close file
fclose(fid);

function type = import_type(fid)
% Import IO data type
line = fgets(fid);
typelist = regexp(line, '\s+', 'split');
type = typelist(1);

function sz = import_size(fid)
% Import the size of something from a file
line = fgets(fid);
n = sscanf(line, '%d');
line = fgets(fid);
sz = sscanf(line, '%d');
sz = sz';
if (size(sz,2) ~= n)
    error('Imported dimensions are not of expected size');
end

function nz = import_nnz(fid)
% Import the size of something from a file
line = fgets(fid);
nz = sscanf(line, '%d');

function r = import_rank(fid)
% Import the rank of something from a file
line = fgets(fid);
r = sscanf(line, '%d');

function data = import_array(fid, n)
% Import dense data that supports numel and linear indexing
data = fscanf(fid, '%e', n);

function [subs, vals] = import_sparse_array(fid, n, nz)
% Import sparse data subs and vals from coordinate format data
data = textscan(fid,[repmat('%d',1,n) '%n']);
subs = cell2mat(data(1:n));
vals = data{n+1};
if (size(subs,1) ~= nz)
    error('Imported nonzeros are not of expected size');
end