% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function [img mat] = MU_load_nifti(fname)
% Load a NIfTI file in as a matlab marix.
% Supports a 4-D NIfTI File
% Format FUNCTION [img mat] = load_nifti(fname)
%
% Inputs:
%    fname - the name of the .nii file
%
% Outputs:
%    img - a 3d (or 4d) matlab image matrix
%    mat - affine matrix describing voxel size & position
%
% External Requirements: SPM (Tested with SPM8)
% Acknowledge to Samuel A. Hurley

% Check if its a .nii.gz
ftype = 0;

if strcmp(fname(length(fname)-2:length(fname)), '.gz');
  ftype = 1;  % gzipped image
  disp('Unzipping image...');
  eval(['!gunzip ' fname]);

  % Change the fname to reflect the un-zipped image
  fname = fname(1:length(fname)-3);
end

% Grab SPM header
D = spm_vol(fname);

% Preallocate img matrix
img = zeros([D(1).dim length(D)]);

% Grab the image
for ii = 1:length(D)
  img(:,:,:,ii) = spm_read_vols(D(ii));
end

% Re-orient the image
img = imflipud(imtranspose(img));

% Grab the mat
mat = D.mat;

% Check if the file needs to be re-zipped
if ftype == 1
  disp('Re-zipping image...');
  eval(['!gzip ' fname]);
end
% Done.

function imout = imtranspose(imin)

% Preallocate, but only for a square image
dims = size(imin);
if dims(1) == dims(2)
  imout = zeros(size(imin));
end

if ndims(imin) == 2
  imout = imin';

% 3-D Image Matrix
elseif ndims(imin) == 3
  zdim = size(imin, 3);

  if zdim == 1
    imout = imin';
  else
    for j = 1:zdim
      imout(:,:,j) = imin(:,:,j)';
    end
  end

% 4-D Image
elseif ndims(imin) == 4
  % Do some recursive magic!
  for ii = 1:size(imin, 4)
    imout(:,:,:,ii) = imtranspose(imin(:,:,:,ii));
  end

% Will we ever do a 5-D image?
else
  error('Does not support 5-D image');
end

% Flips a 3-d array in the up/down direction
function flipped = imflipud(im)

flipped = zeros(size(im));

% 3-D
if ndims(im) == 2
  flipped = flipud(im);

elseif ndims(im) == 3
  for j = 1:size(im,3)
    flipped(:,:,j) = flipud(im(:,:,j));
  end

% 4-D

elseif ndims(im) == 4
  for ii = 1:size(im, 4)
    flipped(:,:,:,ii) = imflipud(im(:,:,:,ii));
  end

else
  error('Does not support 5-D image');
end


function [D1]=triSurfSetDist(F1,V1,F2,V2,distMetric)

% function [D1]=triSurfSetDist(F1,V1,F2,V2,distMetric)
% ------------------------------------------------------------------------
%
% Change log:
% 2021/07/22 KMM Removed waitbar for ray-tracing method
% ------------------------------------------------------------------------

%%

