function representa(robot,qt1,qt2,num,coord,num2)
%REPRESENTA LOS DOS ROBOTS Y LA CAJA SIGUIENDO LA TRAYECTORIA

for i=1:size(qt1,2)

    if num==2

        T= directkinematic(robot.robot1, qt1(:,i));
        Pc = T(1:3,4);
        tangente=atan(T(2,3)/T(1,3));
        robot.robot1.equipment{1}.T0(1:3,4) = Pc;
        robot.robot1.equipment{1}.T0(3,4)=robot.robot1.equipment{1}.T0(3,4)-coord(3,1);
        if num2==1

         robot.robot1.equipment{1}.T0(2,4)=robot.robot1.equipment{1}.T0(2,4)+norm(coord(1:2,1))*sin(tangente);
         robot.robot1.equipment{1}.T0(1,4)=robot.robot1.equipment{1}.T0(1,4)+norm(coord(1:2,1))*cos(tangente);

        robot.robot1.equipment{1}.T0(1:3,1:3)=T(1:3,1:3)*[0 -1  0;
                                                          1  0  0;
                                                          0  0  1];
        else
           robot.robot1.equipment{1}.T0(2,4)=robot.robot1.equipment{1}.T0(2,4)-norm(coord(1:2,1))*sin(tangente);
         robot.robot1.equipment{1}.T0(1,4)=robot.robot1.equipment{1}.T0(1,4)+norm(coord(1:2,1))*cos(tangente);
        end

    end
      draw2robots(robot.robot1,qt1(:,i),robot.robot2,qt2(:,i))

end

end


function plotFeatureImportanceRFvolClin_HN(pathRF)

startpath = pwd;

cd(pathRF), load('training'), load('testingVariableImportance')
nameOutcomes = fieldnames(training.outcomes); nOutcomes = numel(nameOutcomes);
% fSetNames = fieldnames(training.textures.(nameOutcomes{1})); nFset = numel(fSetNames);
fSetNames = {'volClinical'}; nFset = numel(fSetNames);

