function [brainMask,checkSlices] = mrAnatExtractBrain(img, mmPerVox, betLevel, outFile)
%Uses the FSL BET tool to compute a brain mask for the given image volume.
%
% [brainMask,checkSlices] = mrAnatExtractBrain([img], [mmPerVox=[1,1,1]], [betLevel=0.5], [outFile])
%
% The betLevel is the BET fractional intensity threshold parameter. It
% should be [0-1]. The default of 0.5 usually works well. Smaller values
% will yield a larger brain estimate. If you get brain stuff chopped off,
% try a lower value. If you get non-brain stuff included, try a larger
% value.
%
% If you provide an array of betLevel values, you'll get a cell array of
% length(betLevel) brain masks back.
%
% checkSlices is a montage of every 3rd slice with the mask shown in blue
% and the underlying anatomy on yellow. Again, for length(betLevels)>1,
% you'll get a cell array.
%
% img can be a string, in which case it is assumed to be a NIFTI filename.
% or, it can be a NIFTI struct (as from niftiRead). In both cases,
% mmPerVox is ignored and instead gleaned from the NIFTI header.
%
% If no output arguments are captured, the brain mask is saved in the same
% place as the input file, but with '_mask' appended to the name.
%
% HISTORY:
% 2006.02.02 RFD: wrote it.
%
% Bob (c) Stanford VISTASOFT, 2006

if(~exist('betLevel','var') || isempty(betLevel)), betLevel = 0.5; end
if(~exist('mmPerVox','var') || isempty(mmPerVox)), mmPerVox = [1 1 1]; end

if(~exist('img','var') || isempty(img))
    [f,p] = uigetfile({'*.nii.gz';'*.*'},'Select a t1-weighted NIFTI file...');
    if(isnumeric(f)), disp('User canceled.'); return; end
    img = fullfile(p,f);
end

if(isstruct(img))
    % It's a nifti image
    ni = img;
    mmPerVox = ni.pixdim;
    img = ni.data;
elseif(ischar(img))
    % It's a nifti filename
    ni = niftiRead(img);
    mmPerVox = ni.pixdim;
elseif(nargout==0)
    error('If you pass in raw image data, you must capture at least one output!');
end

% TODO: more robust way to find cygwin on PC
if(ispc), bash = 'c:/cygwin/bin/bash.exe';
else      bash = 'bash';
end

% First try the shell for the preferrred FSL location, if it is not found
% then we'll test to see if the user is running OSX, if so then we'll point
% to an OSX version of bet2 (bet2_osx) else we'll point to the linux
% version.
[stat,res] = system('which bet2');
if(stat==0 && ~isempty(res))
    bet = res(1:end-1);
else
    if(ismac)
        bet = fullfile(fileparts(which(mfilename)), 'bet2_osx');
    else
        bet = fullfile(fileparts(which(mfilename)), 'bet2');
    end
end

bet = strtrim(bet); % The name of the bet funciton on the system

% Set temporary file names for the out file and bet script. These will be
% removed at the end of the function run.
out = [tempname, '_bet_temp'];
betScript = [tempname, '_bet_script.sh'];

if(ischar(img))
    out = img;
else
    img = double(img);
    mx = max(abs(img(:)));
    img = int16(round(img./mx.*32767));
    analyzeWrite(img, out, mmPerVox);   % Why not write a nifti?
end

% We could specify a better starting position for the BET surface
% sphere estimate, e.g., by using the talairach landmarks.
if(nargout==0)
    [p,f,e] = fileparts(ni.fname);
    [x,f,e2] = fileparts(f);
    imBaseName = fullfile(p,f);
end

%% Run brain extraction tool for each level
for ii=1:numel(betLevel)
    betOut = tempname;
    betCmd = [bet ' ' out ' ' betOut ' -mnf ' num2str(betLevel(ii))];
    fid = fopen(betScript,'wt');
    fprintf(fid,'#!/bin/bash\nexport FSLOUTPUTTYPE=NIFTI_GZ\n%s\n',betCmd);
    fclose(fid);
    unix([bash ' ' betScript]);
    betOut = [betOut '_mask.nii.gz'];
    if(nargout==0)
        if(numel(betLevel)>1)
            movefile(betOut, sprintf('%s_mask_f%02d',imBaseName,betLevel(ii)*100));
        else
            movefile(betOut, [imBaseName '_mask.nii.gz']);
        end
    else
        brainMaskFile{ii} = betOut;
    end
end

%% Manage outputs, but not sure about the logic here.
if(nargout>0)
    tmp = niftiRead(betOut);
    brainMask{ii} = logical(tmp.data);
    if(nargout>1)
        slices = [1:3:size(brainMask{ii},3)-6];
        if(ischar(img))
            im = double(makeMontage(ni.data, slices));
        else
            im = double(makeMontage(img, slices));
        end
        im = mrAnatHistogramClip(im, 0.4, 0.99);
        im = uint8(round(im./max(im(:)).*255));
        im(:,:,2) = im(:,:,1);
        im(:,:,3) = uint8(double(makeMontage(brainMask{ii},slices)).*255);
        checkSlices{ii} = im;
    end
end

if(nargout>0)
    if(length(betLevel)==1)
        brainMask = brainMask{1};
        if(exist('checkSlices','var')), checkSlices = checkSlices{1}; end
    end
else
    clear all;
end

%% Clean up

% Remove the betScript from the tempdir
delete(betScript)

% If img and out are the same then it is a temporary file and should be removed.
% If not, then it was passed in from a location on disk and should be preserved
if ~strcmp(img, out) && strfind(out, tempdir) && exist(out,'file')
    delete(out)
end

%% Return

return

%% Example script

bd = pwd;
d = dir('*0*');
subDirs = {d.name};
betLevel = 0.45;

for(ii=1:length(subDirs))
    t1 = fullfile(bd,subDirs{ii},'t1','t1');
    if(exist([t1 '.nii.gz'],'file'))
        t1Ni = niftiRead([t1 '.nii.gz']);
        [brainMask,checkSlices] = mrAnatExtractBrain(t1Ni.data, t1Ni.pixdim, betLevel);
        figure(99); image(checkSlices); axis image tight off;
        dtiWriteNiftiWrapper(uint8(brainMask), t1Ni.qto_xyz, [t1 '_mask.nii.gz']);
        ccPerPixel = prod(t1Ni.pixdim(1:3))/1000;
        brainVolumeCc(ii) = numel(find(brainMask)) * ccPerPixel;
    end
end

outFile = 'brainVolumes.txt';
fid = fopen(outFile,'wt');
for(ii=1:length(subDirs))
    %t1Mask = niftiRead(fullfile(bd,subDirs{ii},'t1','t1_mask.nii.gz'));
    %ccPerPixel = prod(t1Mask.pixdim(1:3))/1000;
    %brainVolumeCc(ii) = numel(find(t1Mask.data)) * ccPerPixel;
    fprintf(fid,'%s\t%0.1f\n',subDirs{ii},brainVolumeCc(ii));