switch distMetric
    case 'dist'
        D1=minDist(V1,V2);
    case 'ray'
        [~,~,N1]=patchNormal(F1,V1);

        %Ray trace 1 onto 2
        optionStructRayTrace.tolEps = 1e-6;
        optionStructRayTrace.triSide = 0;
        optionStructRayTrace.rayType = 'ray'; % or line
        optionStructRayTrace.exclusionType = 'normal'; % or exclusive
        optionStructRayTrace.paired=0; % 1 for paired, 0 for non-paired

        [~,indIntersect,d1_all]=triSurfRayTrace(V1,N1,F2,V2,optionStructRayTrace);

        numIntersect=size(indIntersect,1);

        S=sparse(indIntersect(:,1),(1:numIntersect)',1,size(V1,1),numIntersect,numIntersect);
        d1_sparse=sparse(indIntersect(:,1),(1:numIntersect)',abs(d1_all),size(V1,1),numIntersect,numIntersect);

        D1=full(spmin(d1_sparse,[],2,'includenan',S~=0,1));

    case 'dist-ray'
        [D1d]=triSurfSetDist(F1,V1,F2,V2,'dist');
        [D1r]=triSurfSetDist(F1,V1,F2,V2,'ray');
        D1=gnanmin([D1d D1r],[],2);
    case 'near-norm'
        [~,indMin]=minDist(V1,V2);
        [~,~,Nv]=patchNormal(F2,V2);
        N=Nv(indMin,:);
        W=V1-V2(indMin,:);
        D1=abs(dot(N,W,2));
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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 [mustLL, pos_mustLL, mustLL_linear, pos_mustLL_linear] = findMustLLWithGAMS(model, minFluxesW, maxFluxesW, constrOpt, excludedRxns, mustSetFirstOrder, solverName, runID, outputFolder, outputFileName, printExcel, printText, printReport, keepInputs, keepGamsOutputs, verbose)
% This function runs the second step of `optForce`, that is to solve a
% bilevel mixed integer linear programming problem to find a second order
% `MustLL` set.
%
% USAGE:
%
%    [mustLL, pos_mustLL, mustLL_linear, pos_mustLL_linear] = findMustLLWithGAMS(model, minFluxesW, maxFluxesW, varargin)
%
% INPUTS:
%    model:                     (structure) a metabolic model with at least
%                               the following fields:
%
%                                 * .rxns - Reaction IDs in the model
%                                 * .mets - Metabolite IDs in the model
%                                 * .S -    Stoichiometric matrix (sparse)
%                                 * .b -    RHS of `Sv = b` (usually zeros)
%                                 * .c -    Objective coefficients
%                                 * .lb -   Lower bounds for fluxes
%                                 * .ub -   Upper bounds for fluxes
%    minFluxesW:                (double array of size `n_rxns x 1`) minimum
%                               fluxes for each reaction in the model for
%                               wild-type strain. This can be obtained by
%                               running the function `FVAOptForce`. E.g.:
%                               `minFluxesW = [-90; -56];`
%    maxFluxesW:                (double array of size `n_rxns x 1`) maximum
%                               fluxes for each reaction in the model for
%                               wild-type strain. This can be obtained by
%                               running the function `FVA_optForce`. E.g.:
%                               `maxFluxesW = [90; 56];`
%
% OPTIONAL INPUTS:
%    constrOpt:                 (Structure) structure containing
%                               additional contraints. Include here only
%                               reactions whose flux is fixed, i.e.,
%                               reactions whose lower and upper bounds have
%                               the same value. Do not include here
%                               reactions whose lower and upper bounds have
%                               different values. Such contraints should be
%                               defined in the lower and upper bounds of
%                               the model. The structure has the following
%                               fields:
%
%                                 * .rxnList - Reaction list (cell array)
%                                 * .values -  Values for constrained
%                                   reactions (double array)
%                                   E.g.: `struct('rxnList', {{'EX_gluc', 'R75', 'EX_suc'}}, 'values', [-100, 0, 155.5]');`
%    excludedRxns:              (cell array) Reactions to be excluded to
%                               the `MustLL` set. This could be used to avoid
%                               finding transporters or exchange reactions
%                               in the set. Default = empty.
%    mustSetFirstOrder:         (cell array) Reactions that belong to `MustU`
%                               and `MustL` (first order sets). Default = empty.
%    solverName:                (string) Name of the solver used in
%                               GAMS. Default = 'cplex'.
%    runID:                     (string) ID for identifying this run.
%                               Default = ['run' date hour].
%    outputFolder:              (string) name for folder in which
%                               results will be stored. Default =
%                               'OutputsFindMustLL'.
%    outputFileName:            (string) name for files in which
%                               results will be stored. Default =
%                               'MustLLSet'.
%    printExcel:                (double) boolean to describe wheter
%                               data must be printed in an excel file or
%                               not. Default = 1
%    printText:                 (double) boolean to describe wheter
%                               data must be printed in an plaint text file
%                               or not. Default = 1
%    printReport:               (double) 1 to generate a report in a
%                               plain text file. 0 otherwise. Default = 1
%    keepInputs:                (double) 1 to mantain folder with
%                               inputs to run `findMustLL.gms`. 0 otherwise.
%                               Default = 1
%    keepGamsOutputs:           (double) 1 to mantain files returned by
%                               `findMustLL.gms`. 0 otherwise. Default = 1
%    verbose:                   (double) 1 to print results in console.
%                               0 otherwise. Default = 0
%
% OUTPUTS:
%    mustLL:                    (cell array of size number of sets found X
%                               2). Cell array containing the reactions IDs
%                               which belong to the `MustLL` set. Each row
%                               contain a couple of reactions that must
%                               decrease their flux.
%    pos_mustLL:                (double array of size number of sets found
%                               X 2). Double array containing the positions
%                               of each reaction in `mustLL` with regard to
%                               `model.rxns`
%    mustLL_linear:             (cell array of size number of unique
%                               reactions found X 1) Cell array containing
%                               the unique reactions ID which belong to the
%                               `MustLL` Set
%    pos_mustLL_linear:         (double array of size number of unique
%                               reactions found X 1) double array
%                               containing positions for reactions in
%                               `mustLL_linear`. with regard to `model.rxns`
%    outputFileName.xls:        (file) File containing one column
%                               array with identifiers for reactions in
%                               `MustLL`. This file will only be generated if
%                               the user entered `printExcel = 1`. Note that
%                               the user can choose the name of this file
%                               entering the input `outputFileName =
%                               'PutYourOwnFileNameHere';`
%    outputFileName.txt:        (file) File containing one column
%                               array with identifiers for reactions in
%                               `MustLL`. This file will only be generated if
%                               the user entered `printText = 1`. Note that
%                               the user can choose the name of this file
%                               entering the input `outputFileName =
%                               'PutYourOwnFileNameHere';`
%    outputFileName_Info.xls:   (file) File containing one column
%                               array. In each row the user will find a
%                               couple of reactions. Each couple of
%                               reaction was found in one iteration of
%                               `FindMustLL.gms`. This file will only be
%                               generated if the user entered `printExcel = 1`.
%                               Note that the user can choose the name
%                               of this file entering the input
%                               `outputFileName = 'PutYourOwnFileNameHere';`
%    outputFileName_Info.txt:   (file) File containing one column
%                               array. In each row the user will find a
%                               couple of reactions. Each couple of
%                               reaction was found in one iteration of
%                               `FindMustLL.gms`. This file will only be
%                               generated if the user entered `printText = 1`.
%                               Note that the user can choose the name
%                               of this file entering the input
%                               `outputFileName = 'PutYourOwnFileNameHere';`
%    findMustLL.lst:            (file) file autogenerated by GAMS. It
%                               contains information about equations,
%                               variables, parameters as well as
%                               information about the running (values at
%                               each iteration). This file only will be
%                               saved in the output folder if the user
%                               entered `keepGamsOutputs = 1`
%    GtoMLL.gdx:                (file) file containing values for
%                               variables, parameters, etc. which were
%                               found by GAMS when solving `findMustLL.gms`.
%                               This file only will be saved in the output
%                               folder if the user entered `keepInputs = 1`
%
% NOTE:
%
%    This function is based in the GAMS files written by Sridhar
%    Ranganathan which were provided by the research group of Costas D.
%    Maranas. For a detailed description of the `optForce` procedure, please
%    see: `Ranganathan S, Suthers PF, Maranas CD (2010) OptForce: An
%    Optimization Procedure for Identifying All Genetic Manipulations
%    Leading to Targeted Overproductions. PLOS Computational Biology 6(4):
%    e1000744`. https://doi.org/10.1371/journal.pcbi.1000744
%
% .. Author: - Sebastian Mendoza, May 30th 2017, Center for Mathematical Modeling, University of Chile, snmendoz@uc.cl

optionalParameters = {'constrOpt', 'excludedRxns', 'mustSetFirstOrder', 'solverName', 'runID', 'outputFolder', 'outputFileName',  ...
    'printExcel', 'printText', 'printReport', 'keepInputs', 'keepGamsOutputs', 'verbose'};

if (numel(varargin) > 0 && (~ischar(varargin{1}) || ~any(ismember(varargin{1},optionalParameters))))

    tempargin = cell(1,2*(numel(varargin)));
    for i = 1:numel(varargin)

        tempargin{2*(i-1)+1} = optionalParameters{i};
        tempargin{2*(i-1)+2} = varargin{i};
    end
    varargin = tempargin;

end

parser = inputParser();
parser.addRequired('model', @(x) isstruct(x) && isfield(x, 'S') && isfield(model, 'rxns')...
    && isfield(model, 'mets') && isfield(model, 'lb') && isfield(model, 'ub') && isfield(model, 'b')...
    && isfield(model, 'c'))
parser.addRequired('minFluxesW', @isnumeric)
parser.addRequired('maxFluxesW', @isnumeric)
parser.addParamValue('constrOpt', struct('rxnList', {{}}, 'values', []),@ (x) isstruct(x) && isfield(x, 'rxnList') && isfield(x, 'values') ...
    && length(x.rxnList) == length(x.values) && length(intersect(x.rxnList, model.rxns)) == length(x.rxnList))
parser.addParamValue('excludedRxns', {}, @(x) iscell(x) && length(intersect(x, model.rxns)) == length(x))
parser.addParamValue('mustSetFirstOrder', {}, @(x) iscell(x) && length(intersect(x, model.rxns)) == length(x))
solvers = checkGAMSSolvers('MIP');
if isempty(solvers)
    error('there is no GAMS solvers available to solve Mixed Integer Programming problems') ;
else
    if ismember('cplex', lower(solvers))
        defaultSolverName = 'cplex';
    else
        defaultSolverName = lower(solvers(1));
    end
end

parser.addParamValue('solverName', defaultSolverName, @(x) ischar(x))
hour = clock; defaultRunID = ['run-' date '-' num2str(hour(4)) 'h' '-' num2str(hour(5)) 'm'];
parser.addParamValue('runID', defaultRunID, @(x) ischar(x))
parser.addParamValue('outputFolder', 'OutputsFindMustLL', @(x) ischar(x))
parser.addParamValue('outputFileName', 'MustLLSet', @(x) ischar(x))
parser.addParamValue('printExcel', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('printText', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('printReport', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('keepInputs', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('keepGamsOutputs', 1, @(x) isnumeric(x) || islogical(x));
parser.addParamValue('verbose', 1, @(x) isnumeric(x) || islogical(x));

parser.parse(model, minFluxesW, maxFluxesW, varargin{:})
model = parser.Results.model;
minFluxesW = parser.Results.minFluxesW;
maxFluxesW = parser.Results.maxFluxesW;
constrOpt= parser.Results.constrOpt;
excludedRxns= parser.Results.excludedRxns;
mustSetFirstOrder = parser.Results.mustSetFirstOrder;
solverName = parser.Results.solverName;
runID = parser.Results.runID;
outputFolder = parser.Results.outputFolder;
outputFileName = parser.Results.outputFileName;
printExcel = parser.Results.printExcel;
printText = parser.Results.printText;
printReport = parser.Results.printReport;
keepInputs = parser.Results.keepInputs;
keepGamsOutputs = parser.Results.keepGamsOutputs;
verbose = parser.Results.verbose;

% correct size of constrOpt
if ~isempty(constrOpt.rxnList)
    if size(constrOpt.rxnList, 1) > size(constrOpt.rxnList,2); constrOpt.rxnList = constrOpt.rxnList'; end;
    if size(constrOpt.values, 1) > size(constrOpt.values,2); constrOpt.values = constrOpt.values'; end;
end

% first, verify that GAMS is installed in your system
gamsPath = which('gams');
if isempty(gamsPath); error('OptForce: GAMS is not installed in your system. Please install GAMS.'); end;

%name of the function to solve the optimization problem in GAMS
gamsMustLLFunction = 'findMustLL.gms';
%path of that function
pathGamsFunction = which(gamsMustLLFunction);
if isempty(pathGamsFunction); error(['OptForce: ' gamsMustLLFunction ' not in MATLAB path.']); end;
%current path
workingPath = pwd;
%go to the path associate to the ID for this run.
if ~isdir(runID); mkdir(runID); end; cd(runID);

% if the user wants to generate a report.
if printReport
    %create name for file.
    hour = clock;
    reportFileName = ['report-' date '-' num2str(hour(4)) 'h' '-' num2str(hour(5)) 'm.txt'];
    freport = fopen(reportFileName, 'w');
    reportClosed = 0;
    % print date of running.
    fprintf(freport, ['findMustLLWithGAMS.m executed on ' date ' at ' num2str(hour(4)) ':' num2str(hour(5)) '\n\n']);
    % print matlab version.
    fprintf(freport, ['MATLAB: Release R' version('-release') '\n']);
    % print gams version.
    fprintf(freport, ['GAMS: ' regexprep(gamsPath, '\\', '\\\') '\n']);
    % print solver used in GAMS to solve optForce.
    fprintf(freport, ['GAMS solver: ' solverName '\n']);

    %print each of the inputs used in this running.
    fprintf(freport, '\nThe following inputs were used to run OptForce: \n');
    fprintf(freport, '\n------INPUTS------\n');
    %print model.
    fprintf(freport, '\nModel:\n');
    for i = 1:length(model.rxns)
        rxn = printRxnFormula(model, model.rxns{i}, false);
        fprintf(freport, [model.rxns{i} ': ' rxn{1} '\n']);
    end
    %print lower and upper bounds, minimum and maximum values for each of
    %the reactions in wild-type and mutant strain
    fprintf(freport, '\nLB\tUB\tMin_WT\tMax_WT\n');
    for i = 1:length(model.rxns)
        fprintf(freport, '%6.4f\t%6.4f\t%6.4f\t%6.4f\n', model.lb(i), model.ub(i), minFluxesW(i), maxFluxesW(i));
    end

    %print constraints
    fprintf(freport,'\nConstrained reactions:\n');
    for i = 1:length(constrOpt.rxnList)
        fprintf(freport,'%s: fixed in %6.4f\n', constrOpt.rxnList{i}, constrOpt.values(i));
    end

    fprintf(freport, '\nExcluded Reactions:\n');
    for i = 1:length(excludedRxns)
        rxn = printRxnFormula(model, excludedRxns{i}, false);
        fprintf(freport, [excludedRxns{i} ': ' rxn{1} '\n']);
    end

    fprintf(freport, '\nReactions from first order sets(MustU and MustL):\n');
    for i = 1:length(mustSetFirstOrder)
        rxn = printRxnFormula(model, mustSetFirstOrder{i}, false);
        fprintf(freport, [mustSetFirstOrder{i} ': ' rxn{1} '\n']);
    end

    fprintf(freport,'\nrunID(Main Folder): %s \n\noutputFolder: %s \n\noutputFileName: %s \n',...
        runID, outputFolder, outputFileName);

    fprintf(freport,'\nprintExcel: %1.0f \n\nprintText: %1.0f \n\nprintReport: %1.0f \n\nkeepInputs: %1.0f  \n\nkeepGamsOutputs: %1.0f \n\nverbose: %1.0f \n',...
        printExcel, printText, printReport, keepInputs, keepGamsOutputs, verbose);

end

copyfile(pathGamsFunction);

% export inputs for running the optimization problem in GAMS to find the
% MustLL Set
inputFolder = 'InputsMustLL';
exportInputsMustOrder2ToGAMS(model, 'LL', minFluxesW, maxFluxesW, constrOpt, excludedRxns, mustSetFirstOrder, inputFolder)

% create a directory to save results if this don't exist
if ~exist(outputFolder, 'dir')
    mkdir(outputFolder);
end

%run
if verbose
    run = system(['gams ' gamsMustLLFunction ' lo=3 --myroot=' inputFolder '/ --solverName=' solverName ' gdx=GtoMLL']);
else
    run=system(['gams ' gamsMustLLFunction ' --myroot=' inputFolder '/ --solverName=' solverName ' gdx=GtoMLL']);
end

if printReport; fprintf(freport, '\n------RESULTS------\n'); end;

%if user decide not to show inputs files for findMustLL.gms
if ~keepInputs; rmdir(inputFolder, 's'); end;

%if findMustLL.gms was executed correctly "run" should be 0
if run == 0

    if printReport; fprintf(freport, '\nGAMS was executed correctly\n'); end;
    if verbose; fprintf('GAMS was executed correctly\nSummary of information exported by GAMS:\n'); end;
    %show GAMS report in MATLAB console
    if verbose; gdxWhos GtoMLL; end;
    try
        findMustLL.name = 'findMustLL';
        rgdx('GtoMLL', findMustLL); %if do not exist the variable findMustLL in GtoMLL, an error will ocurr.
        if printReport; fprintf(freport, '\nGAMS variables were read by MATLAB correctly\n'); end;
        if verbose; fprintf('GAMS variables were read by MATLAB correctly\n'); end;

        %Using GDXMRW to read solutions found by findMustLL.gms
        %extract matrix 1 found by findMustII.gms. This matrix contains the
        %first reaction in each couple of reactions
        m1.name = 'matrix1';
        m1.compress = 'true';
        m1 = rgdx('GtoMLL', m1);
        uels_m1 = m1.uels{2};

        if ~isempty(uels_m1)
            %if the uel array for m1 is not empty, at least 1 couple of reations was found.
            if printReport; fprintf(freport, '\na MustLL set was found\n'); end;
            if verbose; fprintf('a MustLL set was found\n'); end;

            %find values for matrix 1
            val_m1 = m1.val;
            m1_full = full(sparse(val_m1(:,1), val_m1(:,2:end-1), val_m1(:,3)));

            %find values for matrix 2
            m2.name = 'matrix2';
            m2.compress = 'true';
            m2 = rgdx('GtoMLL', m2);
            uels_m2 = m2.uels{2};
            val_m2 = m2.val;
            m2_full = full(sparse(val_m2(:,1), val_m2(:,2:end-1), val_m2(:,3)));

            %initialize empty array for storing
            n_mustSet = size(m1_full,1);
            mustLL = cell(n_mustSet, 2);
            pos_mustLL = zeros(size(mustLL));
            mustLL_linear = {};

            %write each couple of reactions.
            for i = 1:n_mustSet
                rxn1 = uels_m1(m1_full(i,:) == 1);
                rxn2 = uels_m2(m2_full(i,:) == 1);
                mustLL(i,1) = rxn1;
                mustLL(i,2) = rxn2;
                pos_mustLL(i,1) = find(strcmp(model.rxns, rxn1));
                pos_mustLL(i,2) = find(strcmp(model.rxns, rxn2));
                mustLL_linear = union(mustLL_linear, [rxn1;rxn2]);
            end
            pos_mustLL_linear = cell2mat(arrayfun(@(x)find(strcmp(x, model.rxns)), mustLL_linear, 'UniformOutput', false))';
        else
            %if the uel array for m1 is empty, no couple of reations was found.
            if printReport; fprintf(freport, '\na MustLL set was not found\n'); end;
            if verbose; fprintf('a MustLL set was not found\n'); end;

            %initialize arrays to be returned by this function
            mustLL = {};
            pos_mustLL = [];
            mustLL_linear = {};
            pos_mustLL_linear = [];
        end

        % print info into an excel file if required by the user
        if printExcel
            if  ~isempty(uels_m1)
                currentFolder = pwd;
                cd(outputFolder);
                must = cell(size(mustLL,1), 1);
                for i = 1:size(mustLL, 1)
                    must{i} = strjoin(mustLL(i,:), ' or ');
                end
                xlswrite([outputFileName '_Info'],[{'Reactions'};must]);
                xlswrite(outputFileName, mustLL_linear);
                cd(currentFolder);
                if verbose
                    fprintf(['MustLL set was printed in ' outputFileName '.xls  \n']);
                    fprintf(['MustLL set was also printed in ' outputFileName '_Info.xls  \n']);
                end
                if printReport
                    fprintf(freport, ['\nMustLL set was printed in ' outputFileName '.xls  \n']);
                    fprintf(freport, ['\nMustLL set was printed in ' outputFileName '_Info.xls  \n']);
                end
            else
                if verbose; fprintf('No mustLL set was found. Therefore, no excel file was generated\n'); end;
                if printReport; fprintf(freport, '\nNo mustLL set was found. Therefore, no excel file was generated\n'); end;
            end
        end

        % print info into a plain text file if required by the user
        if printText
            if ~isempty(uels_m1)
                currentFolder = pwd;
                cd(outputFolder);
                f = fopen([outputFileName '_Info.txt'], 'w');
                fprintf(f,'Reactions\n');
                for i = 1:size(mustLL,1)
                    fprintf(f, '%s or %s\n', mustLL{i,1}, mustLL{i,2});
                end
                fclose(f);

                f = fopen([outputFileName '.txt'], 'w');
                for i = 1:length(mustLL_linear)
                    fprintf(f, '%s\n', mustLL_linear{i});
                end
                fclose(f);

                cd(currentFolder);
                if verbose
                    fprintf(['MustLL set was printed in ' outputFileName '.txt  \n']);
                    fprintf(['MustLL set was also printed in ' outputFileName '_Info.txt  \n']);
                end
                if printReport
                    fprintf(freport, ['\nMustLL set was printed in ' outputFileName '.txt  \n']);
                    fprintf(freport, ['\nMustLL set was printed in ' outputFileName '_Info.txt  \n']);
                end

            else
                if verbose; fprintf('No mustLL set was found. Therefore, no plain text file was generated\n'); end;
                if printReport; fprintf(freport, '\nNo mustLL set was found. Therefore, no plain text file was generated\n'); end;
            end
        end

        %close file for saving report
        if printReport; fclose(freport); reportClosed = 1; end;
        if printReport; movefile(reportFileName, outputFolder); end;
        delete(gamsMustLLFunction);

        %remove or move additional files that were generated during running
        if keepGamsOutputs
            if ~isdir(outputFolder); mkdir(outputFolder); end;
            movefile('GtoMLL.gdx', outputFolder);
            movefile(regexprep(gamsMustLLFunction, 'gms', 'lst'), outputFolder);
        else
            delete('GtoMLL.gdx');
            delete(regexprep(gamsMustLLFunction, 'gms', 'lst'));
        end

        %go back to the original path
        cd(workingPath);
    catch
        %GAMS variables were not read correctly by MATLAB
        if verbose; fprintf('GAMS variables were not read by MATLAB corretly\n'); end;
        if printReport && ~reportClosed; fprintf(freport, '\nGAMS variables were not read by MATLAB corretly\n'); fclose(freport); end;
        cd(workingPath);
        error('OptForce: GAMS variables were not read by MATLAB corretly');

    end

    %if findMustLL.gms was not executed correctly "run" should be different from 0
else
    %if GAMS was not executed correcttly
    if printReport && ~reportClosed; fprintf(freport, '\nGAMS was not executed correctly\n'); fclose(freport); end;
    if verbose; fprintf('GAMS was not executed correctly\n'); end;
    cd(workingPath);
    error('OptForce: GAMS was not executed correctly');

end

end


function y = heavisideNoiseOut(noise, mu, varsigma)

% HEAVISIDENOISEOUT Output from heaviside noise model.

% IVM
D = size(mu, 2);
for i = 1:D
  mu(:, i) = mu(:, i) + noise.bias(i);
end
y = sign(mu);


classdef synthetic

methods(Static)

function signal = picket_fence(N)
    import spx.discrete.number;
    if ~number.is_perfect_square(N)
        error('N must be perfect square');
    end
    n = sqrt(N);
    signal = zeros(1, N);
    signal(1:n:end) = 1;
end

function X = uniform(N, S)
    % Generates uniformly distributed signals

    % Generate Gaussian vectors
    X = randn(N, S);
    % Make them unit length
    X = spx.norm.normalize_l2(X);
end

end

end


function [posterior] = tapas_sem_hier_estimate(data, model, inference, pars)
%% Estimates the hgf using mcmc.
%
% Input
%   data        -- Data of the model. It should be a structure array with
%                  fields y, and u of dimensions n times 1, when n is the
%                  number of subjects.
%   model       -- Standar hgf structure object.
%   inference   -- Inference object. (Optional)
%   pars        -- Parameter structure. (Optional)
%
% Output
%  posterior    -- Structure containing the posterior.
%

% aponteeduardo@gmail.com
% copyright (C) 2016
%

n = 2;

n = n + 1;
if nargin < n
    inference = struct();
end

n = n + 1;
if nargin < n
    pars = struct();
end

[pars] = tapas_sem_multiv_pars(data, model, pars);
[data] = tapas_sem_multiv_data(data, model, pars);
[model] = tapas_sem_hier_model(data, model, pars);
[inference] = tapas_sem_hier_inference(inference, pars);

[posterior] = tapas_sem_hier_estimate_interface(data, model, inference);

end


% run_simMegaPressShapedEdit.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% This script is run simply by editing the input parameters and then
% clicking "Run".
%
% DESCRIPTION:
% This script simulates a MEGA-PRESS experiment with fully shaped editing
% pulses.  Phase cycling of editing pulses is performed, and summation
% across phase cycles is performed.
%
% INPUTS:
% To run this script, edit the parameters below as desired and then click
% "run":
% editWaveform          = name of editing pulse waveform.
% editOnFreq            = freqeucny of edit on pulse[ppm]
% editOffFreq           = frequency of edit off pulse[ppm]
% editTp                = duration of editing pulses[ms]
% Npts                  = number of spectral points
% sw                    = spectral width [Hz]
% Bfield                = magnetic field strength [Tesla]
% lw                    = linewidth of the output spectrum [Hz]
% taus                  = vector of pulse sequence timings  [ms]
% spinSys               = spin system to simulate
% editPhCyc1            = vector of phase cycling steps for 1st editing pulse [degrees]
% editPhCyc2            = vector of phase cycling steps for 2nd editing pulse [degrees]
%
% OUTPUTS:
% outON             = Simulated MEGA-PRESS edit-ON spectrum.
% outOFF            = Simulated ExTE-MEGA-PRESS edit-OFF spectrum.
% outDIFF           = Simulated ExTE-MEGA-PRESS difference spectrum.

% ************INPUT PARAMETERS**********************************
editWaveform='sampleEditPulse.pta'; %name of editing pulse waveform.
editOnFreq=1.88; %freqeucny of edit on pulse[ppm]
editOffFreq=7.4; %frequency of edit off pulse[ppm]
editTp=20; %duration of editing pulses[ms]
Npts=2048; %number of spectral points
sw=2000; %spectral width [Hz]
Bfield=3; %magnetic field strength [Tesla]
lw=2; %linewidth of the output spectrum [Hz]
taus=[5,... %Time from excitation to 1st refoc pulse [ms]
    17,...  %Time from 1st refoc pulse to 1st editing pulse [ms]
    17,...  %Time from 1st editing pulse to 2nd refoc pulse [ms]
    17,...  %Time from 2nd refoc pusle to 2nd editing pulse [ms]
    12];    %Time from 2nd editing pulse to ADC onset [ms]
spinSys='GABA'; %spin system to simulate
centreFreq=3.0; %Centre Frequency of MR spectrum [ppm];
editPhCyc1=[0 90]; %phase cycling steps for 1st editing pulse [degrees]
editPhCyc2=[0 90 180 270]; %phase cycling steps for 2nd editing pulse [degrees]
% ************END OF INPUT PARAMETERS**********************************

%Load RF waveforms
editRF=io_loadRFwaveform(editWaveform,'inv',0);

gamma=42577000; %gyromagnetic ratio

%Load spin systems
load spinSystems
sys=eval(['sys' spinSys]);

%This is the step where the editing pulse waveform (initially a pulse with
%zero-frequency) is frequency shifted to produce and edit-on and an
%edit-off pulse;
editRFon=rf_freqshift(editRF,editTp,(centreFreq-editOnFreq)*Bfield*gamma/1e6);
editRFoff=rf_freqshift(editRF,editTp,(centreFreq-editOffFreq)*Bfield*gamma/1e6);

%Initialize structures:
outON_epc=cell(length(editPhCyc1),length(editPhCyc2));
outOFF_epc=cell(length(editPhCyc1),length(editPhCyc2));
outON=struct([]);
outOFF=struct([]);

%loop through space: Don't forget to initialize the parallel processing
%toolbox workers using 'matlabpool open N' (for N workers, 12 max).

%for X=1:length(x);

for EP1=1:length(editPhCyc1)
    for EP2=1:length(editPhCyc2)
                disp(['Executing First Edit phase cycle ' num2str(EP1) ' of ' num2str(length(editPhCyc1)) ', '...
                    ' and Second Edit phase cycle ' num2str(EP2) ' of ' num2str(length(editPhCyc2)) '!!!']);
                outON_epc{EP1}{EP2}=sim_megapress_shapedEdit(Npts,sw,Bfield,lw,taus,sys,...
                    editRFon,editTp,editPhCyc1(EP1),editPhCyc2(EP2));
                outOFF_epc{EP1}{EP2}=sim_megapress_shapedEdit(Npts,sw,Bfield,lw,taus,sys,...
                    editRFoff,editTp,editPhCyc1(EP1),editPhCyc2(EP2));

        if EP1==1 && EP2==1
            outON=outON_epc{EP1}{EP2};
            outOFF=outOFF_epc{EP1}{EP2};
        else
            outON=op_addScans(outON,outON_epc{EP1}{EP2});
            outOFF=op_addScans(outOFF,outOFF_epc{EP1}{EP2});
        end

    end %end of 1st editing phase cycle loop.
end %end of 2nd editing phase cycle loop.

figure
plot(outON.ppm,outON.specs,outOFF.ppm,outOFF.specs);


% Returns a sampling grid for an image (using overlapping window)
%   GRID = SAMPLING_GRID(IMG_SIZE, WINDOW, OVERLAP, BORDER, SCALE)
function grid = sampling_grid(img_size, window, overlap, border, scale)

if nargin < 5
    scale = 1;
end

if nargin < 4
    border = [0 0];
end

if nargin < 3
    overlap = [0 0];
end

% Scale all grid parameters
window = window * scale;
overlap = overlap * scale;
border = border * scale;

% Create sampling grid for overlapping window
index = reshape(1:prod(img_size), img_size);
grid = index(1:window(1), 1:window(2)) - 1;

% Compute offsets for grid's displacement.
skip = window - overlap; % for small overlaps
offset = index(1+border(1):skip(1):img_size(1)-window(1)+1-border(1), ...
               1+border(2):skip(2):img_size(2)-window(2)+1-border(2));
offset = reshape(offset, [1 1 numel(offset)]);

% Prepare 3D grid - should be used as: sampled_img = img(grid);
grid = repmat(grid, [1 1 numel(offset)]) + repmat(offset, [window 1]);


function [nlcon,nlrhs,nle] = detNlcon(nonlcon,x0)
%DETNLCON  Determine OPTI format Nonlinear Constraints

%   Copyright (C) 2011 Jonathan Currie (IPL)

%Sort out nonlinear constraints (unfortunately rather inefficient)
if(isempty(nonlcon))
    nlcon = []; nlrhs = []; nle = [];
    return;
end

[in,eq] = nonlcon(x0);
nineq = length(in);
neq = length(eq);

nlcon = @(x) conval(nonlcon,x);
nlrhs = zeros(nineq+neq,1);
nle = [-1*ones(nineq,1);zeros(neq,1)];

%Dummy function
function c = conval(nonlcon,x)
[in,eq] = nonlcon(x);
c = [in;eq];


% ----------------- Add the path of DeepMIMO function --------------------%
addpath('DeepMIMO_functions')

% -------------------- DeepMIMO Dataset Generation -----------------------%
% Load Dataset Parameters
dataset_params = read_params('parameters.m');
[DeepMIMO_dataset, dataset_params] = DeepMIMO_generator(dataset_params);

% -------------------------- Output Examples -----------------------------%
% DeepMIMO_dataset{i}.user{j}.channel % Channel between BS i - User j
% %  (# of User antennas) x (# of BS antennas) x (# of OFDM subcarriers)
%
% DeepMIMO_dataset{i}.user{j}.params % Parameters of the channel (paths)
% DeepMIMO_dataset{i}.user{j}.LoS_status % Indicator of LoS path existence
% %     | 1: LoS exists | 0: NLoS only | -1: No paths (Blockage)|
%
% DeepMIMO_dataset{i}.user{j}.loc % Location of User j
% DeepMIMO_dataset{i}.loc % Location of BS i
%
% % BS-BS channels are generated only if (params.enable_BSchannels == 1):
% DeepMIMO_dataset{i}.basestation{j}.channel % Channel between BS i - BS j
% DeepMIMO_dataset{i}.basestation{j}.loc
% DeepMIMO_dataset{i}.basestation{j}.LoS_status
%
% % Recall that the size of the channel vector was given by
% % (# of User antennas) x (# of BS antennas) x (# of OFDM subcarriers)
% % Each of the first two channel matrix dimensions follows a certain
% % reshaping sequence that can be obtained by the following
% % 'antennamap' vector: Each entry is 3 integers in the form of
% % 'xyz' where each representing the antenna number in x, y, z directions
% antennamap = antenna_channel_map(params.num_ant_BS(1), ...
%                                  params.num_ant_BS(2), ...
%                                  params.num_ant_BS(3), 1);
%
% -------------------------- Dynamic Scenario ----------------------------%
%
% DeepMIMO_dataset{f}{i}.user{j}.channel % Scene f - BS i - User j
% % Every other command applies as before with the addition of scene ID
% params{f} % Parameters of Scene f
%
% ------------------------------------------------------------------------%


function predicted = classify_costs_classtree_multiclass_learner( CLASSIFIER, x )
% function predicted = classify_costs_classtree_multiclass_learner( CLASSIFIER, x )
% Classify a set of projected using pixel-wise grey level difference.
%
% Input:
%   CLASSIFIER, a structure returned by train_classtree_multiclass_learner.m
%   x, Pxn matrix (P is the number of pixels per image n is the number
%      of images).
%
% Output:
%     predicted -- 1 x n matrix with the lower cost  weak
%                   classifier output for the input data in x.
%
% See also:
%   train_COST_SAMME.m

% Author: Jose M. Buenaposada
n           = size(x,2);

% Evaluate the classification tree on the training data.
z = CLASSIFIER.TREE.eval( x' );
predicted = cellfun(@(x) str2double(x), z);


function [vtx,elt,col] = mshReadMesh(filename)
%+========================================================================+
%|                                                                        |
%|                 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       : mshReadMesh.m                                 |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Read .mesh files (vertex, triangular elements,|
%|  `---'  |                and colours)                                  |
%+========================================================================+

% Open file
fid = fopen(filename,'r');
if( fid==-1 )
    error('Can''t open the file.');
end

% Nodes
str = fgets(fid);
while isempty(strfind(str,'Vertices'))
    str = fgets(fid);
end
Nvtx = str2double(fgets(fid));
vtx  = zeros(Nvtx,3);
for i = 1:Nvtx
    tmp = str2num(fgets(fid));
    vtx(i,:) = tmp(1:3);
end

% Support only triangular elements
str = fgets(fid);
if strcmp(str(1:9),'Triangles')
    Nelt = str2double(fgets(fid));
    elt  = zeros(Nelt,4);
    for i = 1:Nelt
        elt(i,:) = str2num(fgets(fid));
    end
    col = elt(:,4);
    elt = elt(:,1:3);

elseif strcmp(str(1:10),'Tetrahedra')
    Nelt = str2double(fgets(fid));
    elt  = zeros(Nelt,5);
    for i = 1:Nelt
        elt(i,:) = str2num(fgets(fid));
    end
    col = elt(:,5);
    elt = elt(:,1:4);

else
    error('mshReadMesh : unavailable case');
end

% Close file
fclose(fid);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Max manipulability index ALONG A LINE.
% Use stomp like to optimize along a surface/line
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function experiment2B_K50_N30
close all;
global parameters

%STOMP PARAMETERS
%number of particles
K = 50;

%repeat experiment number of times
parameters.n_repeat = 30;
parameters.experiment_name = 'experiment2B_K50_N30.mat';
parameters.animate = 0;

%repeat the experiment E times
random_manips=[];
Gout = [];
for i=1:parameters.n_repeat
    close all
    [pk, final_manip] = path_planning_SCO_4DOF(K);
    Gout{i}=pk;
    random_manips = [random_manips; final_manip];
    save(parameters.experiment_name)
end

'ended'
parameters.experiment_name


function [ y2, m2, d2, f2 ] = ymdf_inc_common ( y1, m1, d1, f1, days )

%*****************************************************************************80
%
%% YMDF_INC_COMMON increments a Common YMDF date by DAYS days.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y1, M1, D1, real F1, the
%    YMDF date.
%
%    Input, real DAYS, the number of days to advance the date.
%
%    Output, integer Y2, M2, D2, real F2, the
%    incremented YMDF date.
%

%
%  Copy the parameters.
%
  y2 = y1;
  m2 = m1;
  d2 = d1;
  f2 = f1 + days;
%
%  Check the parameters.
%
  [ y2, m2, d2, f2, ierror ] = ymdf_check_common ( y2, m2, d2, f2 );

  return
end


function M = dimindex(A,dim,idx)

% DIMINDEX makes a selection from a multi-dimensional array where the dimension is
% selected by a scalar, not by the place between the brackets.
%
% Use as
%   M = dimindex(A,dim,idx)
%
% The purpose of the function is shown by the following example:
%
% A(:,:,:,23,:,:,...) is the same as dimindex(A,4,23)
% A(2,4,3)            is the same as dimindex(A,[1,2,3],[2,4,3])
% A(4,:,[5:10])       is the same as dimindex(A,[1,3],{4,[5:10]})
%
% See also the function DIMASSIGN

% Copyright (C) 2005, Geerten Kramer
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if (~iscell(idx))
  if (~any(size(dim)==1) || ~any(size(idx)==1) || ndims(dim)>2 || ndims(idx)>2 || length(dim)~=length(idx))
    ft_error('dim and idx must be both scalars or both vectors of the same size');
  end
  dummi = [];
  for i=1:length(idx)
    dummi{i} = idx(i);
  end
  idx = dummi;
  clear dummi;
end
if (~any(size(dim)==1) || ~any(size(idx)==1) || ndims(dim)>2 || ndims(idx)>2 || length(dim)~=length(idx))
  ft_error('dim and idx must be both scalars or both must have the same length');
end

if (length(dim)>ndims(A)||any(dim>ndims(A)))
  ft_error('dim, or one of its contents are larger than the number of dimentions in A');
end
if (~isequal(unique(dim),sort(dim)))
  ft_error('dim must be unique, every dimention can be addressed only once');
end

N = ndims(A);
for i=1:N
  ref = find(dim==i);
  if (isempty(ref))
    C{i} = ':';
  else
    C{i} = idx{ref};
  end
end
M = A(C{:});


% SP_REFINE: construct a refined space from a given one. The function only
%                refines the space, the mesh must be refined separately.
%
%     [sp_fine, Proj] = sp_refine (space, msh, nsub, [degree], [regularity]);
%
% The same number of subdivisions, degree and regularity is applied to every patch
%
% INPUTS:
%
%     space:      the coarse space, an object of the sp_multipatch class (see sp_multipatch)
%     msh:        an object of the msh_multipatch class, usuallly the refined mesh (see msh_multipatch)
%     nsub:       number of uniform subdivisions to apply on each knot span, and for each direction
%     degree:     degree of the fine space, and for each direction
%     regularity: regularity for the new space, and for each direction
%
% OUTPUT:
%
%     sp_fine: the refined space, an object of the class sp_multipatch (see sp_multipatch)
%     Proj:    the coefficients relating 1D splines of the coarse and the fine spaces for each patch.
%                A cell-array of size 1 x npatch, each entry containing the
%                coefficients for the patch (either for scalar or vector-valued spaces).
%
% Copyright (C) 2015, 2016, 2018 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_fine, Proj] = sp_refine (space, msh, nsub, varargin)

  if (space.ndof == sum (space.ndof_per_patch))
    gluing = false;
  else
    gluing = true;
  end

  sp_fine = cell (1, space.npatch);
  Proj = cell (1, space.npatch);
  for iptc = 1:space.npatch
    if (nargout == 2)
      [sp_fine{iptc}, Proj{iptc}] = sp_refine (space.sp_patch{iptc}, msh.msh_patch{iptc}, nsub, varargin{:});
    else
      sp_fine{iptc} = sp_refine (space.sp_patch{iptc}, msh.msh_patch{iptc}, nsub, varargin{:});
    end
  end
  if (~isempty (space.boundary))
    sp_fine = sp_multipatch (sp_fine, msh, space.interfaces, space.boundary.interfaces, gluing);
  else
    sp_fine = sp_multipatch (sp_fine, msh, space.interfaces, [], gluing);
  end

end


function identify_redcells_sourcery(db, ops0)

ops = build_ops3(db, ops0);

redcells = [];

for i = 1:length(ops.planesToProcess)

    iplane  = ops.planesToProcess(i);
    %%
    try
        fname = sprintf('%s/F_%s_%s_plane%d.mat', ops.ResultsSavePath, ops.mouse_name, ops.date, iplane);
        dat = load(fname);
        while isfield(dat, 'dat')
            dat = dat.dat;
        end
    catch
        fname = sprintf('%s/F_%s_%s_plane%d_proc.mat', ops.ResultsSavePath, ops.mouse_name, ops.date, iplane, ops.Nk);
        dat = load(fname);
        while isfield(dat, 'dat')
            dat = dat.dat;
        end
    end

    % subtract bleedthrough of green into red channel
    % if there's a specific green channel (from a red+green short session)
    % using non-rigid linear regression
    if isfield(dat.ops,'mimgGREEN')
        mimgG = dat.ops.mimgGREEN;
    else
        mimgG = dat.ops.mimg1;
    end
    mimgR = dat.ops.mimgRED;
    mimgR = mimgR(dat.ops.yrange, dat.ops.xrange);
    mimgG = mimgG(dat.ops.yrange, dat.ops.xrange);
    [Ny, Nx] = size(mimgR);

    % regression
    nblks               = 3;
    yB                  = round(linspace(1,Ny,nblks+1));
    xB                  = round(linspace(1,Nx,nblks+1));
    [yBL,xBL] = MakeQuadrants(yB,xB);
    msk                 = zeros(Ny,Nx,length(yB));
    for j = 1:length(yBL)
        xg              = mimgG(yBL{j},xBL{j});
        xr              = mimgR(yBL{j},xBL{j});
        A               = polyfit(xg,xr,1);
        msk(:,:,j)      = QuadrantMask(yBL,xBL,Ny,Nx,j);
        gw(j)           = A(1);
    end
    msk                 = bsxfun(@times,msk,1./sum(msk,3));
    pixweight           = zeros(Ny,Nx);
    for j = 1:length(yBL)
        pixweight = pixweight + msk(:,:,j)*gw(j);
    end
    mimgR0              = mimgR - pixweight.*mimgG;

    % save to dat for GUI
    dat.ops.mimgREDcorrected = zeros(dat.ops.Ly, dat.ops.Lx);
    dat.ops.mimgREDcorrected(dat.ops.yrange, dat.ops.xrange) = mimgR0;
    %

    %%
    %%%% compute red pixels in cell and in area around cell
    % (exclude pixels from other cells (cellPix))
    % (use surround neuropil masks to compute red around cell)
    ops.ratioNeuropil     = 4;
    ops.minNeuropilPixels = 80;
    [dat.stat, cellPix, ~]       = createCellMasks(dat.stat, Ny, Nx);
    [~, neuropMasks] = createNeuropilMasks(ops, dat.stat, cellPix);

    %
    redSum = zeros(length(dat.stat),2);
    for k = 1:numel(dat.stat)
        ipix                 = dat.stat(k).ipix;
        rpix                 = mimgR0(ipix) .* dat.stat(k).lam/sum(dat.stat(k).lam);
        % rescale by lam? .*(dat.stat(j).lambda'/sum(dat.stat(j).lambda));
        extpix               = squeeze(neuropMasks(k,:,:));
        extpix               = find(extpix~=0);
        ext_rpix             = mimgR0(extpix);
        redSum(k,:)          = [sum(rpix) mean(ext_rpix)];
    end
    redSum = redSum - min(redSum(:));

    % set threshold for redpix
    ops.redthres = getOr(ops, 'redthres', 1.5);
    ops.redmax   = getOr(ops, 'redmax', 1.25);

    rrat = redSum(:,1)./(redSum(:,2)+redSum(:,1));
    redcell  = rrat > nanmean(rrat) + ops.redthres*nanstd(rrat);
    notred   = rrat <= nanmean(rrat) + ops.redmax*nanstd(rrat);

    fprintf('plane %d  reds %d\n',iplane,sum(redcell(:)));

    %%
    for j = 1:length(dat.stat)
        dat.stat(j).redcell = redcell(j);
        dat.stat(j).redprob = rrat(j);
        dat.stat(j).notred  = notred(j);
    end

    dat.ops.redthres = ops.redthres;
    dat.ops.redmax   = ops.redmax;
    save(fname, '-struct', 'dat')
end

%%
if 0
    [~,ix] = sort(rrat, 'descend');
    for k = ix(:)'
        clf;
        cent = round(dat.stat(k).med);
        yl = cent(1) + [-20:20];
        yl(yl<1 | yl > Ny) = [];
        xl = cent(2) + [-20:20];
        xl(xl<1 | xl > Nx) = [];
        imagesc(mimgR0)
        hold all;
        plot(cent(2),cent(1),'k*');
        axis([xl(1) xl(end) yl(1) yl(end)]);
        title([rrat(k) redcell(k)]);
        drawnow;
        pause;
    end
end
    %


function obj = setDriftVector(obj, vf)
    % Set the drift vector field Fvec(x) or Fvec(x,dx) of the
    % system
    %
    % Parameters:
    %  vf: the f(x) @type cell

    % validate the inputs vf(x,dx)
    if ~iscell(vf), vf = {vf}; end
    for i=1:numel(vf)
        %         assert((length(vf{i})==obj.numState) && isvector(vf{i}),...
        %             'The %d-th drift vector field should be a (%d x 1) column vector.',i,obj.numState);
        if isa(vf{i},'SymFunction')
            vars = vf{i}.Vars;
            if strcmp(obj.Type,'SecondOrder') % second order system
                assert(length(vars)==2 && vars{1} == obj.States.x && vars{2}==obj.States.dx,...
                    'The SymFunction (vf{%d}) must be a function of states (x) and (dx).',i);
            else % first order system
                assert(length(vars)==1 && vars{1} == obj.States.x,...
                    'The SymFunction (vf{%d}) must be a function of states (x).',i);
            end
        else
            s_vf = SymExpression(vf{i});
            fvec_name = ['Fvec' num2str(i) '_' obj.Name];
            if strcmp(obj.Type,'SecondOrder') % second order system
                sfun_vf = SymFunction(fvec_name,s_vf,{obj.States.x,obj.States.dx});
            else                         % first order system
                sfun_vf = SymFunction(fvec_name,s_vf,{obj.States.x});
            end
            vf{i} = sfun_vf;
        end
    end

    obj.Fvec = vf;
    obj.FvecName_ = cellfun(@(f)f.Name, vf,'UniformOutput',false);
end


function plotOutput(output)

FigNum = 1;

sol = output.result.solution;

idx = output.result.setup.auxdata.index;

Time = sol.phase.time;
th = sol.phase.state(:,idx.THETA);
phi = sol.phase.state(:,idx.PHI);
Dth = sol.phase.state(:,idx.DTHETA);
Dphi = sol.phase.state(:,idx.DPHI);

uHip = sol.phase.control(:,idx.HIP);
uAnk = sol.phase.control(:,idx.ANK);

%Print things:
disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~')
disp(['Push-off impulse: ' num2str(sol.parameter) ' Nm']);
disp(['Objective value: ' num2str(output.result.objective)...
    ' (' output.result.setup.auxdata.cost.model ')']);
disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~')
figure(FigNum); clf;

TitleFontSize = 16;
LabelFontSize = 14;
AxisFontSize = 10;

    subplot(3,2,1)
    plot(Time,th,'LineWidth',3)
    title('Stance Angle','FontSize',TitleFontSize)
    xlabel('Time (s)','FontSize',LabelFontSize)
    ylabel('Angle (rad)','FontSize',LabelFontSize)
    set(gca,'fontsize',AxisFontSize);

    subplot(3,2,2)
    plot(Time,phi,'LineWidth',3)
    title('Swing Angle','FontSize',TitleFontSize)
    xlabel('Time (s)','FontSize',LabelFontSize)
    ylabel('Angle (rad)','FontSize',LabelFontSize)
    set(gca,'fontsize',AxisFontSize);

    subplot(3,2,3)
    plot(Time,Dth,'LineWidth',3)
    title('Stance Rate','FontSize',TitleFontSize)
    xlabel('Time (s)','FontSize',LabelFontSize)
    ylabel('Rate (rad/s)','FontSize',LabelFontSize)
    set(gca,'fontsize',AxisFontSize);

    subplot(3,2,4)
    plot(Time,Dphi,'LineWidth',3)
    title('Swing Rate','FontSize',TitleFontSize)
    xlabel('Time (s)','FontSize',LabelFontSize)
    ylabel('Rate (rad/s)','FontSize',LabelFontSize)
    set(gca,'fontsize',AxisFontSize);

    subplot(3,2,5)
    plot(Time,uHip,'LineWidth',3)
    title('Hip Torque','FontSize',TitleFontSize)
    xlabel('Time (s)','FontSize',LabelFontSize)
    ylabel('Torque (Nm)','FontSize',LabelFontSize)
    set(gca,'fontsize',AxisFontSize);

    subplot(3,2,6)
    plot(Time,uAnk,'LineWidth',3)
    title('Ankle Torque','FontSize',TitleFontSize)
    xlabel('Time (s)','FontSize',LabelFontSize)
    ylabel('Torque (Nm)','FontSize',LabelFontSize)
    set(gca,'fontsize',AxisFontSize);

end


function [cost, grad] = getCostGrad(problem, x, storedb, key)
% Computes the cost function and the gradient at x in one call if possible.
%
% function [cost, grad] = getCostGrad(problem, x)
% function [cost, grad] = getCostGrad(problem, x, storedb)
% function [cost, grad] = getCostGrad(problem, x, storedb, key)
%
% Returns the value at x of the cost function described in the problem
% structure, as well as the gradient at x.
%
% storedb is a StoreDB object, key is the StoreDB key to point x.
%
% See also: canGetCost canGetGradient getCost getGradient

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%
%   April 3, 2015 (NB):
%       Works with the new StoreDB class system.

    % Allow omission of the key, and even of storedb.
    if ~exist('key', 'var')
        if ~exist('storedb', 'var')
            storedb = StoreDB();
        end
        key = storedb.getNewKey();
    end

    if isfield(problem, 'costgrad')
    %% Compute the cost/grad pair using costgrad.

        % Check whether this function wants to deal with storedb or not.
        switch nargin(problem.costgrad)
            case 1
                [cost, grad] = problem.costgrad(x);
            case 2
                % Obtain, pass along, and save the store for x.
                store = storedb.getWithShared(key);
                [cost, grad, store] = problem.costgrad(x, store);
                storedb.setWithShared(store, key);
            case 3
                % Pass along the whole storedb (by reference), with key.
                [cost, grad] = problem.costgrad(x, storedb, key);
            otherwise
                up = MException('manopt:getCostGrad:badcostgrad', ...
                    'costgrad should accept 1, 2 or 3 inputs.');
                throw(up);
        end

    else
    %% Revert to calling getCost and getGradient separately

        cost = getCost(problem, x, storedb, key);
        grad = getGradient(problem, x, storedb, key);

    end

end


function varargout = contourfcmap(x,y,z,clev,cmap,lo,hi,cbarloc)
%CONTOURFCMAP Filled contour plot with specified colors
%
% h = contourfcmap(x,y,z,clev,cmap,lo,hi,cbarloc)
% h = contourfcmap(x,y,z,clev,cmap)
% h = contourfcmap(x,y,z,clev,cmap,lo)
% h = contourfcmap(x,y,z,clev,cmap,lo,hi)
%
% This function creates a shaded contour map, similar to that created by
% the contourf function.  However, the relationship between a contourf plot
% and it's colormap (i.e. exactly which color corresponds to each contour
% interval), can often be confusing and inconsistent, in my opinion.  This
% function instead allows the user to specify exactly which colors to use
% in each interval, and also to choose colors for regions that exceed the
% contour line limits.
%
% Input variables
%
%   x:          x-coordinates of grid, following size restrictions of surf
%               command
%
%   y:          y-coordinates of grid, following size restrictions of surf
%               command
%
%   z:          array of data to be contoured, following size restritions
%               of surf command
%
%   clev:       vector of length n, contour levels, must be monotonically
%               increasing
%
%   cmap:       n-1 x 3 colormap array, specifying colors used to shade
%               each contour interval
%
%   lo:         1 x 3 colormap array, specifying color used for all data
%               falling below the first contour level.  If not included or
%               empty, default will be to white.
%
%   hi:         1 x 3 colormap array, specifying color used for all data
%               falling above the last contour level.  If not included or
%               empty, default will be to white.
%
%   cbarloc:    string specifying colorbar location (see colorbar),  or a
%               1 x 4 position vector for colorbar.  If not included, no
%               colorbar will be created.  Note that the colorbar created
%               is not a colorbar object, but just an axis with plotted
%               patches; it is for labeling only and is not linked to the
%               "peer axis" in any way.
%
% Output variables:
%
%   h:          1 x 1 structure
%
%               c:      contour matrix for filled contour plot
%
%               h:      handle to contourgroup
%
%               cbax:   handle to axis of colorbar
%
% Example:
%
% [x,y] = meshgrid(linspace(0,1,100));
% z = peaks(100);
% contourfcmap(x,y,z,[-5 -3 -2:.5:2 3 5],jet(12), ...
%              [.8 .8 .8], [.2 .2 .2], 'eastoutside')
%

% Copyright 2010 Kelly Kearney

%------------------------
% Parse input
%------------------------

error(nargchk(5,8,nargin));

% Check clev

if ~isvector(clev)
    error('clev must be a vector');
end
nlev = length(clev);

% Check cmap

if size(cmap,2) ~=3 || size(cmap,1) ~= (nlev-1) || any(cmap(:)<0|cmap(:)>1)
    error('cmap must be nlev-1 x 3 colormap array');
end

% Colors for too-low and too-high patches

if nargin < 6 || isempty(lo)
    lo = [1 1 1];
end

if nargin < 7 || isempty(hi)
    hi = [1 1 1];
end

if ~isequal(size(lo),[1 3])
    error('lo must be 1 x 3 colormap array');
end

if ~isequal(size(hi),[1 3])
    error('hi must be 1 x 3 colormap array');
end

% Colorbar location

if nargin == 8 && ~isempty(cbarloc)
    pos = {'north', 'south', 'east', 'west', 'northoutside', 'southoutside', 'eastoutside', 'westoutside'};
    if ischar(cbarloc)
        if ~any(strcmp(lower(cbarloc), pos))
            error('Unrecognizd colorbar position');
        end
    elseif ~isequal(size(cbarloc), [1 4]) || any(cbarloc > 1 | cbarloc < 0)
        error('cbarloc must be position string or  1 x 4 normalized position');
    end
    showcb = true;
else
    showcb = false;
end

% Axis

ax = gca;

%------------------------
% Create filled contour
% plot
%------------------------

[c,h] = contourf(x,y,z,clev);

hpatch = get(h, 'children');

cdata = cell2mat(get(hpatch, 'cdata'));

% Mark too-high contours

isabove = cdata == max(clev);

% Distinguish between too-lo contours and NaN contours

if ~any(isnan(z(:)))
    isbelow = isnan(cdata);
else
    warning('NaN in data; haven''t figured NaN vs lo yet');
    isbelow = isnan(cdata);
end

level = get(h, 'LevelList');
if ~isequal(level, clev)
    error('oops, something new with levels, check this');
end

[tf, idx] = ismember(cdata, clev);

isweirdextra = idx == 0 & ~isbelow & ~isabove; % Why do these show up?!

for ip = 1:length(hpatch)
    if isbelow(ip)
            set(hpatch(ip), 'facecolor', lo);
    elseif isabove(ip)
            set(hpatch(ip), 'facecolor', hi);
    elseif isweirdextra(ip)
        dist = cdata(ip) - clev;
        dist(dist<0) = Inf;
        [blah, imin] = min(dist);
        set(hpatch(ip), 'facecolor', cmap(imin,:));
    else
        set(hpatch(ip), 'facecolor', cmap(idx(ip),:));
    end
end

%------------------------
% Create colorbar
%------------------------

if showcb
    clevcbar = reshape(clev,1,[]);
    height = 0.1 * (max(clevcbar) - min(clevcbar));
    y1 = [clev(1)-height; clev(1); clev(1); clev(1)-height; clev(1)-height];
    y2 = [clev(end); clev(end)+height; clev(end)+height; clev(end); clev(end)];

    yp = [y1 [clev(1:end-1); clev(2:end); clev(2:end); clev(1:end-1); clev(1:end-1)] y2];
    xp = [0;0;1;1;0] * ones(1,nlev+1);
    cp = [lo; cmap; hi];
    cp = permute(cp, [3 1 2]);

    if ~ischar(cbarloc)
        cbarcoord = cbarloc;
        hascbcoord = true;
        if cbarcoord(3)-cbarcoord(1) < cbarcoord(4)-cbarcoord(2)
            isvert = true;
            cbarloc = 'east';
        else
            isvert = false;
            cbarloc = 'south';
        end
    else
        hascbcoord = false;
        if any(strcmp(pos([3 4 7 8]), lower(cbarloc)))
            isvert = true;
        else
            isvert = false;
        end
    end

    if ~isvert
        tmp = xp;
        xp = yp;
        yp = tmp;
    end

    cbax = colorbar(cbarloc);
    axpos = get(ax, 'position');
    cbpos = get(cbax, 'position');

    delete(cbax);
    set(ax, 'position', axpos);

    cbax = axes('position', cbpos);
    set(cbax, 'userdata', 'contourfcolorbar');

    patch(xp,yp,cp);
    if isvert
        set(cbax, 'ytick', clev, 'ylim', minmax(yp), 'xlim', [0 1], 'xtick', []);
    else
        set(cbax, 'xtick', clev, 'xlim', minmax(xp), 'ylim', [0 1], 'ytick', []);
    end
end

if showcb && hascbcoord
    set(cbax, 'position', cbarcoord);
end

%------------------------
% Output
%------------------------

hndl.c = c;
hndl.h = h;
if showcb
    hndl.cbax = cbax;
end

if nargout > 0
    varargout{1} = hndl;
end

% Return focus to axis (needed if colorbar created)

axes(ax);

function a = minmax(b)
a = [min(b(:)) max(b(:))];


% F1=figure;
% set(F1,'position',[18 144 560 420]);%Numero de paralelepipedos
lo=2;               %Lado del paralelepipedo base
deltax=5;           %Altura del paralelepipedo
xo=-lo/2;           %Punto de origen en x
yo=-lo/2;           %Punto de origen en y
zo=0;               %Punto de origen en z

line([0 0],[0 0],[0 10],'color','r')
line([0 5],[0 0],[5 5],'color','g')
line([0 0],[0 5],[5 5],'color','b')

x1=xo;
x2=x1+lo;
x3=x1+lo;
x4=x1;
x5=x1;
x6=x1;
x7=x1+lo;
x8=x1+lo;

y1=yo;
y2=y1;
y3=y1+lo;
y4=y1+lo;
y5=y1+lo;
y6=y1;
y7=y1;
y8=y1+lo;

z1=zo;
z2=zo;
z3=zo;
z4=zo;
z5=zo+deltax;
z6=zo+deltax;
z7=zo+deltax;
z8=zo+deltax;

x=[x1 x2 x3 x4 x5 x6 x7 x8];
y=[y1 y2 y3 y4 y5 y6 y7 y8];
z=[z1 z2 z3 z4 z5 z6 z7 z8];

%///////////////////////////////////////////////////
%//////////////////////////////////////////////////
for n=1:8
T(1:4,n)=[cos(teta) -sin(teta) 0 0
   sin(teta) cos(teta) 0 0
   0 0 1 0
   0 0 0 1]*[x(n);y(n);z(n);1];
end

a=0.4;
tcolor(1,1,1:3) = [0.8 0.4 0.3];
A1=patch([T(1,1) T(1,2) T(1,3) T(1,4)],[T(2,1) T(2,2) T(2,3) T(2,4)],[T(3,1) T(3,2) T(3,3) T(3,4)],tcolor);
A2=patch([T(1,1) T(1,2) T(1,7) T(1,6)],[T(2,1) T(2,2) T(2,7) T(2,6)],[T(3,1) T(3,2) T(3,7) T(3,6)],tcolor);
A3=patch([T(1,5) T(1,6) T(1,7) T(1,8)],[T(2,5) T(2,6) T(2,7) T(2,8)],[T(3,5) T(3,6) T(3,7) T(3,8)],tcolor);
A4=patch([T(1,2) T(1,3) T(1,8) T(1,7)],[T(2,2) T(2,3) T(2,8) T(2,7)],[T(3,2) T(3,3) T(3,8) T(3,7)],tcolor);
A5=patch([T(1,1) T(1,4) T(1,5) T(1,6)],[T(2,1) T(2,4) T(2,5) T(2,6)],[T(3,1) T(3,4) T(3,5) T(3,6)],tcolor);
A6=patch([T(1,3) T(1,4) T(1,5) T(1,8)],[T(2,3) T(2,4) T(2,5) T(2,8)],[T(3,3) T(3,4) T(3,5) T(3,8)],tcolor);
%
% light('Position',[1 0 0],'Style','infinite');
% light('Position',[0 1 0],'Style','infinite');
% light('Position',[0 0 1],'Style','infinite');
% light('Position',[-1 0 0],'Style','infinite');
% light('Position',[0 -1 0],'Style','infinite');
% % light('Position',[0 0 -1],'Style','infinite');

deltax2=deltax;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%ESLABON 2
lo=1.5;               %Lado del paralelepipedo base
deltax=1.5;           %Altura del paralelepipedo
l3=2;                 %Longitud del eslabon 3
xo=-1;                %Punto de origen en x
yo=1;                 %Punto de origen en y
zo=-1.5/2;            %Punto de origen en z
l2=7;
x1=xo;
x2=x1+l2;
x3=x1+l2;
x4=x1;
x5=x1;
x6=x1;
x7=x1+l2;
x8=x1+l2;

y1=yo;
y2=y1;
y3=y1+lo;
y4=y1+lo;
y5=y1+lo;
y6=y1;
y7=y1;
y8=y1+lo;

z1=zo;
z2=zo;
z3=zo;
z4=zo;
z5=zo+deltax;
z6=zo+deltax;
z7=zo+deltax;
z8=zo+deltax;
x=[x1 x2 x3 x4 x5 x6 x7 x8];
y=[y1 y2 y3 y4 y5 y6 y7 y8];
z=[z1 z2 z3 z4 z5 z6 z7 z8];

for n=1:8

T(1:4,n)=[cos(teta) -sin(teta) 0 0
   sin(teta) cos(teta) 0 0
   0 0 1 deltax2-deltax/2
   0 0 0 1]*[cos(teta2) 0 sin(teta2) 0
          0 1 0 0
          -sin(teta2) 0 cos(teta2) 0
          0 0 0 1]*[x(n);y(n);z(n);1];

% Ta(1:4,n)=[cos(teta) -sin(teta) 0 0
%    sin(teta) cos(teta) 0 0
%    0 0 1 0
%    0 0 0 1]*[x(n);y(n);z(n);1];
end

a=0.4;
tcolor(1,1,1:3) = [0.4 0.1 0.1 ];
A7=patch([T(1,1) T(1,2) T(1,3) T(1,4)],[T(2,1) T(2,2) T(2,3) T(2,4)],[T(3,1) T(3,2) T(3,3) T(3,4)],tcolor);
A8=patch([T(1,1) T(1,2) T(1,7) T(1,6)],[T(2,1) T(2,2) T(2,7) T(2,6)],[T(3,1) T(3,2) T(3,7) T(3,6)],tcolor);
A9=patch([T(1,5) T(1,6) T(1,7) T(1,8)],[T(2,5) T(2,6) T(2,7) T(2,8)],[T(3,5) T(3,6) T(3,7) T(3,8)],tcolor);
A10=patch([T(1,2) T(1,3) T(1,8) T(1,7)],[T(2,2) T(2,3) T(2,8) T(2,7)],[T(3,2) T(3,3) T(3,8) T(3,7)],tcolor);
A11=patch([T(1,1) T(1,4) T(1,5) T(1,6)],[T(2,1) T(2,4) T(2,5) T(2,6)],[T(3,1) T(3,4) T(3,5) T(3,6)],tcolor);
A12=patch([T(1,3) T(1,4) T(1,5) T(1,8)],[T(2,3) T(2,4) T(2,5) T(2,8)],[T(3,3) T(3,4) T(3,5) T(3,8)],tcolor);
% light('Position',[1 0 0],'Style','infinite');
% light('Position',[0 1 0],'Style','infinite');
% light('Position',[0 0 1],'Style','infinite');
% light('Position',[-1 0 0],'Style','infinite');
% light('Position',[0 -1 0],'Style','infinite');
% light('Position',[0 0 -1],'Style','infinite');

grid on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%//////Eslabon 3 //////////////////////////////////////////////////

lo=1;                   %Lado del paralelepipedo base
deltax=1;               %Altura del paralelepipedo
xo=-deltax/2;           %Punto de origen en x
yo=0;                   %Punto de origen en y
zo=-.5;                 %Punto de origen en z
l2=5.5;
x1=xo;
x2=x1+l2;
x3=x1+l2;
x4=x1;
x5=x1;
x6=x1;
x7=x1+l2;
x8=x1+l2;

y1=yo;
y2=y1;
y3=y1+lo;
y4=y1+lo;
y5=y1+lo;
y6=y1;
y7=y1;
y8=y1+lo;

z1=zo;
z2=zo;
z3=zo;
z4=zo;
z5=zo+deltax;
z6=zo+deltax;
z7=zo+deltax;
z8=zo+deltax;
x=[x1 x2 x3 x4 x5 x6 x7 x8];
y=[y1 y2 y3 y4 y5 y6 y7 y8];
z=[z1 z2 z3 z4 z5 z6 z7 z8];

for n=1:8
T(1:4,n)=[cos(teta) -sin(teta) 0 0
   sin(teta) cos(teta) 0 0
   0 0 1 deltax2-deltax/2
   0 0 0 1]*[cos(teta2) 0 sin(teta2) 0
          0 1 0 0
          -sin(teta2) 0 cos(teta2) 0
          0 0 0 1]*[cos(teta3) 0 sin(teta3) 5+deltax
          0 1 0 0
          -sin(teta3) 0 cos(teta3) 0
          0 0 0 1]*[x(n);y(n);z(n);1];
end

crgb=[249 193 120]/255;
tcolor(1,1,1:3) = [crgb(1) crgb(2) crgb(3)];

A13=patch([T(1,1) T(1,2) T(1,3) T(1,4)],[T(2,1) T(2,2) T(2,3) T(2,4)],[T(3,1) T(3,2) T(3,3) T(3,4)],tcolor);
A14=patch([T(1,1) T(1,2) T(1,7) T(1,6)],[T(2,1) T(2,2) T(2,7) T(2,6)],[T(3,1) T(3,2) T(3,7) T(3,6)],tcolor);
A15=patch([T(1,5) T(1,6) T(1,7) T(1,8)],[T(2,5) T(2,6) T(2,7) T(2,8)],[T(3,5) T(3,6) T(3,7) T(3,8)],tcolor);
A16=patch([T(1,2) T(1,3) T(1,8) T(1,7)],[T(2,2) T(2,3) T(2,8) T(2,7)],[T(3,2) T(3,3) T(3,8) T(3,7)],tcolor);
A17=patch([T(1,1) T(1,4) T(1,5) T(1,6)],[T(2,1) T(2,4) T(2,5) T(2,6)],[T(3,1) T(3,4) T(3,5) T(3,6)],tcolor);
A18=patch([T(1,3) T(1,4) T(1,5) T(1,8)],[T(2,3) T(2,4) T(2,5) T(2,8)],[T(3,3) T(3,4) T(3,5) T(3,8)],tcolor);
%
% light('Position',[1 0 0],'Style','infinite');
% light('Position',[0 1 0],'Style','infinite');
% light('Position',[0 0 1],'Style','infinite');
% light('Position',[-1 0 0],'Style','infinite');
% light('Position',[0 -1 0],'Style','infinite');
% light('Position',[0 0 -1],'Style','infinite');

lo=10;                %Lado del paralelepipedo base
deltax=1;             %Altura del paralelepipedo
xo=-lo/2;             %Punto de origen en x
yo=-lo/2;             %Punto de origen en y
zo=-deltax;           %Punto de origen en z

x1=xo;
x2=x1+lo;
x3=x1+lo;
x4=x1;
x5=x1;
x6=x1;
x7=x1+lo;
x8=x1+lo;

y1=yo;
y2=y1;
y3=y1+lo;
y4=y1+lo;
y5=y1+lo;
y6=y1;
y7=y1;
y8=y1+lo;

z1=zo;
z2=zo;
z3=zo;
z4=zo;
z5=zo+deltax;
z6=zo+deltax;
z7=zo+deltax;
z8=zo+deltax;
a=0.4;
tcolor(1,1,1:3) = [0.3 0.5 0.4];
patch([x1 x2 x3 x4],[y1 y2 y3 y4],[z1 z2 z3 z4],tcolor);
patch([x1 x2 x7 x6],[y1 y2 y7 y6],[z1 z2 z7 z6],tcolor);
patch([x5 x6 x7 x8],[y5 y6 y7 y8],[z5 z6 z7 z8],tcolor);
patch([x2 x3 x8 x7],[y2 y3 y8 y7],[z2 z3 z8 z7],tcolor);
patch([x1 x4 x5 x6],[y1 y4 y5 y6],[z1 z4 z5 z6],tcolor);
patch([x3 x4 x5 x8],[y3 y4 y5 y8],[z3 z4 z5 z8],tcolor);
%
% light('Position',[1 0 0],'Style','infinite');
% light('Position',[0 1 0],'Style','infinite');
% light('Position',[0 0 1],'Style','infinite');
% light('Position',[-1 0 0],'Style','infinite');
% light('Position',[0 -1 0],'Style','infinite');
% light('Position',[0 0 -1],'Style','infinite');

p=[0;0;0;1];

Tf=[cos(teta) -sin(teta) 0 0
   sin(teta) cos(teta) 0 0
   0 0 1 deltax2-deltax/2
   0 0 0 1]*[cos(teta2) 0 sin(teta2) 0
          0 1 0 0
          -sin(teta2) 0 cos(teta2) 0
          0 0 0 1]*[cos(teta3) 0 sin(teta3) 5+deltax
          0 1 0 0
          -sin(teta3) 0 cos(teta3) 0
          0 0 0 1]*p
Tf(2)=T(2,8)
Tf(3)=T(3,8)
Tf(1)=T(1,8)

E1=gca;
axis([-10 8 -10 8 0 15])
hold on
plot3(E1,p(1),p(2),p(3));
grid on


function block_size = decide_max_block_size_RBM(para)

if para.useGPU == 1
    gpu = gpuDevice;
    GPU_max_memory =  gpu.FreeMemory/8; % This is the maximum number of double precision numbers that can be stored in the GPU
    % Compute how much training data can we put into GPU
    inputDim = para.layerSize(1);
    outputDim = para.layerSize(end);
    max_sample = GPU_max_memory / (inputDim + outputDim);
    block_size = max_sample / 5;   % Let's be conservative, don't use all the memory of GPU
else
    block_size = 1e5;
end


function g = whitehKernGradient(kern, x, varargin)

% WHITEHKERNGRADIENT Gradient of WHITEH kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the
% whiteh noise
% kernel's parameters. As well as the kernel structure and the
% input positions, the user provides a matrix PARTIAL which gives
% the partial derivatives of the function with respect to the
% relevant elements of the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x : the input locations for which the gradients are being
% computed.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The argument takes
% the form of a square matrix of dimension  numData, where numData is
% the number of rows in X.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters. The ordering of the vector should match
% that provided by the function kernExtractParam.
%
% FORMAT
% DESC computes the derivatives as above, but input locations are
% now provided in two matrices associated with rows and columns of
% the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x1 : the input locations associated with the rows of the
% kernel matrix.
% ARG x2 : the input locations associated with the columns of the
% kernel matrix.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The matrix should
% have the same number of rows as X1 and the same number of columns
% as X2 has rows.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters.
%
% SEEALSO whitehKernParamInit, kernGradient, whitehKernDiagGradient, kernGradX
%
% COPYRIGHT : Mauricio A. Alvarez, Neil D. Lawrence, 2009

% KERN

if nargin < 4
  g(1, 1) = sum(diag(varargin{end}).*(1./x(:,end)));
else
  g = 0;
end


function PosMove=findpossmoveRec(graph,curr,ts)
%% Written by Muhammet Balcilar, France
% all rights reverved
cf=size(graph,1)/ts;
PosMove=[];
I1=find(graph(curr(1),:)==1);
for i1=1:length(I1)
    nx1=I1(i1);
    g1=graph;
    g1(:,nx1)=0;
    g1(nx1-cf,curr(1)+cf)=0;
    if length(curr)==1
        PosMove=[PosMove;nx1];
    else
        pm=findpossmoveRec(g1,curr(2:end),ts);
        if ~isempty(pm)
            PosMove=[PosMove;nx1*ones(size(pm,1),1) pm];
        end
    end

end


function ObjVal = step2(Chrom,mydata)
Dim=size(Chrom,2);
[Nind,Nvar] = size(Chrom);
Chrom=ceil(Chrom+0.5);
ObjVal = sum((Chrom .* Chrom)')';
return


function [gx,dgdx,dgdp] = g_demo_susceptibility(Xt,P,ut,in)

%- initializations
idX1 = 1:in.n5(end);
nreg=length(in.n5);
nresp=length(in.r);

gxr=zeros(nresp,1);
dgdxr=zeros(length(Xt),nresp);
dgdpr=zeros(length(P),nresp);

%== compute gradients
[gxn,dgdxn,dgdpn] = g_HRF3(Xt(idX1),P,ut,in) ;
if nresp>0
[gxr,dgdxr(in.r,:),dgdpr] = g_dummy(Xt(in.r),P,ut,in);
else
    gxr=[];
    dgdxr=[];
    dgdpr=[];
end
%== concatenate BOLD and behavioral responses
%- state
gx = [gxn ; gxr] ;

%- Jacobian
dgdx = zeros(length(Xt),length(gx));
dgdx(idX1,1:nreg) = dgdxn ;
dgdx(:,nreg+(1:nresp)) = dgdxr ;

%- gradient wrt parameters
dgdp = zeros(length(P),nreg+nresp);
dgdp(1:size(dgdpn,1),1:nreg) = dgdpn ;
dgdp(:,nreg+(1:nresp)) = dgdpr ;

end

function [gxr,dgdxr,dgdpr]=g_dummy(Xt,P,ut,in)
gxr = Xt * 10;
dgdxr =  10;
dgdpr = 0;
end


function [GX,dGdX,dGdP] = g_embedAR(X,P,ut,in)
% AR(1) evolution function

n = size(X,1);
x = X(1:n/2);
z = X(n/2+1:n);

[gx,J,dgdP] = VBA_evalFun('g',x,P,ut,in.opt,in.dim,0);

GX = gx;
dGdX = [J;zeros(n/2)];
dGdP = dgdP;


function oR = cleanUp(oR)

% testing:
% cs = crystalSymmetry('222');
% oR = cs.fundamentalRegion;

% we need to restrict to the relevant normals
% this works nice if rotations about 180 degree are present
is180 = abs(oR.N.angle)>pi - 1e-3;
if ~any(is180)

  % then the normals should be inside itself
  Nq = unique(oR.N);
  Nq = Nq.subSet(oR.checkInside(-reciprocal(Nq)));
  oR.N = Nq;

else % otherwise the condition seems to strict

  % we consider first the case without 180 degree rotations
  oRfull = oR;
  oRfull.N(is180) = [];
  oRfull = oRfull.cleanUp;

  ind = oR.checkInside(oRfull.V);

  inside = cellfun(@(x) any(ind(x)), oRfull.F);

  Nq = [oRfull.N(inside),oR.N(is180)];
  oR.N = Nq;
end

nqa = unique(Nq.axis,'antipodal');

if isempty(nqa)

  oR.V = orientation(oR.CS1,oR.CS2);
  oR.F = {};
  return

elseif length(nqa)==1

  v = perp(nqa);
  rot = rotation.byAxisAngle(nqa,(0:90:270)*degree);
  oR.V = orientation.byAxisAngle(rot*v,pi*ones(4,1),oR.CS1,oR.CS2);
  oR.F = repcell((1:4).',size(oR.N));

  return
end

% compute vertices

% set up faces
for j = 1:length(Nq)

  Nqj = Nq(j);
  oNq = Nq; oNq(j) = [];
  oNq(abs(dot(oNq.axis,Nqj.axis))>1-1e-6) = [];

  % find closest other normal
  [~,j0] = min(angle(Nqj,oNq) + angle(Nqj.axis,oNq.axis));

  % rotate axis such that current face is z-direction
  % and closest plane
  aNq = oNq.axis;
  aNq0 = aNq(j0);
  aNqj = Nqj.axis;
  aNq0 = aNq0 - dot(aNq0,aNqj) * aNqj;
  rot = rotation.map(aNqj,zvector,aNq0,xvector);
  aNq = rot * aNq; % the rotated axes

  % order the other normals according to
  % 1. angle to aNq0
  % 2. angle to Nqj
  [~,order] = sort(100*mod(aNq.rho+1e-5,2*pi) + aNq.theta);
  order(end+1) = order(1); %#ok<AGROW>

  % the first edge
  e = order(1);
  % go through all potential edges and compute vertices
  V{j} = quaternion;
  for i = 2:length(order)

    % if axis is in the same direction -> skip
    if isnull(aNq(order(i)).rho - aNq(e).rho), continue; end

    % is axis is inline whith Nqj -> skip
    %if abs(dot(aNq(order(i)),zvector))>1-1e-6, continue; end

    % compute vertice
    v = cross(Nqj,oNq(e),oNq(order(i)));
    v = v ./ norm(v);
    if abs(angle(v * Nqj))<1e-5 , continue; end

    % check vertex is inside
    if ~oR.checkInside(v), continue; end

    % if the rotational angle is 180 degree then +- axis is possible
    % take the quaternion is the axis inside the fundamental region

    if abs(v.a)<1e-5, v.a = sign(v.a)*1e-5; end

    if ~oR.axisSector.checkInside(v.axis) && (abs(v.a) < 1e-4)
      v.a = -v.a;
    end

    % if we have found a vertice - store it
    V{j}(end+1) = v./norm(v);
    e = order(i);

  end
end

% make a unique list of vertices
sV = cellfun(@length,V);
[V,~,iV] = unique([V{:}]);
oR.V = orientation(V,oR.CS1,oR.CS2);
sV = sV(sV>0);
F = 1:sum(sV);
F = mat2cell(F,1,sV);
F = cellfun(@(x) iV(x),F,'uniformOutput',false);
oR.F = cellfun(@(x) x([diff(x)~=0; true]),F,'uniformOutput',false);

end

function q = reciprocal(q)

q = q .* rotation.byAxisAngle(-q.axis,pi);

end


function model = ivmRun(XTrain, yTrain, options, kernelTieStructure, noiseTieStructure)

% IVMRUN Run the IVM on a given data set.
% FORMAT
% DESC takes an input data matrix, a target data matrix and an options
% structure (as defined by ivmOptions). It then creates and
% optimises an IVM using ivmOptimise.
% ARG X : the input training data.
% ARG y : the target training data.
% ARG options : options structure (as defined by ivmOptions).
%
% FORMAT
% DESC is as above but includes the facility to 'tie' some of the
% kernel an noise parameters together.
% ARG X : the input training data.
% ARG y : the target training data.
% ARG options : options structure (as defined by ivmOptions).
% ARG kernelTie : structure which speficies which kernel parameters
% are forced to be equal to each other (see cmpndTieParameters).
% ARG noiseTie : structure which speficies which noise parameters
% are forced to be equal to each other (see cmpndTieParameters).
%
% SEEALSO : ivmCreate, ivmOptimise, ivmOptions, cmpndTieParameters
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006, 2007

% IVM

% Intitalise IVM
model = ivmCreate(size(XTrain, 1), size(yTrain, 2), XTrain, yTrain, options);

if nargin > 8 & ~isempty(noiseTieStructure);
  % Some of the noise parameters are constrained equal to each other
  model.noise = cmpndTieParameters(model.noise, noiseTieStructure);
end
if nargin > 7 & ~isempty(kernelTieStructure);
  % Some of the kernel parameters are constrained to equal each other.
  model.kern = cmpndTieParameters(model.kern, kernelTieStructure);
end

% Optimise the parameters
model = ivmOptimise(model, options);

% Select data-points in an IVM with the given parameters.
model = ivmOptimiseIvm(model, options.display);


function [y,O,X,S] = filterAdjust(O)
% :Usage:
% ::
%
%     [y,O,X,S] = filterAdjust(OPTIONS)
%
% :O:
%
%   **O.y:**
%        signal
%
%   **O.HP:**
%        high pass freq. cutoff
%
%   **O.TR:**
%        sampling rate in s
%
%   **O.doHP:**
%        [0 or 1] - do HP filter (spm), default is 0
%
%   **O.doLP:**
%        [0, 1, 2] - do LP filter (spm), default is 0
%           2 = Gaussian filter with TR*2 s length
%
%   **O.firstimg:**
%        sets values for first image in each run to mean of the
%        remaining values.  Good for removing first-image artifacts present in
%        some scanner sequences.  Default is 1.
%
%   **O.cyclecorrection:**
%        checks for unimodal (normally distributed) data
%        within each session, because some bimodal data that cycles between two
%        mean scanner values has been observed in some data.  if a high proportion
%        of outliers are found in non-normal data, subtracts mean of higher mode
%        to adjust data. Sorry-not clear.  Check the code. Default is 0
%
%   **O.cyclecorrection2:**
%        removes large transitions from data, as in
%        detransition.m.  Default is 0.  Artifacts may be acquisition or
%        motion-correction/resampling related.
%
%        We do this after filtering, but if there's trouble, we re-do the
%        scanadjust and filtering, because 'cycling' can affect these.
%
%   **O.scanadjust:**
%        [0 or 1] - adjust to scan means, default is 0
%           - If O.X is entered, assumes this is the session mean matrix,
%           instead of recomputing.
%
%   **O.percent:**
%        [0 or 1] - adjust to percent change from baseline, default is 0
%
%   **O.filtertype:**
%        filter style, default is 'none'
%         - 'spm', use spm's filtering
%         - if O.S is entered, uses this instead of recomputing
%         - 'fourier', Doug's fourier filter
%         - 'fouriernotch', Omit frequencies between HP(1) and HP(2)
%         - 'cheby', chebyshev
%         - 'Luis', Luis' custom filter
%         - 'none', no filtering (or leave field out).
%
%   **O.HP:**
%        for SPM, the filter cutoff in s
%        for fourier, the HP value or the [HP LP] values
%        notches out everything slower than HP and faster than LP, in s
%        (1/s = Hz).
%
%   **O.nruns:**
%        number of runs (scanadjust), default is 1
%
%   **O.adjustmatrix:**
%        custom adjustment matrix to regress out (e.g., movement params)
%
%   **O.plot [0 or 1]:**
%        plots intermediate products, default is 0
%
%   **O.verbose [0 or 1]:**
%        verbose output
%
%   **O.trimts [0 or std]:**
%        trim overall timseries values to std, 0 for no trimming
%
%   **O.lindetrend:**
%        specify linear detrending of timeseries.
%        occurs after adjustment and filtering and windsorizing
%            - detrending option -> what to enter in this field:
%            - no detrending  -> empty, missing field, or 0
%            - detrending every n elements -> single number (n)
%            - piecewise linear detrend -> ROW vector of breakpoints
%              (do not specify 1 as the start of the 1st segment.)
%
% ..
%    by Tor Wager, 09/24/01
%    modified 10/15/02 to add trial baseline adjustment and linear detrending.
% ..

if ~isfield(O,'verbose'), O.verbose = 0;,end
if ~isfield(O,'trimts'), O.trimts = 0;,end
if ~isfield(O,'lindetrend'), O.lindetrend = 0;, end
if ~isfield(O,'percent'), O.percent = 0;, end

% out = voistat('adjusty',y,HChoice,TR,nruns, [opt] adjustmatrix);
% disp('*	voistat adjusty')

y = O.y;
TR = O.TR;
npoints = size(y,1);

if size(y,2) > size(y,1), y = y';, end

nruns = 1;
if isfield(O,'scanadjust'), if O.scanadjust, nruns = O.nruns;,end,else, O.scanadjust = 0;,end
if isfield(O,'adjustmatrix'), docustomadjust = 1;,adjustmatrix = O.adjustmatrix;,else, docustomadjust = 0;,adjustmatrix = [];,end

scanlen = npoints./nruns;
if scanlen ~= round(scanlen),warning(['Scan length is not an integer! scanlen = ' num2str(scanlen) ', rounded = ' num2str(round(scanlen))]),end
scanlen = round(scanlen);

wholeyavg = mean(y(~isnan(y)));
X = [];

if ~isfield(O,'doHP'), O.doHP = 0;,HChoice = [];,else, HChoice = O.HP;end
if O.doHP,if isempty(HChoice),error('filterAdjust: specify HP filter length in input options!'),end,end
if ~isfield(O,'doLP'), O.doLP = 0;,end
if ~isfield(O,'filtertype'), O.filtertype = 'none';,end
if ~isfield(O,'plot'), O.plot = 0;,end
if ~isfield(O,'cyclecorrection'), O.cyclecorrection = 0;,end
if ~isfield(O,'cyclecorrection2'), O.cyclecorrection2 = 0;,end
if ~isfield(O,'firstimg'), O.firstimg = 1;,end

% plot raw
if O.plot, f1 = figure('Color','w'); hold on; set(gca,'FontSize',16); plot(scale(y),'k');,title('Raw timeseries (standardized for display)'); pause(1);,end

if ((O.doHP | O.doLP) & strcmp(O.filtertype,'spm'))
	% MAKE FILTER - use_spm_filter.m
	% --------------------------------------------------------
	% Ran out of memory doing this with whole ts, so trying to make more efficient...do it scan by scan.
   if O.doHP | O.doLP
       if isfield(O,'S')
           if O.verbose, disp(	'		...voistat adjusty: Using input S filter: filter shape plot is final S matrix only'), end
           S = O.S; KL = O.S; KH = O.S;
       else

           if O.verbose, disp(	'		...voistat adjusty: making S filter'), end

           if O.doHP & O.doLP

               if O.doLP == 2

                   [S,KL,KH] = use_spm_filter(TR,scanlen,'Gaussian','specify',HChoice,TR*2);

               else

                   [S,KL,KH] = use_spm_filter(TR,scanlen,'hrf','specify',HChoice);

               end

           elseif O.doHP

               [S,KL,KH] = use_spm_filter(TR,scanlen,'none','specify',HChoice);

           elseif O.doLP == 2

               [S,KL,KH] = use_spm_filter(TR,scanlen,'Gaussian','none',[],TR*2);

           elseif O.doLP

               [S,KL,KH] = use_spm_filter(TR,scanlen,'hrf','none',[]);

           else error('Script bug.  Check filterAdjust.m')

           end

       end  % if S

   else warning('No filtering specified, though spm is filtertype.'),
      S = [];
   end

	if O.plot, figure;
   		subplot(1,3,1);imagesc(S); title('Smoothing matrix (spm) *used')
   		subplot(1,3,2);imagesc(KL); title('Low-pass filter(spm)')
      subplot(1,3,3);imagesc(KH); title('High-pass filter(spm)')
      pause(1)
      close
    end

end

if (O.doHP | O.doLP) & strcmp(O.filtertype,'none')
    error('For filtering to be used, enter ''spm'' ''cheby'' ''doug'', etc. in O.filtertype.')
end

if O.cyclecorrection2
    if O.verbose, disp('		...removing large transitions from data (presumably artifacts).'),end

    for startimg = 1:scanlen:npoints
        tmp = y(startimg:startimg+scanlen-1);
        tmp = detransition(tmp,O.plot);
        y(startimg:startimg+scanlen-1) = tmp;
    end

    if O.plot, figure(f1); hold off; plot(scale(y),'k'); hold on; title('Raw (detransitioned, std for display)');,pause(1);end
end

if O.scanadjust
	% adjust scan means
	% --------------------------------------------------------
	if O.verbose, disp('		...adjusting scan means to session mean.'),end

    if isfield(O,'X')
        X = O.X;
    else
	    index = 1;
	    for startimg = 1:scanlen:npoints
		    X(startimg:startimg+scanlen-1,index) = 1;
		    index = index + 1;
	    end
    end

	if size(X,1) > size(y,1)
		disp(['	* * * voistat.m adjusty: WARNING: X is longer than y.  truncating.'])
		X = X(1:length(y),:);
	end

   %y = y - (X * (X \ y));	% y-xB to adjust to mean.
   y = y - X * pinv(X) * y;

   if O.plot, figure;
      imagesc(X); title('Model matrix for effects to regress out.')
      pause(1),close

      figure(f1); hold off; plot(scale(y),'k'); hold on; title('Session means removed, std for display)');,pause(1);
   end

end

if O.firstimg
    if O.verbose, disp('		...Setting values of first 2 images in each run to timeseries mean value.'),end
    tmp = 1:scanlen:length(y);
    tmp = [tmp 2:scanlen:length(y)];

    % first adjust to session means - rough estimate
    tmpn = 1:length(y); tmpn(tmp) = [];
    y(tmp) = mean(y(tmpn));

    % then adjust to expected
    tmpx = (1:length(y))';
    [P] = polyfit(tmpx,y,3);
    yy = polyval(P,tmpx);
    y(tmp) = yy(tmp);

    if O.plot, figure(f1); plot(scale(y),'m'); hold on;
        plot(scale(yy));
        title('Raw (1-2 removed, std for display)');,
    end
end

switch O.filtertype

case 'spm'
% filter each scan
% --------------------------------------------------------
if ~isempty(S)
   filty = [];
	if O.verbose, disp('		...applying to each session: high-pass filtering.'),end

	% whos y

	for startimg = 1:scanlen:npoints
		try
			sessy = y(startimg:startimg+scanlen-1,:);
		catch
			startimg
			whos y
			error(['	voistat adjusty: ERROR.  y is too short, npoints is wrong, or numscans is wrong? Check GUI values.'])
		end

		try
			sessy = S * sessy;
		catch
			try
				sessy = (S * sessy')';
			catch
				disp('		can''t multiply S and sessy: matrix dimensions not correct. Try transposing y?')
				whos S
				whos sessy
				disp(['Scanlength = ' num2str(scanlen)])
				disp(['total time pts = ' num2str(npoints)])
				disp(['Num Sessions = ' num2str(nruns)])
				error('Exiting now...')
			end
		end

		filty = [filty; sessy];
   end

% whos filty

   if O.plot
      figure(f1);hold off; plot(scale(y),'b','LineWidth',1);, hold on; plot(scale(filty),'r','LineWidth',1);
      legend({'Before filtering' 'After filtering'})

      %pause(1),close
   end

   y = filty;

end

   % Old custom filter stuff.
       %Wn = .35;
       %[B,A] = cheby2(3,40,Wn);
       %y = filter(B,A,y);

% Doug's custom fourier LP filter.
%len = length(y) /2;
%ind = [1:len] ./ len;
%ff = 1./(1+exp((ind-.30)./0.05));
%ff2 = [ff ff(len:-1:1)];
%y = real(ifft(fft(y).*ff2'));

case 'cheby'
if ~isempty(HChoice)
	nyquist = TR/2;	% in seconds, not frequencies TR/nyquist = .5
	Wn = [TR/HChoice .35];
	if O.verbose, disp([		'voistat adjusty: Chebyshev filter freqency is ' num2str(Wn)]),end
	for startimg = 1:scanlen:npoints
		sessy = y(startimg:startimg+scanlen-1);
 		[B,A] = cheby2(1,20,Wn);
		sessy = filter(B,A,y);
	end
end

case 'fourier'
if ~isempty(HChoice)
	filty = []; nyquist = (1/TR)/2; lowerlim = 1/HChoice(1);
	hzfreqs = (1:scanlen) ./ (scanlen * TR);
	if length(HChoice) == 1
        ftopass = hzfreqs >= lowerlim & hzfreqs <= nyquist;
    else
        ftopass = hzfreqs >= lowerlim & hzfreqs <= 1./(HChoice(2));
    end
	if O.verbose, disp(['		...applying to each session: notch filtering: ' num2str(lowerlim) ' to ' num2str(nyquist) ' Hz.']),end
	for startimg = 1:scanlen:npoints
		myfft = fft(y(startimg:startimg+scanlen-1));
		mypass = zeros(1,scanlen);
		mypass(ftopass) = myfft(ftopass);
		% plot(abs(myfft));hold on;plot(abs(mypass),'rx');
		sessy = real(ifft(mypass))';
		filty = [filty; sessy];
	end
	y = filty;
    %hold on; plot(y,'r');
end

case 'fouriernotch'
if ~isempty(HChoice)
	filty = []; nyquist = (1/TR)/2; lowerlim = 1/HChoice(1);
	hzfreqs = (1:scanlen) ./ (scanlen * TR);
    ftopass = ones(size(hzfreqs));
    ftopass(hzfreqs > lowerlim & hzfreqs < 1./(HChoice(2))) = 0;
    ftopass = find(ftopass);

	if O.verbose, disp(['		...applying to each session: notch filtering: ' num2str(lowerlim) ' to ' num2str(nyquist) ' Hz.']),end
	for startimg = 1:scanlen:npoints
		myfft = fft(y(startimg:startimg+scanlen-1));
		mypass = zeros(1,scanlen);
		mypass(ftopass) = myfft(ftopass);
		%figure; plot(abs(myfft));hold on;plot(abs(mypass),'rx');
		sessy = real(ifft(mypass))';
		filty = [filty; sessy];
	end
	y = filty;
    %hold on; plot(y,'r');
end

case 'bandpass'
if ~isempty(HChoice)
	filty = []; nyquist = (1/TR)/2; lowerlim = 1/HChoice(1);
	hzfreqs = (1:scanlen) ./ (scanlen * TR);
    ftopass = zeros(size(hzfreqs));
    ftopass(hzfreqs > lowerlim & hzfreqs < 1./(HChoice(2))) = 1;
    ftopass = find(ftopass);

	if O.verbose, disp(['		...applying to each session: band pass filtering: ' num2str(lowerlim) ' to ' num2str(1./(HChoice(2))) ' Hz.']),end
	for startimg = 1:scanlen:npoints
		myfft = fft(y(startimg:startimg+scanlen-1));
		mypass = zeros(1,scanlen);
		mypass(ftopass) = myfft(ftopass);
		%figure; plot(abs(myfft));hold on;plot(abs(mypass),'rx');
		sessy = real(ifft(mypass))';
		filty = [filty; sessy];
	end
	y = filty;
    %hold on; plot(y,'r');
end

case 'Luis'
   if O.plot
      y = luisFilter(y,O.TR,.35,'v');pause(1),close,pause(1),close
   else
      y = luisFilter(y,O.TR,.35);
   end

case 'none'

end % end switch

if O.cyclecorrection
    % check for bimodal 'cycling' of mean BOLD signal within sessions, and
    % correct if necessary, before removing scan means, etc.
    % if we find problems, then re-do session-centering
	% --------------------------------------------------------
    if O.verbose, disp('		...checking for normally distributed data (to avoid bimodal cycling observed in some data).'),end
	cycor = []; ind = 1; classes = [];
    for startimg = 1:scanlen:npoints
		   tmp = y(startimg:startimg+scanlen-1);

           [tmp,IDX] = mixture_model(tmp,O.plot);

           if max(IDX) > 1 & ~isempty(S)
               tmp = S * tmp;
               if O.plot, subplot(1,3,1),hold on;plot(tmp,'g'),end
           end

           y(startimg:startimg+scanlen-1) = tmp - mean(tmp);
           classes(ind) = max(IDX);

            ind = ind+1;
    end
    cycor = classes > 1;

        if O.verbose,

            fprintf(1,'		...%3.0f Sessions were not normally distributed.\n',sum(cycor)),
            if any(cycor), fprintf(1,['\t\t\tClasses by session: ' num2str(classes) '\n']),end
        else
            if any(cycor), fprintf(1,['\t\t\tClasses by session: ' num2str(classes) '\n']),end
        end
end

% do overall trimming here
% --------------------------------------------------------
if O.trimts
    [y,ntrimmed] = trimts(y,O.trimts,[],1);
    if O.verbose,fprintf(1,['\t\t...Windsorized ' num2str(ntrimmed) ' points from overall timeseries to ' num2str(O.trimts) ' std. deviations.\n']), end

    if O.plot
       figure(f1); plot(scale(y),'k--');, legend({'Before filtering' 'After filtering' 'After filt+trimming'})
    end
    %tpts = abs(y) > O.trimts .* std(y);
    %y(tpts) = NaN;
end

if O.lindetrend
     % linear detrending at specified knot points

    if O.verbose, fprintf(1,['\t\t...piecewise linear detrending requested with ' num2str(length(O.lindetrend)) ' breakpoints.\n']),end
    % set bp to [] for removal of one linear trend from the whole column
    if O.lindetrend == 1, O.lindetrend = [];, end
    % if a single number, interpret as knotpoints every n elements.

    if length(O.lindetrend) == 1, O.lindetrend = O.lindetrend:O.lindetrend:length(y);, end

    if O.plot,
        figure; plot(y);,
        set(gcf,'Position',[144         542        1306         504],'Color','w')
        disp(['         Breakpoints: ' num2str(O.lindetrend)])
    end
    hold on;
    y = detrend(y,'linear',O.lindetrend);
    y = y + wholeyavg;  % preserve mean of the original timeseries

    if O.plot,
        figure(f1); hold on; plot(y,'m');
        plot([O.lindetrend' O.lindetrend']',[ones(length(O.lindetrend),1) * min(y) ones(length(O.lindetrend),1)*max(y)]','k')
        legend({'Original' 'Detrended'})

        mystd = std(y) * 3;
        plot([0 length(y)],[mean(y)+mystd mean(y)+mystd],'k--')
        plot([0 length(y)],[mean(y)-mystd mean(y)-mystd],'k--')
        text(length(y)-10,mean(y)+mystd,'3 std lines')

        drawnow
        pause(1)
        %close
    end
end

if O.percent

	if O.verbose, disp(['		...converting to % change from overall mean:  original mean is ' num2str(wholeyavg)]),end
	y = (y-mean(y))*100 / wholeyavg;		% percent change from 0.

end

if docustomadjust
		if O.verbose, disp('		...adjusting for user-entered covariates -e.g. movement params'),end
 		if sum(sum(isnan(adjustmatrix))) > 0,
			disp('		...WARNING: NaN values in user covariates!  Setting these to 0.')
			adjustmatrix(isnan(adjustmatrix)) = 0;
		end
		try
			X = [adjustmatrix];
            X(:,end+1) = 1;     % add intercept
		catch
			warning('		voistat ''adjusty'': X and adjustmatrix are different sizes.  no covariates entered.')
			whos X
			whos adjustmatrix
        end
        O.custombeta = pinv(X) * y;
        y = y - X * O.custombeta;
 end

% figure; plot(abs(fft(y)),'ro');set(gca,'YLim',[0 1000])

return
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%