for o = 1:nOutcomes
    for f = 1:nFset
        percentAUCdecrease = variableImportance.(nameOutcomes{o}).(fSetNames{f}).percentAUCdecrease;
        varNames = variableImportance.(nameOutcomes{o}).(fSetNames{f}).varNames; nVar = numel(varNames);
        figure
        barh(1:nVar,percentAUCdecrease*100)
        for i = 1:nVar
            ind = strfind(varNames{i},'_'); nInd = numel(ind);
            if ~isempty(ind)
                for n = 1:nInd
                    varNames{i} = [varNames{i}(1:(ind(n)-1+(n-1))),'\',varNames{i}((ind(n)+(n-1)):end)];
                end
            end
        end
        set(gca,'yticklabel',varNames)
        nameOutcome = nameOutcomes{o};
        ind = strfind(nameOutcome,'Death');
        if ~isempty(ind)
            nameOutcome(ind:ind+4) = [];
            nameOutcome = ['Survival',nameOutcome];
        end
        title(['RANDOM PERMUTATIONS (RF):',nameOutcome,' -- ',fSetNames{f}])
        xlabel('Percent AUC decrease')
    end
end

cd(startpath)
end


function outputImage = smooth(this, varargin)
% Smoothes image (or image time series) spatially with sinc-truncated Gaussian kernel,
% i.e., using spm_smooth and mimicking its functionality
%
%   Y = MrImageSpm4D()
%   sY = Y.smooth('fwhm', fwhm)
%
% outputImage is a method of class MrImageSpm4D.
%
% NOTE: For complex-valued data, use MrImage.smooth
%
% IN
%   fwhm    [1,1] or [1,3] Full width at half maximum of Gaussian kernel (in mm)
%           if single value is given, isotropic kernel is assumed
%           default: 8
%
% OUT
%
% EXAMPLE
%   smooth
%
%   See also MrImageSpm4D MrImage.smooth spm_smooth

% Author:   Saskia Klein & Lars Kasper
% Created:  2014-07-02
% Copyright (C) 2014 Institute for Biomedical Engineering
%                    University of Zurich and ETH Zurich
%
% outputImage file is part of the TAPAS UniQC 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/>.

defaults.fwhm = 8;

args = tapas_uniqc_propval(varargin, defaults);
tapas_uniqc_strip_fields(args);

if length(fwhm) == 1
    fwhm = fwhm*[ 1 1 1];
end

outputImage = this.copyobj;

% save image file for processing as nii in SPM
outputImage.save('fileName', outputImage.get_filename('prefix', 'raw'));

matlabbatch = outputImage.get_matlabbatch('smooth', fwhm);
save(fullfile(outputImage.parameters.save.path, 'matlabbatch.mat'), ...
            'matlabbatch');
spm_jobman('run', matlabbatch);

% clean up: move/delete processed spm files, load new data into matrix
outputImage.finish_processing_step('smooth');


function opt_option_out = fcnChooseOption(opt_option,tol_opt, u0)
% specify and configure optimization method
%refer to goo.gl/aTQFwr
%(http://www.mathworks.com/help/optim/ug/fmincon.html#inputarg_options)
    warning off all;
    if ( opt_option == 0 )
        opt_option_out = optimset('Display','off',...
            'TolFun', tol_opt,...
            'MaxIter', 10000,...
            'Algorithm', 'active-set',...
            'FinDiffType', 'forward',...
            'RelLineSrchBnd', [],...
            'RelLineSrchBndDuration', 1,...
            'TolConSQP', 1e-6);
    elseif ( opt_option == 1 )
        opt_option_out = optimset('Display','off',...
            'TolFun', tol_opt,...
            'MaxIter', 2000,...
            'Algorithm', 'interior-point',...
            'AlwaysHonorConstraints', 'bounds',...
            'FinDiffType', 'forward',...
            'HessFcn', [],...
            'Hessian', 'bfgs',...
            'HessMult', [],...
            'InitBarrierParam', 0.1,...
            'InitTrustRegionRadius', sqrt(size(u0,1)*size(u0,2)),...
            'MaxProjCGIter', 2*size(u0,1)*size(u0,2),...
            'ObjectiveLimit', -1e20,...
            'ScaleProblem', 'obj-and-constr',...
            'SubproblemAlgorithm', 'cg',...
            'TolProjCG', 1e-2,...
            'TolProjCGAbs', 1e-10);
    %                       'UseParallel','always',...
    elseif ( opt_option == 2 )
        opt_option_out = optimset('Display','off',...
            'TolFun', tol_opt,...
            'MaxIter', 2000,...
            'Algorithm', 'trust-region-reflective',...
            'Hessian', 'off',...
            'MaxPCGIter', max(1,floor(size(u0,1)*size(u0,2)/2)),...
            'PrecondBandWidth', 0,...
            'TolPCG', 1e-1);
    end

end


%% Select Grain Boundaries
%
%%
% In this section we explain how to extract specific grain boundaries.
% Therefore we start by importing some EBSD data and reconstructing the
% grain structure.

close all; plotx2east

% import the data
mtexdata forsterite silent

% restrict it to a subregion of interest.
ebsd = ebsd(inpolygon(ebsd,[5 2 10 5]*10^3));

[grains,ebsd.grainId] = calcGrains(ebsd('indexed'));

% remove very small grains
ebsd(grains(grains.grainSize <= 5)) = [];

% and recompute grains
[grains,ebsd.grainId] = calcGrains(ebsd('indexed'));

% smooth the grains a bit
grains = smooth(grains,4);

% visualize as a phase map
plot(ebsd)
hold on
plot(grains.boundary,'linewidth',2)
hold off

%%
% The output of

grains.boundary

%%
% tells us the number of boundary segments between the different phsaes.
% Those segments with notIndexed phase include also those boundary segments
% where the grains are cutted by the scanning boundary. To restrict the
% grain boundaries to a specific phase transistion you shall do

hold on
plot(grains.boundary('Fo','Fo'),'lineColor','blue','micronbar','off','lineWidth',2)
hold off

%%
% Similarly we may select all Forsterite to enstatite boundary segements.

hold on
plot(grains.boundary('Fo','En'),'lineColor','darkgreen','micronbar','off','lineWidth',2)
hold off

%%
% Note, that the order of the phase names matter when considering the
% corresponding misorintations

grains.boundary('Fo','En').misorientation(1)
grains.boundary('En','Fo').misorientation(1)

%%
% In the fist case the misorientation returned is from Forsterite to
% Enstatite and in the second case its exactly the inverse

%%
% The selection of grain boundaries according to specific misorientationsm
% according to twist / tild character or twinning is explained in linked
% sections.
%


filename = 'CantileverSquare';%'CantileverSquareSYmmetricMesh';%'CantileverSquareNewFine';%'LshapeTriFine';%'CantileverSquareNew';'ArchTriFine';%'CantileverSquare';%'ArchTriFine';%'CantileverSquare';%'Lshape';%'LshapeTriFine';%'CantileverSquareSmall';%'';%'Lshape';'CantileverSquare';'Lshape';%'CantileverSquare';%'LshapeFine';%'Bridge_quad_coarse';%'Arch_quad_coarse';%'BridgeCool_Quadrilateral_Bilinear_Structured_Coarse';%'Bridge';%'CantileverSquareSmall';
ptype = 'MACRO';
initial_case = 'given';
m1 = 0.0101;
m2 = 0.0101;
%cost = {'compliance'};
cost = {'stressNorm'};
%cost = {'stressNorm','compliance'};
%weights = [0.55,0.45];
weights = 1;
constraint = {'volumeConstraint'};
filterType = 'P1';
constraint_case = 'EQUALITY';

Vfrac_initial = 0.3;
optimality_initial = 1e-5;
constr_initial = 1e-5;

Vfrac_final = 0.3;
optimality_final = 1e-5;
constr_final = 1e-5;

stressNormExponent_initial = 2;
stressNormExponent_final = 64;

optimizer = 'DualNestedInPrimal';
optimizerUnconstrained = 'PROJECTED GRADIENT';

designVariable = 'MicroParams';
ub = 0.989;
lb = 0.011;
homegenizedVariablesComputer = 'ByVademecum';
%
%vademecumFileName = 'SuperEllipseQMax';
%vademecumFileName = 'SuperEllipseQ2';
vademecumFileName = 'SuperEllipseQOptAnalytic';
%
% designVariable = 'Density';
% homegenizedVariablesComputer = 'ByInterpolation';
% method = 'SIMPALL';
% materialType = 'ISOTROPIC';

kfrac = 2;
nsteps = 20;

plotting = false;
printing = true;
monitoring = true;
monitoring_interval = 1;
maxiter = 500;


function x = binary(x)
%BINARY Constrains variables to be binary (0/1)
%
%   F = BINARY(x) is used to a posteriori constrain
%   variables to be binary, in contrast to BINVAR
%   that declares variables as binary a priori.
%
%   NOTE
%    The binary constraint is imposed on the involved
%    decision variables, not on the actual SDPVAR object.
%
%   INPUT
%    x : SDPVAR object
%
%   OUTPUT
%    F : Constraint object
%
%   EXAMPLE
%    F = binary(x);                  % Add integrality
%    F = binary(x*pi)                % Equivalent to code above
%
%   See also INTEGER, SET, SDPVAR, INTVAR, BINVAR

x.typeflag = 8;
x = lmi(x);


function [output,validFrameMask] = F_cmn(input_layer)
input = input_layer.a;
[D,T,N] = size(input);

if N==1
    output = CMN(input')';
    validFrameMask = [];
else
    [validFrameMask, variableLength] = getValidFrameMask(input_layer);
    if variableLength
        input2 = ExtractVariableLengthTrajectory(input, validFrameMask);
        precision = class(gather(input(1)));
        if IsInGPU(input)
            output = gpuArray.zeros(size(input), precision);
        else
            output = zeros(size(input), precision);
        end
        for i=1:N
            output(:,1:size(input2{i},2),i) = CMN(input2{i}')';
        end
    else
        input2 = reshape(permute(input, [1 3 2]), D*N,T);
        output = CMN(input2')';
        output = permute(reshape(output, D, N, T), [1 3 2]);
    end
end
end