end
fclose(fid);


function [models,M,test_set] = esvm_download_models(cls)
% Download a pre-trained PASCAL VOC2007 model from my MIT CSAIL
% homepage (http://people.csail.mit.edu/tomasz/)
%
% Copyright (C) 2011-12 by Tomasz Malisiewicz
% All rights reserved.
%
% This file is part of the Exemplar-SVM library and is made
% available under the terms of the MIT license (see COPYING file).
% Project homepage: https://github.com/quantombone/exemplarsvm

f = sprintf('%s.mat',cls);
models = [];
M = [];
test_set = {};
if ~exist(f,'file')
  unix(sprintf('wget http://people.csail.mit.edu/tomasz/exemplarsvm/models/%s',f));
else
  fprintf(1,'Found %s, not downloading\n',f);
end

if nargout == 0
  return;
end

load(f);


function json=savejson(rootname,obj,varargin)
%
% json=savejson(rootname,obj,filename)
%    or
% json=savejson(rootname,obj,opt)
% json=savejson(rootname,obj,'param1',value1,'param2',value2,...)
%
% convert a MATLAB object (cell, struct or array) into a JSON (JavaScript
% Object Notation) string
%
% author: Qianqian Fang (fangq<at> nmr.mgh.harvard.edu)
%            created on 2011/09/09
%
% $Id: savejson.m 415 2013-10-07 16:38:31Z fangq $
%
% input:
%      rootname: name of the root-object, if set to '', will use variable name
%      obj: a MATLAB object (array, cell, cell array, struct, struct array)
%      filename: a string for the file name to save the output JSON data
%      opt: a struct for additional options, use [] if all use default
%        opt can have the following fields (first in [.|.] is the default)
%
%        opt.FileName [''|string]: a file name to save the output JSON data
%        opt.FloatFormat ['%.10g'|string]: format to show each numeric element
%                         of a 1D/2D array;
%        opt.ArrayIndent [1|0]: if 1, output explicit data array with
%                         precedent indentation; if 0, no indentation
%        opt.ArrayToStruct[0|1]: when set to 0, savejson outputs 1D/2D
%                         array in JSON array format; if sets to 1, an
%                         array will be shown as a struct with fields
%                         "_ArrayType_", "_ArraySize_" and "_ArrayData_"; for
%                         sparse arrays, the non-zero elements will be
%                         saved to _ArrayData_ field in triplet-format i.e.
%                         (ix,iy,val) and "_ArrayIsSparse_" will be added
%                         with a value of 1; for a complex array, the
%                         _ArrayData_ array will include two columns
%                         (4 for sparse) to record the real and imaginary
%                         parts, and also "_ArrayIsComplex_":1 is added.
%        opt.ParseLogical [0|1]: if this is set to 1, logical array elem
%                         will use true/false rather than 1/0.
%        opt.NoRowBracket [1|0]: if this is set to 1, arrays with a single
%                         numerical element will be shown without a square
%                         bracket, unless it is the root object; if 0, square
%                         brackets are forced for any numerical arrays.
%        opt.ForceRootName [0|1]: when set to 1 and rootname is empty, savejson
%                         will use the name of the passed obj variable as the
%                         root object name; if obj is an expression and
%                         does not have a name, 'root' will be used; if this
%                         is set to 0 and rootname is empty, the root level
%                         will be merged down to the lower level.
%        opt.Inf ['"$1_Inf_"'|string]: a customized regular expression pattern
%                         to represent +/-Inf. The matched pattern is '([-+]*)Inf'
%                         and $1 represents the sign. For those who want to use
%                         1e999 to represent Inf, they can set opt.Inf to '$11e999'
%        opt.NaN ['"_NaN_"'|string]: a customized regular expression pattern
%                         to represent NaN
%        opt.JSONP [''|string]: to generate a JSONP output (JSON with padding),
%                         for example, if opt.JSON='foo', the JSON data is
%                         wrapped inside a function call as 'foo(...);'
%        opt.UnpackHex [1|0]: conver the 0x[hex code] output by loadjson
%                         back to the string form
%        opt can be replaced by a list of ('param',value) pairs. The param
%        string is equivallent to a field in opt.
% output:
%      json: a string in the JSON format (see http://json.org)
%
% examples:
%      a=struct('node',[1  9  10; 2 1 1.2], 'elem',[9 1;1 2;2 3],...
%           'face',[9 01 2; 1 2 3; NaN,Inf,-Inf], 'author','FangQ');
%      savejson('mesh',a)
%      savejson('',a,'ArrayIndent',0,'FloatFormat','\t%.5g')
%
% license:
%     BSD license, see LICENSE_BSD.txt files for details
%
% -- this function is part of jsonlab toolbox (http://iso2mesh.sf.net/cgi-bin/index.cgi?jsonlab)
%

if(nargin==1)
   varname=inputname(1);
   obj=rootname;
   if(isempty(varname))
      varname='root';
   end
   rootname=varname;
else
   varname=inputname(2);
end
if(length(varargin)==1 && ischar(varargin{1}))
   opt=struct('FileName',varargin{1});
else
   opt=varargin2struct(varargin{:});
end
opt.IsOctave=exist('OCTAVE_VERSION');
rootisarray=0;
rootlevel=1;
forceroot=jsonopt('ForceRootName',0,opt);
if((isnumeric(obj) || islogical(obj) || ischar(obj) || isstruct(obj) || iscell(obj)) && isempty(rootname) && forceroot==0)
    rootisarray=1;
    rootlevel=0;
else
    if(isempty(rootname))
        rootname=varname;
    end
end
if((isstruct(obj) || iscell(obj))&& isempty(rootname) && forceroot)
    rootname='root';
end
json=obj2json(rootname,obj,rootlevel,opt);
if(rootisarray)
    json=sprintf('%s\n',json);
else
    json=sprintf('{\n%s\n}\n',json);
end

jsonp=jsonopt('JSONP','',opt);
if(~isempty(jsonp))
    json=sprintf('%s(%s);\n',jsonp,json);
end

% save to a file if FileName is set, suggested by Patrick Rapin
if(~isempty(jsonopt('FileName','',opt)))
    fid = fopen(opt.FileName, 'wt');
    fwrite(fid,json,'char');
    fclose(fid);
end

%%-------------------------------------------------------------------------
function txt=obj2json(name,item,level,varargin)

if(iscell(item))
    txt=cell2json(name,item,level,varargin{:});
elseif(isstruct(item))
    txt=struct2json(name,item,level,varargin{:});
elseif(ischar(item))
    txt=str2json(name,item,level,varargin{:});
else
    txt=mat2json(name,item,level,varargin{:});
end

%%-------------------------------------------------------------------------
function txt=cell2json(name,item,level,varargin)
txt='';
if(~iscell(item))
        error('input is not a cell');
end

dim=size(item);
len=numel(item); % let's handle 1D cell first
padding1=repmat(sprintf('\t'),1,level-1);
padding0=repmat(sprintf('\t'),1,level);
if(len>1)
    if(~isempty(name))
        txt=sprintf('%s"%s": [\n',padding0, checkname(name,varargin{:})); name='';
    else
        txt=sprintf('%s[\n',padding0);
    end
elseif(len==0)
    if(~isempty(name))
        txt=sprintf('%s"%s": []',padding0, checkname(name,varargin{:})); name='';
    else
        txt=sprintf('%s[]',padding0);
    end
end
for i=1:len
    txt=sprintf('%s%s%s',txt,padding1,obj2json(name,item{i},level+(len>1),varargin{:}));
    if(i<len) txt=sprintf('%s%s',txt,sprintf(',\n')); end
end
if(len>1) txt=sprintf('%s\n%s]',txt,padding0); end

%%-------------------------------------------------------------------------
function txt=struct2json(name,item,level,varargin)
txt='';
if(~isstruct(item))
	error('input is not a struct');
end
len=numel(item);
padding1=repmat(sprintf('\t'),1,level-1);
padding0=repmat(sprintf('\t'),1,level);
sep=',';

if(~isempty(name))
    if(len>1) txt=sprintf('%s"%s": [\n',padding0,checkname(name,varargin{:})); end
else
    if(len>1) txt=sprintf('%s[\n',padding0); end
end
for e=1:len
  names = fieldnames(item(e));
  if(~isempty(name) && len==1)
        txt=sprintf('%s%s"%s": {\n',txt,repmat(sprintf('\t'),1,level+(len>1)), checkname(name,varargin{:}));
  else
        txt=sprintf('%s%s{\n',txt,repmat(sprintf('\t'),1,level+(len>1)));
  end
  if(~isempty(names))
    for i=1:length(names)
	txt=sprintf('%s%s',txt,obj2json(names{i},getfield(item(e),...
             names{i}),level+1+(len>1),varargin{:}));
        if(i<length(names)) txt=sprintf('%s%s',txt,','); end
        txt=sprintf('%s%s',txt,sprintf('\n'));
    end
  end
  txt=sprintf('%s%s}',txt,repmat(sprintf('\t'),1,level+(len>1)));
  if(e==len) sep=''; end
  if(e<len) txt=sprintf('%s%s',txt,sprintf(',\n')); end
end
if(len>1) txt=sprintf('%s\n%s]',txt,padding0); end

%%-------------------------------------------------------------------------
function txt=str2json(name,item,level,varargin)
txt='';
if(~ischar(item))
        error('input is not a string');
end
item=reshape(item, max(size(item),[1 0]));
len=size(item,1);
sep=sprintf(',\n');

padding1=repmat(sprintf('\t'),1,level);
padding0=repmat(sprintf('\t'),1,level+1);

if(~isempty(name))
    if(len>1) txt=sprintf('%s"%s": [\n',padding1,checkname(name,varargin{:})); end
else
    if(len>1) txt=sprintf('%s[\n',padding1); end
end
isoct=jsonopt('IsOctave',0,varargin{:});
for e=1:len
    if(isoct)
        val=regexprep(item(e,:),'\\','\\');
        val=regexprep(val,'"','\"');
        val=regexprep(val,'^"','\"');
    else
        val=regexprep(item(e,:),'\\','\\\\');
        val=regexprep(val,'"','\\"');
        val=regexprep(val,'^"','\\"');
    end
    if(len==1)
        obj=['"' checkname(name,varargin{:}) '": ' '"',val,'"'];
	if(isempty(name)) obj=['"',val,'"']; end
        txt=sprintf('%s%s%s%s',txt,repmat(sprintf('\t'),1,level),obj);
    else
        txt=sprintf('%s%s%s%s',txt,repmat(sprintf('\t'),1,level+1),['"',val,'"']);
    end
    if(e==len) sep=''; end
    txt=sprintf('%s%s',txt,sep);
end
if(len>1) txt=sprintf('%s\n%s%s',txt,padding1,']'); end

%%-------------------------------------------------------------------------
function txt=mat2json(name,item,level,varargin)
if(~isnumeric(item) && ~islogical(item))
        error('input is not an array');
end

padding1=repmat(sprintf('\t'),1,level);
padding0=repmat(sprintf('\t'),1,level+1);

if(length(size(item))>2 || issparse(item) || ~isreal(item) || ...
   isempty(item) ||jsonopt('ArrayToStruct',0,varargin{:}))
    if(isempty(name))
    	txt=sprintf('%s{\n%s"_ArrayType_": "%s",\n%s"_ArraySize_": %s,\n',...
              padding1,padding0,class(item),padding0,regexprep(mat2str(size(item)),'\s+',',') );
    else
    	txt=sprintf('%s"%s": {\n%s"_ArrayType_": "%s",\n%s"_ArraySize_": %s,\n',...
              padding1,checkname(name,varargin{:}),padding0,class(item),padding0,regexprep(mat2str(size(item)),'\s+',',') );
    end
else
    if(isempty(name))
    	txt=sprintf('%s%s',padding1,matdata2json(item,level+1,varargin{:}));
    else
        if(numel(item)==1 && jsonopt('NoRowBracket',1,varargin{:})==1)
            numtxt=regexprep(regexprep(matdata2json(item,level+1,varargin{:}),'^\[',''),']','');
           	txt=sprintf('%s"%s": %s',padding1,checkname(name,varargin{:}),numtxt);
        else
    	    txt=sprintf('%s"%s": %s',padding1,checkname(name,varargin{:}),matdata2json(item,level+1,varargin{:}));
        end
    end
    return;
end
dataformat='%s%s%s%s%s';

if(issparse(item))
    [ix,iy]=find(item);
    data=full(item(find(item)));
    if(~isreal(item))
       data=[real(data(:)),imag(data(:))];
       if(size(item,1)==1)
           % Kludge to have data's 'transposedness' match item's.
           % (Necessary for complex row vector handling below.)
           data=data';
       end
       txt=sprintf(dataformat,txt,padding0,'"_ArrayIsComplex_": ','1', sprintf(',\n'));
    end
    txt=sprintf(dataformat,txt,padding0,'"_ArrayIsSparse_": ','1', sprintf(',\n'));
    if(size(item,1)==1)
        % Row vector, store only column indices.
        txt=sprintf(dataformat,txt,padding0,'"_ArrayData_": ',...
           matdata2json([iy(:),data'],level+2,varargin{:}), sprintf('\n'));
    elseif(size(item,2)==1)
        % Column vector, store only row indices.
        txt=sprintf(dataformat,txt,padding0,'"_ArrayData_": ',...
           matdata2json([ix,data],level+2,varargin{:}), sprintf('\n'));
    else
        % General case, store row and column indices.
        txt=sprintf(dataformat,txt,padding0,'"_ArrayData_": ',...
           matdata2json([ix,iy,data],level+2,varargin{:}), sprintf('\n'));
    end
else
    if(isreal(item))
        txt=sprintf(dataformat,txt,padding0,'"_ArrayData_": ',...
            matdata2json(item(:)',level+2,varargin{:}), sprintf('\n'));
    else
        txt=sprintf(dataformat,txt,padding0,'"_ArrayIsComplex_": ','1', sprintf(',\n'));
        txt=sprintf(dataformat,txt,padding0,'"_ArrayData_": ',...
            matdata2json([real(item(:)) imag(item(:))],level+2,varargin{:}), sprintf('\n'));
    end
end
txt=sprintf('%s%s%s',txt,padding1,'}');

%%-------------------------------------------------------------------------
function txt=matdata2json(mat,level,varargin)
if(size(mat,1)==1)
    pre='';
    post='';
    level=level-1;
else
    pre=sprintf('[\n');
    post=sprintf('\n%s]',repmat(sprintf('\t'),1,level-1));
end
if(isempty(mat))
    txt='null';
    return;
end
floatformat=jsonopt('FloatFormat','%.10g',varargin{:});
formatstr=['[' repmat([floatformat ','],1,size(mat,2)-1) [floatformat sprintf('],\n')]];

if(nargin>=2 && size(mat,1)>1 && jsonopt('ArrayIndent',1,varargin{:})==1)
    formatstr=[repmat(sprintf('\t'),1,level) formatstr];
end
txt=sprintf(formatstr,mat');
txt(end-1:end)=[];
if(islogical(mat) && jsonopt('ParseLogical',0,varargin{:})==1)
   txt=regexprep(txt,'1','true');
   txt=regexprep(txt,'0','false');
end
%txt=regexprep(mat2str(mat),'\s+',',');
%txt=regexprep(txt,';',sprintf('],\n['));
% if(nargin>=2 && size(mat,1)>1)
%     txt=regexprep(txt,'\[',[repmat(sprintf('\t'),1,level) '[']);
% end
txt=[pre txt post];
if(any(isinf(mat(:))))
    txt=regexprep(txt,'([-+]*)Inf',jsonopt('Inf','"$1_Inf_"',varargin{:}));
end
if(any(isnan(mat(:))))
    txt=regexprep(txt,'NaN',jsonopt('NaN','"_NaN_"',varargin{:}));
end

%%-------------------------------------------------------------------------
function newname=checkname(name,varargin)
isunpack=jsonopt('UnpackHex',1,varargin{:});
newname=name;
if(isempty(regexp(name,'0x([0-9a-fA-F]+)_','once')))
    return
end
if(isunpack)
    isoct=jsonopt('IsOctave',0,varargin{:});
    if(~isoct)
        newname=regexprep(name,'(^x|_){1}0x([0-9a-fA-F]+)_','${native2unicode(hex2dec($2))}');
    else
        pos=regexp(name,'(^x|_){1}0x([0-9a-fA-F]+)_','start');
        pend=regexp(name,'(^x|_){1}0x([0-9a-fA-F]+)_','end');
        if(isempty(pos)) return; end
        str0=name;
        pos0=[0 pend(:)' length(name)];
        newname='';
        for i=1:length(pos)
            newname=[newname str0(pos0(i)+1:pos(i)-1) char(hex2dec(str0(pos(i)+3:pend(i)-1)))];
        end
        if(pos(end)~=length(name))
            newname=[newname str0(pos0(end-1)+1:pos0(end))];
        end
    end
end


function wrapper_rgbd_to_ucm(imset, modelfile, ucm_dir, contours_cues_dir, C)
  sc = [2 1 0.5];
  imlist = getImageSet(imset);

  model = load(modelfile); %fullfile_ext(contours_model_dir, 'forest', modelname, 'mat'));
  model = model.model;

  exists_or_mkdir(fullfile(ucm_dir, 'multi'));
  exists_or_mkdir(fullfile(ucm_dir, 'scale_2.0'));
  exists_or_mkdir(fullfile(ucm_dir, 'scale_1.0'));
  exists_or_mkdir(fullfile(ucm_dir, 'edges_1.0'));
  exists_or_mkdir(fullfile(ucm_dir, 'scale_0.5'));
  exists_or_mkdir(fullfile(ucm_dir, 'multi-png'));

  for i = 1:length(imlist),
    id = imlist{i};
    I = getImage(id, 'images');
    D = getImage(id, 'depth');
    cacheFile = fullfile_ext(contours_cues_dir, id, 'mat');
    [E, Es, O] = detectEdge(I, D, [], C, model, sc, [], cacheFile);
    [ucm2 ucms] = contours_to_ucm(I, sc, Es, O);
    parsave(fullfile_ext(ucm_dir, 'multi', id, 'mat'), 'ucm2', ucm2);
    parsave(fullfile_ext(ucm_dir, 'edges_1.0', id, 'mat'), 'E', E{2}, 'Es', Es{2}, 'O', O{2});
    parsave(fullfile_ext(ucm_dir, 'scale_2.0', id, 'mat'), 'ucm2', ucms(:,:,1));
    parsave(fullfile_ext(ucm_dir, 'scale_1.0', id, 'mat'), 'ucm2', ucms(:,:,2));
    parsave(fullfile_ext(ucm_dir, 'scale_0.5', id, 'mat'), 'ucm2', ucms(:,:,3));
    imwrite(im2uint8(ucm2(3:2:end, 3:2:end)), fullfile_ext(ucm_dir, 'multi-png', id, 'png'));
  end
end


function display(sp)
% sympoly/display: Display a sympoly object, calls disp

name = inputname(1);
if ~isempty(name)
  display([name,' ='])
else
  display('ans =')
end

% is it a scalar or an array?
s=size(sp);
if any(s>1)
  % an array or vector
  disp(['Sympoly array has size = [',num2str(s),']'])
  disp(' ')

  sj = [0,ones(1,length(s)-1)];
  sj(1) = 0;
  for j=1:numel(sp)
    sj(1) = sj(1)+1;
    while any(sj>s)
      k = find(sj>s,1,'first');
      sj(k) = 1;
      sj(k+1) = sj(k+1)+1;
    end

    disp(['Sympoly array element [',num2str(sj),']'])
    disp(sp(j))
  end

else
  % A scalar

  disp(sp)

end


function e = end(X,k,n)
%END Last index of indexing expression for sptenmat.
%
%   The expression X(end,:) will call END(X,1,2) to determine
%   the value of the first index.
%
%   See also SPTENMAT, SPTENMAT/SUBSREF, END.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

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

if n > 2
  error('Subscript out of range.');
end
e = size(X,k);


%%% [4dmatrix] = convert_video_to_4d(struct)
%
function [V] = convert_video_to_4d(video)
  for i = 1:video.nrFramesTotal
    frame = video.frames(i).cdata;
    V(:,:,:,i) = frame;
  end
end


% eeg_bad_channels_script - process files with eeg_bad_channels

clear all;
close all;

path = '/floyd3/psdlw/ptsd-pet/original-scan/';
volt_ext = '.avg';
%var_ext = 'var.asc'; % Not used, assumed to be voltfile.var

elec_path = '/floyd3/psdlw/ptsd-pet/3dd_mean/corrected_elp/';
elec_ext = 'txt';

groups = {'c' 'p'};
subjects = {'01' '02' '03' '04' '05' '06' '07' '08' '09' '10'};
conditions = {'o' 'oac' 'ouc' 'out' 't' 'tac' 'tad' 'tat' 'tuc' 'tud' 'tut'};

for g=1:length(groups),

    for s=1:length(subjects),

        for c = 1:length(conditions),

            subj = strcat(char(groups(g)),char(subjects(s)));
            cond = char(conditions(c));

           [p] = eeg_toolbox_defaults('create');

            p.volt.path = path;
            p.volt.file = strcat(subj,cond,volt_ext);
            p.volt.type = 'Scan3x';

           [p] = eeg_open(p);

            p.elec.path = elec_path;
            p.elec.file = strcat(subj,'_124fit',elec_ext);
            p.elec.type = 'Cartesian';

           [p] = elec_open(p);

           [p] = eeg_bad_channels(p);
        end
    end
end


% emacs, this is -*-Matlab-*- mode

% The main script. Performes the self calibration
% The point coordinates are expected to be known
% see the directory FindingPoints
%
% $Author: svoboda $
% $Revision: 2.7 $
% $Id: gocal.m,v 2.7 2005/05/24 09:15:11 svoboda Exp $
% $State: Exp $

clear variables globals

Octave = exist('OCTAVE_VERSION', 'builtin') ~= 0;

% add necessary paths
addpath ('../CommonCfgAndIO')
addpath ('../RadialDistortions')
addpath ('./CoreFunctions')
addpath ('./OutputFunctions')
addpath ('./BlueCLocal')
addpath ('./LocalAlignments')
addpath ('../CalTechCal')
addpath ('../RansacM'); % ./Ransac for mex functions (it is significantly faster for noisy data)
% get the configuration
config = read_configuration();
disp('Multi-Camera Self-Calibration, Tomas Svoboda et al., 07/2003')
disp('************************************************************')
disp(sprintf('Experiment name: %s',expname))

%%%
% how many cameras to be filled
% if 0 then only points visible in all cameras will be used for selfcal
% the higher, the more points -> better against Gaussian noise
% however the higher probability of wrong filling
% Then the iterative search for outliers takes accordingly longer
% However, typically no more than 5-7 iterations are needed
% this number should correspond to the total number of the cameras
NUM_CAMS_FILL  = config.cal.NUM_CAMS_FILL;
%%%
% tolerance for inliers. The higher uncorrected radial distortion
% the higher value. For BlueC cameras set to 2 for the ViRoom
% plastic cams, set to 4 (see FINDINL)
INL_TOL = config.cal.INL_TOL;
%%%
% Use Bundle Adjustment to refine the final (after removing outliers) results
% It is often not needed at all
DO_BA = config.cal.DO_BA;

UNDO_RADIAL = config.cal.UNDO_RADIAL;		% undo radial distortion, parameters are expected to be available
BA_RADIAL = config.cal.BA_RADIAL;		% bundle adjustment
                                                % also finds
                                                % non-linear parameters
SAVE_STEPHI = 1;		% save calibration parameters in Stephi's Carve/BlueC compatible form
SAVE_PGUHA	= 1;		% save calib pars in Prithwijit's compatible form
USED_MULTIPROC = 0;		% was the multipropcessing used?
						% if yes then multiple IdMat.dat and points.dat have to be loaded
						% setting to 1 it forces to read the multiprocessor data against the
						% monoprocessor see the IM2POINTS, IM2PMULTIPROC.PL

%%%
% Data structures
% lin.* corrected values which obey linear model
% in.* inliers, detected by a chain application of Ransac

if strfind(expname,'oscar')
  % add a projector idx to the cameras
  % they are handled the same
  config.files.cams2use = [config.files.idxcams,config.files.idxproj];
end

selfcal.par2estimate = config.cal.nonlinpar;
selfcal.iterate = 1;
selfcal.count	= 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Load initial distortion parameters
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nl_params_all_cams = [];
if BA_RADIAL
  for i=1:CAMS,
    if UNDO_RADIAL
      [K,kc] = ...
	  readradfile(sprintf(config.files.rad,config.cal.cams2use(i)));
    else
      % no radial distortion
      K = [ 1 0 config.cal.Res(i,1)/2; ...
	    0 1 config.cal.Res(i,2)/2;
	0 0 1];
      kc = [0,0,0,0];
    end
    cam_pvec = rad2pvec(K,kc); % convert all NL params to row vector
    nl_params_all_cams(i) = cam_pvec; % append row vector to matrix
  end
end

if ~isempty(nl_params_all_cams)
  % This error is here to prevent a long run just to be confronted
  % with it in bundle_PX_proj. Remove this test when support is
  % implemented.
  error('Not implemented: radial distortion in bundle adjustment');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Main global cycle begins
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while selfcal.iterate && selfcal.count < config.cal.GLOBAL_ITER_MAX,
  % read the input data
  loaded = loaddata(config);
  linear = loaded;		% initalize the linear structure

  CAMS = size(config.cal.cams2use,2);
  FRAMES = size(loaded.IdMat,2);

  if CAMS < 3 || FRAMES < 20
	error('gocal: Not enough cameras or images -> Problem in loading data?')
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%% correct the required amount of cameras to be filled
  if CAMS-NUM_CAMS_FILL < 3
	NUM_CAMS_FILL = CAMS-3;
  end

  config.cal.Res= loaded.Res;
  config.cal.pp = reshape([loaded.Res./2,zeros(size(loaded.Res(:,1)))]',CAMS*3,1);
  config.cal.pp = [loaded.Res./2,zeros(size(loaded.Res(:,1)))];

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % See the README how to compute data
  % for undoing of the radial distortion
  if UNDO_RADIAL
	for i=1:CAMS,
	  [K,kc] = readradfile(sprintf(config.files.rad,config.cal.cams2use(i)));
	  xn	 = undoradial(loaded.Ws(i*3-2:i*3,:),K,[kc,0]);
	  linear.Ws(i*3-2:i*3,:) = xn;
	end
	linear.Ws = linear.Ws - repmat(reshape(config.cal.pp',CAMS*3,1), 1, FRAMES);
  elseif config.cal.UNDO_HEIKK,
	for i=1:CAMS,
	  heikkpar = load(sprintf(config.files.heikkrad,config.cal.cams2use(i)),'-ASCII');
	  xn = undoheikk(heikkpar(1:4),heikkpar(5:end),loaded.Ws(i*3-2:i*3-1,:)');
	  linear.Ws(i*3-2:i*3-1,:) = xn';
	end
	linear.Ws = linear.Ws - repmat(reshape(config.cal.pp',CAMS*3,1), 1, FRAMES);
  else
	linear.Ws = loaded.Ws - repmat(reshape(config.cal.pp',CAMS*3,1), 1, FRAMES);
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Detection of outliers
  % RANSAC is pairwise applied
  disp(' ')
  disp(sprintf('********** After %d iteration *******************************************',selfcal.count))
  %  disp('****************************************************************************')
  disp(sprintf('RANSAC validation step running with tolerance threshold: %2.2f ...',INL_TOL));

  inliers.IdMat = findinl(linear.Ws,linear.IdMat,INL_TOL);

  addpath ('./MartinecPajdla');
  setpaths;		% set paths for M&P algorithms

  % remove zero-columns or just 1 point columns
  % create packed represenatation
  % it is still a bit tricky, the number of the minimum number of cameras
  % are specified here, may be some automatic method would be useful
  packed.idx	  = find(sum(inliers.IdMat)>=size(inliers.IdMat,1)-NUM_CAMS_FILL);
  packed.IdMat  = inliers.IdMat(:,packed.idx);
  packed.Ws	  = linear.Ws(:,packed.idx);

  if size(packed.Ws,2)<20
	error(sprintf('Only %d points survived RANSAC validation and packing: probably not enough points for reliable selfcalibration',size(packed.Ws,2)));
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%% fill cam(i) structures
  for i=1:CAMS,
	cam(i).camId  = config.cal.cams2use(i);
	cam(i).idlin  = find(linear.IdMat(i,:)); % loaded structure
	cam(i).idin	  = find(inliers.IdMat(i,:));	% survived initial pairwise validation
	cam(i).xdist  = loaded.Ws(3*i-2:3*i,cam(i).idlin);  % original distorted coordinates
	cam(i).xgt	  = linear.Ws(3*i-2:3*i,cam(i).idlin);
	cam(i).xgtin  = linear.Ws(3*i-2:3*i,cam(i).idin);
	% convert the ground truth coordinates by using the known principal point
	cam(i).xgt    = cam(i).xgt + repmat(config.cal.pp(i,:)', 1, size(cam(i).xgt,2));
	cam(i).xgtin  = cam(i).xgtin + repmat(config.cal.pp(i,:)', 1, size(cam(i).xgtin,2));
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%% options for the Martinec-Pajdla filling procedure
  options.verbose = 0;
%  options.strategy = 1; % force a particular central camera
  options.no_BA = 1;
  options.iter = 5;
  options.detection_accuracy = 2;
  options.consistent_number  = 9;
  options.consistent_number_min = 6;
  options.samples = 1000; %1000; %10000;
  options.sequence = 0;
  options.create_nullspace.trial_coef = 10; %20;

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%% start of the *compute and remove outliers cycle*
  outliers = 1;
  inliers.idx = packed.idx;
  inliers.reprerr = [9e9];
  while outliers
	disp(sprintf('%d points/frames have survived validations so far',size(inliers.idx,2)))
	disp('Filling of missing points is running ...')
	[P,X, u1,u2, info] = fill_mm_bundle(linear.Ws(:,inliers.idx),config.cal.pp(:,1:2)',nl_params_all_cams,options);
	%
	Rmat = P*X;
	Lambda = Rmat(3:3:end,:);
	%
	[Pe,Xe,Ce,Re] = euclidize(Rmat,Lambda,P,X,config);
	disp('************************************************************')
	%
	% compute reprojection errors
	cam = reprerror(cam,Pe,Xe,FRAMES,inliers);
	%
	% detect outliers in cameras
	[outliers,inliers] = findoutl(cam,inliers,INL_TOL,NUM_CAMS_FILL);
	%
	disp(sprintf('Number of detected outliers: %3d',outliers))
	disp('About cameras (Id, 2D reprojection error, #inliers):')
	dispcamstats(cam,inliers);
	% [[cam(:).camId]',[cam(:).std2Derr]',[cam(:).mean2Derr]', sum(inliers.IdMat')']
	disp('***************************************************************')
	%
	% do BA after removing very bad outliers or if the process starts to diverge
	% and only if required config.cal.START_BA
	inliers.reprerr = [inliers.reprerr, mean([cam(:).mean2Derr])];
	if inliers.reprerr(end)<5*INL_TOL || inliers.reprerr(end-1)<inliers.reprerr(end),
	   try, options.no_BA = ~config.cal.START_BA; catch, options.no_BA = 1; end % 1 0
	end
  end
  %%% end of the cycle
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  %%% Do the final refinement through the BA if required and if not
  %%% performed during the iteration steps
  if (DO_BA && ~config.cal.START_BA) || (DO_BA && config.cal.START_BA && size(inliers.reprerr,2)<3)
	disp('**************************************************************')
	disp('Refinement by using Bundle Adjustment')
	options.no_BA = 0;
	[P,X, u1,u2, info] = fill_mm_bundle(linear.Ws(:,inliers.idx),config.cal.pp(:,1:2)',nl_params_all_cams,options);
	Rmat = P*X;
	Lambda = Rmat(3:3:end,:);
	[in.Pe,in.Xe,in.Ce,in.Re] = euclidize(Rmat,Lambda,P,X,config);
	cam = reprerror(cam,in.Pe,in.Xe,FRAMES,inliers);
	% [outliers,inliers] = findoutl(cam,inliers,INL_TOL,NUM_CAMS_FILL);
  else
	in.Pe = Pe; in.Xe = Xe; in.Ce = Ce; in.Re = Re;
  end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  if ~Octave,
	% plot reconstructed cameras and points
	drawscene(Xe,Ce,Re,3,'cloud','reconstructed points/camera setup');
	drawscene(in.Xe,in.Ce,in.Re,4,'cloud','reconstructed points/camera setup only inliers are used',config.cal.cams2use);
	% plot measured and reprojected 2D points
	for i=1:CAMS
	  in.xe		= in.Pe(((3*i)-2):(3*i),:)*in.Xe;
	  cam(i).inxe	= in.xe./repmat(in.xe(3,:),3,1);
	  figure(i+10)
	  clf
	  plot(cam(i).xdist(1,:),cam(i).xdist(2,:),'go');
	  hold on, grid on
	  plot(cam(i).xgt(1,:),cam(i).xgt(2,:),'ro');
	  plot(cam(i).xgtin(1,:),cam(i).xgtin(2,:),'bo');
	  % plot(cam(i).xe(1,:),cam(i).xe(2,:),'r+')
	  plot(cam(i).inxe(1,:),cam(i).inxe(2,:),'k+','MarkerSize',7)
	  %plot(xe(1,:),xe(2,:),'r+','linewidth',3,'MarkerSize',10)
	  title(sprintf('o: measured (green=distorted, blue=inlier, red=discarded). +: reprojected. (camera: %d)',config.cal.cams2use(i)));
	  for j=1:size(cam(i).visandrec,2); % plot the reprojection errors
		line([cam(i).xgt(1,cam(i).visandrec(j)), ...
		      cam(i).inxe(1,cam(i).recandvis(j))], ...
		    [cam(i).xgt(2,cam(i).visandrec(j)), ...
		      cam(i).inxe(2,cam(i).recandvis(j))],'Color','g');
	  end
	  % draw the image border
	  line([0 0 0 2*config.cal.pp(i,1) 2*config.cal.pp(i,1) 2*config.cal.pp(i,1) 2*config.cal.pp(i,1) 0],[0 2*config.cal.pp(i,2) 2*config.cal.pp(i,2) 2*config.cal.pp(i,2) 2*config.cal.pp(i,2) 0 0 0],'Color','k','LineWidth',2,'LineStyle','--')
	  axis('equal')
	  drawnow
	  eval(['print -depsc ', config.paths.data, sprintf('%s%d.reprojection.eps',config.files.basename,cam(i).camId)])
	end
  end

  %%%
  % SAVE camera matrices
  P = in.Pe;
  X = in.Xe;
  R = in.Re;
  C = in.Ce;
  if Octave
    % all Octave data in ASCII format
    save(config.files.Pmats,'P');
    save(config.files.Xe,'X');
    save(config.files.Re,'R');
    save(config.files.Ce,'C');
  else
    save(config.files.Pmats,'P','-ASCII');
    save(config.files.Xe,'X','-ASCII');
    save(config.files.Re,'R','-ASCII');
    save(config.files.Ce,'C','-ASCII');
  end

  % save normal data
  if SAVE_STEPHI || SAVE_PGUHA
	 [in.Cst,in.Rot] = savecalpar(in.Pe,config);
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % local routines for the BlueC installations
  % The main functionality of these functions that
  % they align the coordinate frame from the selfcalibration
  % with the pre-defined world frame
  % it is assumed the necessary informations are avialable
  if strfind(expname,'BlueCRZ')
	  [align] = bluecrz(in,config);
  end
  if strfind(expname,'Hoengg')
	  [align] = bluechoengg(in,config);
  end
  if strfind(expname,'Erlangen')
	  [align] = erlangen(in,config);
	  % [align] = planarmove(in,config);
  end
  if strfind(expname,'G9')
	  [align] = g9(in,config);
  end
  if strfind(expname,'flydra')
	  [align] = flydra(in,config);
  end
  if strfind(expname,'mamarama')
	  [align] = mamarama(in,config);
  end
  if strfind(expname,'humdra')
	  [align] = humdra(in,config);
  end

  if config.cal.ALIGN_EXISTING
      align_existing_camera_centers(in,config);
  end

  % planar alignement if knowledge available
  % [align,cam] = planarmove(in,cam,config);
  % try, [align,cam] = planarcams(in,cam,config,config.cal.planarcams); end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Evaluate reprojection error
  %%%
  cam = evalreprerror(cam,config);

  %%%%
  % Save the 2D-3D correpondences for further processing
  for i=1:CAMS,
	xe = cam(i).xdist(1:3,cam(i).visandrec);	% save the original distorted coordinates
 	% save the reconstructed points (aligned if available)
	try, Xe = align.X(1:4,cam(i).recandvis); catch, Xe = in.Xe(1:4,cam(i).recandvis); end;
	% Xe = in.Xe(1:4,cam(i).recandvis);
	corresp = [Xe',xe'];
	if Octave
	  % all Octave data in ASCII format
          save(sprintf(config.files.points4cal,config.cal.cams2use(i)),'corresp');
	else
          save(sprintf(config.files.points4cal,config.cal.cams2use(i)),'corresp','-ASCII');
	end
  end

%%%
% TO-DO:
% - find a suitable end condition for the global iteration.
%   This threshold may very depend on local conditions
%
% - how to check meaningful number of iterations
%   typically only few iterations are needed
%
% - The precision of the non-linear estimation should be somehow taken into account

  selfcal.count = selfcal.count+1;

  if max([cam.mean2Derr])>config.cal.GLOBAL_ITER_THR && config.cal.DO_GLOBAL_ITER && selfcal.count < config.cal.GLOBAL_ITER_MAX
    % if the maximal reprojection error is still bigger
    % than acceptable estimate radial distortion and
    % iterate further
    cd ../CalTechCal
    selfcalib = goradf(config,selfcal.par2estimate,INL_TOL);
    cd ../MultiCamSelfCal
    selfcal.iterate = 1;
    UNDO_RADIAL = 1;
    if ~selfcalib.goradproblem;
      % if all non-linear parameters estimated reliable
      % we can reduce the tolerance threshold
      INL_TOL = max([(2/3)*INL_TOL,config.cal.GLOBAL_ITER_THR]);
      % add the second radial distortion parameter
      if config.cal.NL_UPDATE(4), selfcal.par2estimate(4) = 1; end
      % estimate also the principal point
      if selfcal.count > 1 && config.cal.NL_UPDATE(2), selfcal.par2estimate(2) = 1; end
      % estimate also the tangential distortion
      if selfcal.count > 3 && all(config.cal.NL_UPDATE(5:6)), selfcal.par2estimate(5:6) = 1; end
    else
      INL_TOL = min([3/2*INL_TOL,config.cal.INL_TOL]);
    end
  else
    % ends the iteration
    % the last computed parameters will be taken as valid
    selfcal.iterate = 0;
  end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% End of the main global cycle
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function idxCst = addCst(obj, cst2add)
% ADDCST Add constant(s) to problem

noCst2add = numel(cst2add); % no. of constants to add

% Add constants with 'direct indexing' (faster than other methods)
% Indices
if isempty(obj.cst)
    idxCst2add = 1:noCst2add;
else
    noCst = numel(obj.cst); % actual no. of constants
    idxCst2add = noCst+1:noCst+noCst2add;
end

% Add
obj.cst(idxCst2add,1) = cst2add;

% Indices of constants
idxCst = uint32(idxCst2add)';

end


% MOST
%   Version 1.2         13-Dec-2022
%
%   The MATPOWER Optimal Scheduling Tool (MOST) is framework for solving
%   generalized steady-state electric power scheduling problems.
%
%   MOST can be used to solve problems as simple as a deterministic,
%   single period economic dispatch problem with no transmission
%   constraints or as complex as a stochastic, security-constrained,
%   combined unit-commitment and multiperiod optimal power flow problem
%   with locational contingency and load-following reserves, ramping
%   costs and constraints, deferrable demands, lossy storage resources
%   and uncertain renewable generation.
%
%   While the problem formulation is general and incorporates a full
%   nonlinear AC network model, the current implementation is limited
%   to DC power flow modeling of the network. Some work has been done
%   on an AC implementation, but it is not yet ready for release.
%
%   The primary developers of MOST are Carlos E. Murillo-Sanchez and
%   Ray D. Zimmerman, with significant contributions from
%   Daniel Munoz-Alvarez and Alberto J. Lamadrid. It is built on top of
%   MATPOWER, a package of MATLAB/Octave M-files for solving power flow
%   and optimal power flow problems.
%
%   The latest version of MOST can be found on GitHub at:
%
%       https://github.com/MATPOWER/most
%
%   MOST is covered by the 3-clause BSD License (see LICENSE for details).

%   MOST
%   Copyright (c) 2010-2022, Power Systems Engineering Research Center (PSERC)
%   by Carlos E. Murillo-Sanchez, PSERC Cornell & Universidad Nacional de Colombia
%   and Ray Zimmerman, PSERC Cornell
%
%   This file is part of MOST.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/most for more info.


clear;

%%
% Find the dataset location
if exist('D:/Datasets/HeadPose', 'file')
    database_root = 'D:/Datasets/HeadPose/';
elseif(exist([getenv('USERPROFILE') '/Dropbox/AAM/test data/'], 'file'))
    database_root = [getenv('USERPROFILE') '/Dropbox/AAM/test data/'];
elseif(exist(['F:/Dropbox/Dropbox/AAM/test data/'], 'file'))
    database_root = 'F:/Dropbox/Dropbox/AAM/test data/';
elseif(exist(['F:/Dropbox/AAM/test data/'], 'file'))
    database_root = 'F:/Dropbox/AAM/test data/';
elseif(exist('/media/tadas/5E08AE0D08ADE3ED/Dropbox/AAM/test data', 'file'))
    database_root = '/media/tadas/5E08AE0D08ADE3ED/Dropbox/AAM/test data';
end

buDir = [database_root, '/bu/uniform-light/'];

% The fast and accurate clnf
%%
[resFolderBU_OF] = run_bu_experiment(buDir, false, 'model', 'model/main_clnf_general.txt');
[bu_error_OF, pred_hp_bu, gt_hp_bu, all_errors_bu_OF, rels_bu] = calcBUerror(resFolderBU_OF, buDir);

%%
% Run the Biwi test
biwi_dir = '/biwi pose/';

[res_folder_biwi_OF] = run_biwi_experiment(database_root, biwi_dir, false, false, 'model', 'model/main_clnf_general.txt');
% Calculate the resulting errors
[biwi_error_OF, pred_hp_biwi, gt_hp_biwi, ~, all_errors_biwi_OF, rels_biwi] = calcBiwiError(res_folder_biwi_OF, [database_root biwi_dir]);

%% Run the ICT test
ict_dir = ['/ict/'];

% Intensity
[res_folder_ict_OF] = run_ict_experiment(database_root, ict_dir, false, false, 'model', 'model/main_clnf_general.txt');
% Calculate the resulting errors
[ict_error_OF, pred_hp_ict, gt_hp_ict, ~, all_errors_ict_OF, rel_ict] = calcIctError(res_folder_ict_OF, [database_root ict_dir]);

%% Save the results
filename = 'results/Pose_OF_CLNF';
save(filename);

% Also save them in a reasonable .txt format for easy comparison
f = fopen('results/Pose_OF_CLNF.txt', 'w');
fprintf(f, 'Dataset and model,        pitch,  yaw,  roll,  mean,  median\n');
fprintf(f, 'biwi error:  %.3f,   %.3f, %.3f,  %.3f,  %.3f\n', biwi_error_OF, mean(all_errors_biwi_OF(:)), median(all_errors_biwi_OF(:)));
fprintf(f, 'bu error:    %.3f,   %.3f, %.3f,  %.3f,  %.3f\n', bu_error_OF, mean(all_errors_bu_OF(:)), median(all_errors_bu_OF(:)));
fprintf(f, 'ict error:   %.3f,   %.3f, %.3f,  %.3f,  %.3f\n', ict_error_OF, mean(all_errors_ict_OF(:)), median(all_errors_ict_OF(:)));

fclose(f);
clear 'f'


function imageHandle = prtPlotUtilPlotGriddedEvaledClassifier(DS, linGrid, gridSize, cMap, prtActor)
% Internal function
% xxx Need Help xxx

% Copyright (c) 2013 New Folder Consulting
%
% Permission is hereby granted, free of charge, to any person obtaining a
% copy of this software and associated documentation files (the
% "Software"), to deal in the Software without restriction, including
% without limitation the rights to use, copy, modify, merge, publish,
% distribute, sublicense, and/or sell copies of the Software, and to permit
% persons to whom the Software is furnished to do so, subject to the
% following conditions:
%
% The above copyright notice and this permission notice shall be included
% in all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
% OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
% MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN
% NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
% DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
% OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
% USE OR OTHER DEALINGS IN THE SOFTWARE.

% Check to see if we are plotting a decision
if ~isempty(prtActor.internalDecider)
    % When using a decider DS contains integers and the linear spacing
    % applied using the colormap may not be correct (depending on the
    % specific integer labels). Therefore we have to change DS to be
    % consecutive integers corresponding to the integer labels that were
    % used.

    [dontNeed, DS] = ismember(DS,prtActor.internalDecider.classList); %#ok<ASGLU>

%     if ~isempty(prtActor.internalDecider.dataSetSummary.uniqueClasses)
%         [dontNeed, DS] = ismember(DS,prtActor.internalDecider.dataSetSummary.uniqueClasses); %#ok<ASGLU>
%     else
%         % Have something unsupervised like a clusterer
%         % Just roll with it.
%     end
end
nDims = size(linGrid,2);

switch nDims
    case 1
        DS = repmat(DS,1,max(prtActor.dataSetSummary.uniqueClasses));
        imageHandle = imagesc(linGrid,[min(prtActor.dataSetSummary.uniqueClasses),max(prtActor.dataSetSummary.uniqueClasses)]',DS');
        set(gca,'YTickLabel',[])
        colormap(cMap)
    case 2
        % find decision boundary, center colormap around it; THIS CODE IS
        % BROKEN FOR MARY CLASSIFIERS
        % 		results = prtActor.run(prtActor.dataSet);
        % 		decider = prtDecisionBinaryMinPe; % can't use prtActor.internalDecider
        % 		decider = decider.train(results);
        % 		threshold = decider.threshold;
        % 		crange = threshold + min(max(results.X)-threshold,threshold-min(results.X))*[-1,1];

        xx = reshape(linGrid(:,1),gridSize);
        yy = reshape(linGrid(:,2),gridSize);
        % 		if all(isfinite(crange))
        imageHandle = imagesc(xx(1,:),yy(:,1),reshape(DS,gridSize));
        % 		else
        % 			imageHandle = imagesc(xx(1,:),yy(:,1),reshape(DS,gridSize));
        % 		end
        colormap(cMap)
    case 3
        xx = reshape(linGrid(:,1),gridSize);
        yy = reshape(linGrid(:,2),gridSize);
        zz = reshape(linGrid(:,3),gridSize);
        imageHandle = slice(xx,yy,zz,reshape(DS,gridSize),max(xx(:)),max(yy(:)),[min(zz(:)),mean(zz(:))]);
        view(3)
        colormap(cMap)
        imageHandle = imageHandle(1); % Sorry we need to throw the others away
end
axis tight;
axis xy;

end


classdef TestFisheyeEstimateNewCameraMatrixForUndistortRectify
    %TestFisheyeEstimateNewCameraMatrixForUndistortRectify

    methods (Static)
        function test_1
            %TODO
        end

        function test_error_argnum
            try
                cv.fisheyeEstimateNewCameraMatrixForUndistortRectify();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end