function x = buncompress( xR, x, sx )
error( nargchk( 2, 3, nargin ) );

if ~isa( xR, 'double' ) && ~isa( xR, 'sparse' ),
    error( 'First argument must be a structure matrix.' );
elseif size( x.basis_, 2 ) ~= size( xR, 1 ),
    error( 'Structure matrix incompatible with vector.' );
elseif nargin < 3 || isempty( sx ),
    sx = size( xR, 2 );
elseif ~cvx_check_dimlist( sx, false ),
    error( 'Third argument must be a size matrix.' );
elseif prod( sx ) ~= isempty( xR ) * prod( x.size_ ) + ~isempty( xR ) * size( xR, 2 ),
    error( 'Incompatible size matrix.' );
end

if isempty( xR ),
    x = cvx( sx, x.basis_ );
else
    x = cvx( sx, x.basis_ * xR );
end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [ir,x0] = ir_localwfs(X,head_orientation,xs,src,sofa,conf)
%IR_LOCALWFS binaural simulation of local WFS
%
%   Usage: [ir,x0] = ir_localwfs(X,head_orientation,xs,src,sofa,conf)
%
%   Input parameters:
%       X                - listener position / m
%       head_orientation - orientation of the listener with [phi theta] /
%                          (rad, rad)
%       xs               - virtual source position / m
%       src              - source type: 'pw' -plane wave
%                                       'ps' - point source
%       sofa             - impulse response data set for the secondary sources
%       conf             - configuration struct (see SFS_config)
%
%   Output parameters:
%       ir               - impulse responses for the desired WFS array
%                          (nx2 matrix)
%       x0               - secondary sources / m
%
%   IR_LOCALWFS(X,head_orientation,xs,src,sofa,conf) calculates a binaural room
%   impulse response for a virtual source at xs for a virtual local WFS array
%   and a listener located at X.
%
%   See also: ssr_brs_wfs, ir_point_source, auralize_ir

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% 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.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 6;
nargmax = 6;
narginchk(nargmin,nargmax);
if conf.debug
    isargposition(X);
    isargxs(xs);
    isargvector(head_orientation);
    isargchar(src);
    isargstruct(conf);
end

%% ===== Computation =====================================================
% Get secondary sources
x0 = secondary_source_positions(conf);
% Get driving signals
[d, x0] = driving_function_imp_localwfs(x0,xs,src,conf);
% Generate the impulse response for WFS
ir = ir_generic(X,head_orientation,x0,d,sofa,conf);


function [retRes,retAlgo] = training(retAlgo,dat,lossType)

%  Does cross validation and returns the results as a data object
%  plus the updated group (after training models).

  rndState = rand( 'state');
  if retAlgo.algorithm.verbosity
    disp(['training ' get_name(retAlgo) '.... '])
  end
  if nargin==1
    dat=data; % <-- in case data is not specified
  end;

  if nargin<3
    lossType=[];  % <-- in case loss is not specified
  end

  untrained = retAlgo.child{1};
  if isa(untrained,'cell')
    untrained = group(untrained); % <-- convert retAlgo.child{1} to group
  end;

  [num,vecDim,outDim]=get_dim(dat);
  orig_name=get_name(dat);
  retRes = {};

  retAlgo.trialbytrial=[];
  for loop=1:retAlgo.repeats % <-- do cross validation vecDim times

    rand('seed',loop);
    if retAlgo.balanced     % <-- balance the number of positives/negatives labels in folds
      labels=get_y(dat);
      fin1=find(labels==1);
      fin2=find(labels==-1);

      pe1=randperm(sum(labels==1));
      pe1=fin1(pe1);

      pe2=randperm(sum(labels==-1));
      pe2=fin2(pe2);
      perm=[pe1 ;pe2];
    else
      perm=randperm(num);
    end

    trialbytrial = [];
    for i=1:retAlgo.folds
      if retAlgo.train_on_fold==0
        tst=perm(i:retAlgo.folds:num);
        trn=setdiff(1:num,tst);
      else  % <--- take fraction as training data
        trn=perm(i:retAlgo.folds:num);
        tst=setdiff(1:num,trn);
      end

      if retAlgo.store_all
        indx=i+(loop-1)*(retAlgo.folds);
      else
        indx=1;
      end;

      str=[' fold=' num2str(i+(loop-1)*(retAlgo.folds))];
      if retAlgo.output_train_error==0  % <--- testing on left out fold
        dat=set_name(dat,[ orig_name ' -> cv' str]);
      else
        dat=set_name(dat,[ orig_name ' -> cv' str ' output_train_error=1']);
      end

      if isfield(struct(untrained), 'test_on_the_fly')
        if isstruct(untrained.test_on_the_fly)
          %% untrained.test_on_the_fly.data = set_name(get(dat, tst), [orig_name ' -> cv' str]);
          % line above fails because subsasgn has suddenly stopped working recursively
          % work around it as follows:
          sss = untrained.test_on_the_fly;
          sss.data = set_name(get(dat, tst), [orig_name ' -> cv' str]);
          untrained.test_on_the_fly = sss;
          % some algorithms train another algorithm multiple times in the course of
          % their own training: for example, RFE trains a classifier once per
          % feature elimination step. It is often handy to evaluate cv-error at each
          % stage: previously this involved either re-training the individual classifiers
          % afterwards (time-consuming), or storing a whole series of trained
          % classifiers (memory-consuming). This solution gives the wrapper
          % a sneak preview of the test data for this fold, so that cv error can be
          % evaluated as it goes. Of course, the sneak preview should NOT be used
          % for training!
        end
      end
      if retAlgo.algorithm.verbosity
        str = sprintf('training cv fold %d of %d', i, retAlgo.folds);
        if retAlgo.repeats > 1, str = sprintf('%s (repeat %d of %d)', str, loop, retAlgo.repeats); end
        disp(str)
      end

      if retAlgo.output_train_error
        [res, retAlgo.child{indx}] = train(untrained, get(dat, trn), lossType);
      else
        [res, retAlgo.child{indx}] = traintest(untrained, dat, trn, tst, lossType);
      end
      if retAlgo.store_trialbytrial
        trialbytrial = [trialbytrial; tst(:) repmat(i, length(tst), 1) res.Y res.X];
      end
      %	[res retAlgo.child{indx}]=train(untrained,get(dat,trn));
      %    if retAlgo.output_train_error==0  % <--- test on left out fold
      %      [res]=test(retAlgo.child{indx},get(dat,tst));
      %    end
      %    if ~isempty(lossType)        % <--- calculate loss on results
      %      res=loss(res,lossType,[],1);
      %    end

      if ~isa(res, 'cell'), res = {res}; end
      retRes = [retRes res(:)'];

    end
    if retAlgo.store_trialbytrial
      retAlgo.trialbytrial = cat(3, retAlgo.trialbytrial, sortrows(trialbytrial));
    end
  end

  retRes=group(retRes);

  rand( 'state', rndState);  % restore the random generator


function a = repmat(a,m,n)
%REPMAT       Implements  repmat(a)  for Hessians
%
%Functionality as in Matlab.
%

% written  05/03/09     S.M. Rump
%

%Code adapted from Matlab R2008a
%

if nargin < 2
    error('repmat requires at least 2 inputs')
end

if nargin == 2
    if prod(size(m))==1
        siz = [m m];
    else
        siz = m;
    end
else
    siz = [m n];
end

if prod(size(a))==1
    %VVVV  a = a(ones(siz));
    s.type = '()'; s.subs = {ones(siz)}; a = subsref(a,s);
    %AAAA  Matlab bug fix
elseif ndims(a) == 2 & length(siz) == 2
    [m,n] = size(a);

    if (m == 1 & siz(2) == 1)
        %VVVV  a = a(ones(siz(1), 1), :);
        s.type = '()'; s.subs = {ones(siz(1), 1), ':'}; a = subsref(a,s);
        %AAAA  Matlab bug fix
    elseif (n == 1 & siz(1) == 1)
        %VVVV  a = a(:, ones(siz(2), 1));
        s.type = '()'; s.subs = {':', ones(siz(2), 1)}; a = subsref(a,s);
        %AAAA  Matlab bug fix
    else
        mind = (1:m)';
        nind = (1:n)';
        mind = mind(:,ones(1,siz(1)));
        nind = nind(:,ones(1,siz(2)));
        %VVVV  a = a(mind,nind);
        s.type = '()'; s.subs = {mind,nind}; a = subsref(a,s);
        %AAAA  Matlab bug fix
    end
else
    asiz = size(a);
    asiz = [asiz ones(1,length(siz)-length(asiz))];
    siz = [siz ones(1,length(asiz)-length(siz))];
    for i=length(asiz):-1:1
        ind = (1:asiz(i))';
        subs{i} = ind(:,ones(1,siz(i)));
    end
    %VVVV  a = a(subs{:});
    s.type = '()'; s.subs = {subs{:}}; a = subsref(a,s);
    %AAAA  Matlab bug fix
end


function out = path_manager_dubins(in,p_drone,start_of_simulation)

% PATH_MANAGER_DUBINS - follow Dubins paths between waypoint configurations
%
% Inputs:
%   nb_wpts      - number of waypoint configurations
%   wpt_list     - an array of dimension 5 by p_drone.size_wpt_array.
%                - the first nb_wpts rows define waypoint
%                  configurations
%                - format for each waypoint configuration:
%                  [wn, we, wd, chi_d, Va_d]
%                  where the (wn, we, wd) is the NED position of the
%                  waypoint, chi_d is the desired course at the waypoint,
%                  and Va_d is the desired airspeed along the path.
%
% Outputs:
%   flag   - if flag==1, follow waypoint path
%            if flag==2, follow orbit
%   Va_d   - desired airspeed
%   r      - inertial position of start of waypoint path
%   q      - unit vector that defines inertial direction of waypoint path
%   c      - center of orbit
%   rho    - radius of orbit
%   lambda - direction of orbit (+1 for CW, -1 for CCW)
%

  NN = 0;
  nb_wpts = in(1+NN);
  wpt_list = reshape(in(2+NN:5*p_drone.size_waypoint_array+1+NN),5,p_drone.size_waypoint_array);
  NN = NN + 1 + 5*p_drone.size_waypoint_array;
  pn        = in(1+NN);
  pe        = in(2+NN);
  h         = in(3+NN);
  % Va      = in(4+NN);
  % alpha   = in(5+NN);
  % beta    = in(6+NN);
  % phi     = in(7+NN);
  % theta   = in(8+NN);
  chi     = in(9+NN);
  % p       = in(10+NN);
  % q       = in(11+NN);
  % r       = in(12+NN);
  % Vg      = in(13+NN);
  % wn      = in(14+NN);
  % we      = in(15+NN);
  % psi     = in(16+NN);
  state     = in(1+NN:16+NN);
  NN = NN + 16;
  t         = in(1+NN);

  p = [pn; pe; -h];
  R = p_drone.radius;                 % radius of circular orbits

  persistent wpt_list_prev      % stored copy of old waypoints
  persistent idx_a              % waypoint pointer
  persistent flag_transition    % state of transition state machine
  persistent dubinspath
  persistent flag_need_new_wpts % flag that request new waypoints from path planner
  persistent flag_first_time_in_state

  if start_of_simulation
      wpt_list_prev = zeros(5,p_drone.size_wpt_array);
      flag_need_new_wpts = 0;
      flag_transition = 0;
      flag_first_time_in_state = 1;
  end

  % If waypoints have changed, update the waypoint pointer and plan new
  % Dubin's path
  if min(min(wpt_list==wpt_list_prev))==0
      wpt_list_prev = wpt_list;
      if start_of_simulation
        flag_transition = 0;
      end
      idx_a = 1;
      idx_b = 2;
      start_node = [wpt_list(1:4,idx_a)', 0, 0];
      end_node   = [wpt_list(1:4,idx_b)', 0, 0];
      dubinspath = compute_dubins_param(start_node, end_node, R);
      flag_need_new_wpts = 0;
      flag_first_time_in_state = 1;
  end

  % Define transition state machine
  switch flag_transition
      case 0             % beginning of simulation
          flag   = 1;
          Va_d   = p_drone.Va0;
          r      = dubinspath.ps;
          q      = dubinspath.q1;
          c      = dubinspath.cs;       % not used for waypoint path
          rho    = dubinspath.R;        % not used for waypoint path
          lambda = dubinspath.lams;     % not used for waypoint path
          if flag_first_time_in_state
              flag_first_time_in_state =0;
          end
          flag_transition = 1;

      case 1	% follow first orbit on Dubins path until intersect H1
          flag   = 2;
          Va_d   = p_drone.Va0;
          r      = dubinspath.w1;
          q      = dubinspath.q1;
          c      = dubinspath.cs;       % not used for waypoint path
          rho    = dubinspath.R;        % not used for waypoint path
          lambda = dubinspath.lams;     % not used for waypoint path

          if ((p-dubinspath.w1)'*dubinspath.q1 >= 0)&&(flag_first_time_in_state==1) % start in H1
              flag_transition = 2;
              flag_first_time_in_state = 1;
           elseif (p-dubinspath.w1)'*dubinspath.q1 >= 0 % entering H1
              flag_transition = 3;
              flag_first_time_in_state = 1;
          else
              flag_first_time_in_state = 0;
          end

      case 2    % follow first orbit on Dubins path until on right side of H1
          flag   = 2;
          Va_d   = p_drone.Va0;
          r      = dubinspath.w1;
          q      = dubinspath.q1;
          c      = dubinspath.cs;       % not used for waypoint path
          rho    = dubinspath.R;        % not used for waypoint path
          lambda = dubinspath.lams;     % not used for waypoint path

          if (p-dubinspath.w1)'*dubinspath.q1 < 0 % get to right side H1
              flag_transition = 1;
              flag_first_time_in_state = 1;
          else
              flag_first_time_in_state = 0;
          end

      case 3    % follow straight line on Dubins path until intersect H2
          flag   = 1;
          Va_d   = p_drone.Va0;
          r      = dubinspath.w1;
          q      = dubinspath.q1;
          c      = dubinspath.cs;       % not used for waypoint path
          rho    = dubinspath.R;        % not used for waypoint path
          lambda = dubinspath.lams;     % not used for waypoint path
          flag_first_time_in_state = 0;

          if (p-dubinspath.w2)'*dubinspath.q1 >= 0 % entering H2
              flag_transition = 4;
              flag_first_time_in_state = 1;
          end

      case 4    % follow second orbit on Dubins path until intersect H3
          flag   = 2;
          Va_d   = p_drone.Va0;
          r      = dubinspath.w1;
          q      = dubinspath.q1;
          c      = dubinspath.ce;       % not used for waypoint path
          rho    = dubinspath.R;        % not used for waypoint path
          lambda = dubinspath.lame;     % not used for waypoint path
          flag_first_time_in_state = 0;

          if ((p-dubinspath.w3)'*dubinspath.q3 >= 0)&&(flag_first_time_in_state==1) % start in H3
              flag_transition = 5;
              flag_first_time_in_state=1;
          elseif (p-dubinspath.w3)'*dubinspath.q3 >= 0 % entering H3
              % increase the waypoint pointer
              if idx_a==nb_wpts-1
                  flag_need_new_wpts = 1;
                  idx_b = idx_a+1;
              else
                  idx_a = idx_a+1;
                  idx_b = idx_a+1;
                  flag_transition = 1;
                  flag_first_time_in_state = 1;
              end
              % Plan new Dubin's path to next waypoint configuration
              start_node = [wpt_list(1:4,idx_a)', 0, 0];
              end_node   = [wpt_list(1:4,idx_b)', 0, 0];
              dubinspath = compute_dubins_param(start_node, end_node, R);
          else
              flag_first_time_in_state = 0;
          end

      case 5    % follow first orbit on Dubins path until on right side of H3
          flag   = 2;
          Va_d   = p_drone.Va0;
          r      = dubinspath.w1;
          q      = dubinspath.q1;
          c      = dubinspath.cs;       % not used for waypoint path
          rho    = dubinspath.R;        % not used for waypoint path
          lambda = dubinspath.lams;     % not used for waypoint path
          flag_first_time_in_state = 0;

          if (p-dubinspath.w1)'*dubinspath.q1 < 0 % get to right side of H3
              flag_transition = 4;
              flag_first_time_in_state = 1;
          else
              flag_first_time_in_state = 0;
          end

  end

  out = [flag; Va_d; r; q; c; rho; lambda; state; flag_need_new_wpts];

end


function [ bs ] = LandmarkDetector( im )
%LANDMARKDETECTOR Summary of this function goes here
%   Detailed explanation goes here
load('ZhuRamananModel.mat');
%posemap = 90:-15:-90;
%clf; imagesc(im); axis image; axis off; drawnow;
    bs = detect(im, model, model.thresh);
    bs = clipboxes(im, bs);
    bs = nms_face(bs,0.3);
% figure,showboxes(im, bs(1),posemap),title('Highest scoring detection');
% text((bs.xy(:,1)+bs.xy(:,3))/2,(bs.xy(:,2)+bs.xy(:,4))/2,cellstr(num2str([1:68]')))

end


function openCalcParamWindowSLI(obj)
		%Here a window for the calculation parameters should be created
		%I suggest using the inputsdlg function

		Title = 'SLIDER Decluster ';
		Prompt={'Selection Method:', 'Sli_Type';...
			'Min. Mainshock Magnitude:','MainMag';...
			'Time before Eq (d):', 'BackTime';...
			'Time after Eq (d):', 'ForeTime';...
			'Min. search radius (km):', 'MinRad';...
			'Mag. radius scaler:', 'MagRadScale';...
			'Mag. radius constant:','MagRadConst'};

		LabelList2 =  {	'Magnitude';...
				'Time'};

		%Sli Method
		Formats(1,1).type='list';
		Formats(1,1).style='popupmenu';
		Formats(1,1).items=LabelList2;

		%min MainMag
		Formats(2,1).type='edit';
		Formats(2,1).format='float';
		Formats(2,1).limits = [-99 99];

		%Time before eq
		Formats(3,1).type='edit';
		Formats(3,1).format='float';
		Formats(3,1).limits = [0 9999];

		%Time after eq
		Formats(4,1).type='edit';
		Formats(4,1).format='float';
		Formats(4,1).limits = [0 9999];

		%Min Search Radius
		Formats(5,1).type='edit';
		Formats(5,1).format='float';
		Formats(5,1).limits = [0 9999];

		%Mag Search Radius Scaler
		Formats(6,1).type='edit';
		Formats(6,1).format='float';
		Formats(6,1).limits = [0 9999];

		%Mag Search Radius Constant
		Formats(7,1).type='edit';
		Formats(7,1).format='float';
		Formats(7,1).limits = [0 9999];

		%%%% SETTING DIALOG OPTIONS
		Options.WindowStyle = 'modal';
		Options.Resize = 'on';
		Options.Interpreter = 'tex';
		Options.ApplyButton = 'off';

		%open the dialog window
		[NewParameter,Canceled] = inputsdlg(Prompt,Title,Formats,obj.CalcParameter,Options);

		obj.CalcParameter=NewParameter;

		if Canceled==1
			obj.StartCalc=false;
		else
			obj.StartCalc=true;
		end

end


function [headinfo,fid]=decode_sp3h(fid)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%read file header information
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
global glc gls
type=' '; time=gls.gtime; ns=0;
sats=zeros(1,glc.MAXSAT); tsys=''; bfact=zeros(1,2);
n=0; idxns=0; idxc=0; idxf=0; idx_break=0;

while ~feof(fid)
    line=fgetl(fid);n=n+1;
    if n==1
        type=line(3);
        time=str2time(line(4:31));
    elseif strcmp(line(1),'+')
        if n==3,ns=str2double(line(3:6));end
        if idxns<ns
            j=0;
            while j<17
                sys=code2sys(line(10+3*j));
                prn=str2double(line(11+3*j:12+3*j));
                if sys==glc.SYS_QZS,prn=prn+192;end
                if idxns<=glc.MAXSAT
                    sats(idxns+1)=satno(sys,prn);
                    idxns=idxns+1;
                end
                j=j+1;
            end
        end
    elseif strcmp(line(1:2),'%c')&&idxc==0
        tsys=line(10:12);
        idxc=idxc+1;
    elseif strcmp(line(1:2),'%f')&&idxf==0
        bfact(1)=str2double(line(4:13));
        bfact(2)=str2double(line(15:26));
        idxf=idxf+1;
    elseif strcmp(line(1:2),'/*')
        idx_break=idx_break+1;
        if idx_break==4
            break;
        end
    end

end

headinfo.type=type;
headinfo.time=time;
headinfo.ns=ns;
headinfo.sats=sats;
headinfo.tsys=tsys;
headinfo.bfact=bfact;

return


function SMTout=time_delay_embedding(SMT,CNT,tau,time_interval)
% TDE: time-delay embedding

SMT_in=SMT;
CNT_in=CNT;
interval=time_interval;

SMT_in.x=permute(SMT_in.x,[1,3,2]);
SMT_in_d=prep_segmentation(CNT_in, {'interval', interval-tau});
SMT_in_d.x=permute(SMT_in_d.x,[1,3,2]);
SMT_in.x=[SMT_in.x,SMT_in_d.x];
SMT_in.x=permute(SMT_in.x,[1,3,2]);

SMTout=SMT_in;
end


function [jiDim,jiUL,resolu,ZC,bands]=envihdrread(filename)
% any ENVI file uint 8, int16, and uint16
% [jiDim,jiUL,resolu,ZC]=envihdrread(filename)
% Input file directory
% Output 1) im = data
% Output 2) jiDim dimension cols and rows
% Output 3)jiUL UpperLeft coner of the pixel (x,y)
% Output  4) resolution [x,y]
% Output 5) ZC zone code

filename_HDR=[filename,'.HDR'];
filename_hdr=[filename,'.hdr'];

fid_in1=fopen(filename_hdr,'r');
fid_in2=fopen(filename_HDR,'r');

if fid_in1~=-1
    fid_in=fid_in1;
elseif fid_in2~=-1
    fid_in=fid_in2;
else
    fprintf('Wrong ENVI header file!\n');
    fprintf('%s\n',filename); % show envi hdr file
    return;
end

geo_char=fscanf(fid_in,'%c',inf);
fclose(fid_in);

geo_char=geo_char';
geo_str=strread(geo_char,'%s');

indx_samples = strmatch('samples',geo_str)+2;
indx_lines = strmatch('lines',geo_str)+2;
indx_bands = strmatch('bands',geo_str)+2;
indx_datatype = strmatch('data',geo_str)+3;
indx_interleave = strmatch('interleave',geo_str)+2;
indx_xUL = strmatch('map',geo_str)+6;
indx_yUL = strmatch('map',geo_str)+7;
indx_xreso = strmatch('map',geo_str)+8;
indx_yreso = strmatch('map',geo_str)+9;
indx_zc = strmatch('map',geo_str)+10;

% read input image hdr
cols = str2double(geo_str(indx_samples));
rows = str2double(geo_str(indx_lines));
jiDim = [cols,rows];

bands = str2double(geo_str(indx_bands));
datatype = str2double(geo_str(indx_datatype));
interleave = char(geo_str(indx_interleave));
jiUL(1)=str2double(geo_str(indx_xUL));
jiUL(2)=str2double(geo_str(indx_yUL));
resolu(1)=str2double(geo_str(indx_xreso));
resolu(2)=str2double(geo_str(indx_yreso));
ZC=str2double(geo_str(indx_zc));

if datatype == 1
    in_type = 'uint8';
elseif datatype == 2
    in_type = 'int16';
elseif datatype == 3
    in_type = 'int32';
elseif datatype == 4
    in_type = 'single';
elseif datatype == 12
    in_type = 'uint16';
else
    error('Invalid read data type!');
end

% % Define the data set.
% % Read every other band of the data using the Band-Sequential format.
% im = multibandread(filename, [rows cols bands], ...
%                     [in_type,'=>',in_type], 0, interleave, 'ieee-le');
end


function spy( x, mode )
global cvx___

%   SPY is not a computational function, so it can be applied to any
%   CVX variables.

switch nargin,
    case 0,
        cvx_throw( 'Not enough arguments.' );
    case 1,
        mode = '';
    case 2,
        if ~ischar( mode ) || size( mode, 1 ) > 1,
            cvx_throw( 'Second argument must be a string.' );
        end
end

b = x.basis_;
s = x.size_;

switch mode,
    case { '2-d', '2-D', '2d', '2D' },
        b = sum( b ~= 0, 1 );
        b = reshape( b, s );
    case { '', '3-d', '3-D', '3d', '3D' },
        p = length( cvx___.classes ) + 1;
        if size( b, 1 ) < p, b( p, end ) = 0; end
    otherwise,
        cvx_throw( 'Unknown spy mode: %s', mode );
end

spy( b );

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


% -*- INTERNAL UNDOCUMENTED FUNCTION -*-
function [geometry, msh, sp, u, gnum] = ...
                    mp_solve_plane_strain_2d (problem_data, method_data)

warning ('geopdes:obsolete', 'Function MP_SOLVE_PLANE_STRAIN_2D is obsolete. Using MP_SOLVE_LINEAR_ELASTICITY instead')

[geometry, msh, sp, u, gnum] = mp_solve_linear_elasticity (problem_data, method_data);


clear
% close all
clc;

addpath(genpath('../libs'));
path_to_matconvnet = '/home/skong2/scratch/matconvnet-1.0-beta23_modifiedDagnn';

run(fullfile(path_to_matconvnet, 'matlab', 'vl_setupnn'));
addpath(genpath(fullfile('dependencies', 'matconvnet','examples')));
%% read matconvnet model
load('imdb_toydata_v3_from_mnist.mat');
imdb.path = './toydata_v3';
imdb.path_to_dataset = './toydata_v3';

% set GPU
gpuId = 2; %[1, 2];
gpuDevice(gpuId);
flagSaveFig = true; % {true false} whether to store the result

saveFolder = 'main001_instSeg_v1_absEucMM/';
modelName = 'softmax_net-epoch-31.mat';
%% modify network for testing
netMat = load( fullfile('./exp', saveFolder, modelName) );
netMat = netMat.net;
netMat = dagnn.DagNN.loadobj(netMat);

for loopIdx = 5:-1:1
    rmLayerName = sprintf('loop%d_meanshift_Y_l2norm', loopIdx);
    netMat.vars(netMat.layers(netMat.getLayerIndex(rmLayerName)).outputIndexes).precious = 1;

    % add regression loss
    rmLayerName = sprintf('loop%d_instSeg_reg', loopIdx);
    netMat.removeLayer(rmLayerName); % remove layer

    % add max-margin loss
    rmLayerName = sprintf('loop%d_instSeg_MM', loopIdx);
    netMat.removeLayer(rmLayerName); % remove layer

    % add max-margin loss
    rmLayerName = sprintf('loop%d_meanshift_cosSim', loopIdx);
    netMat.removeLayer(rmLayerName); % remove layer
end

rmLayerName = 'obj_instSeg_MM';
if ~isnan(netMat.getLayerIndex(rmLayerName))
    netMat.removeLayer(rmLayerName); % remove layer
end
rmLayerName = 'obj_instSeg_reg';
if ~isnan(netMat.getLayerIndex(rmLayerName))
    netMat.removeLayer(rmLayerName); % remove layer
end
rmLayerName = 'res7_cosSim';
if ~isnan(netMat.getLayerIndex(rmLayerName))
    netMat.removeLayer(rmLayerName); % remove layer
end
netMat.vars(netMat.layers(netMat.getLayerIndex('res7_l2norm')).outputIndexes).precious = 1;
%% configure training environment
saveFolder = [strrep(saveFolder, '/', '') '_visualization'];
if ~isdir(saveFolder) && flagSaveFig
    mkdir(saveFolder);
end
netMat.move('gpu');
% netMat.mode = 'test';
netMat.mode = 'normal';
testList = find(imdb.set==2);
rawCountsAll = {};

semanticMaskMat = [];
instanceMaskMat = [];
imgMat = [];
predInstanceMaskMat_loop0 = [];
predInstanceMaskMat = {};
for loopIdx = 1:5
    predInstanceMaskMat{loopIdx} = [];
end
st = 0;
for i = 1:5%length(testList)
    curBatch = fullfile(imdb.path_to_dataset, imdb.imgList{testList(i)});
    datamat = load(curBatch);

    datamat.semanticMaskMat = reshape(datamat.semanticMaskMat, [size(datamat.semanticMaskMat,1), size(datamat.semanticMaskMat,2), 1, size(datamat.semanticMaskMat,3)]);
    datamat.instanceMaskMat = reshape(datamat.instanceMaskMat, [size(datamat.instanceMaskMat,1), size(datamat.instanceMaskMat,2), 1, size(datamat.instanceMaskMat,3)]);

    imFeed = bsxfun(@minus, datamat.imgMat, imdb.meta.meanvalue);
    inputs = {'input', gpuArray(imFeed)};
    netMat.eval(inputs) ;
    res7_l2norm = gather(netMat.vars(netMat.layers(netMat.getLayerIndex('res7_l2norm')).outputIndexes).value);
    predInstanceMaskMat_loop0(:,:,:,st+1:st+size(datamat.imgMat,4)) = res7_l2norm;

    for loopIdx = 1:5
        curLayerName = sprintf('loop%d_meanshift_Y_l2norm', loopIdx);
        predInstanceMaskMat{loopIdx}(:,:,:,st+1:st+size(datamat.imgMat,4)) = ...
            gather(netMat.vars(netMat.layers(netMat.getLayerIndex(curLayerName)).outputIndexes).value);
    end

    imgMat(:,:,:,st+1:st+size(datamat.imgMat,4)) = datamat.imgMat;
    semanticMaskMat(:,:,:,st+1:st+size(datamat.imgMat,4)) = datamat.semanticMaskMat;
    instanceMaskMat(:,:,:,st+1:st+size(datamat.imgMat,4)) = datamat.instanceMaskMat;
    %predInstanceMaskMat(:,:,:,st+1:st+size(datamat.imgMat,4)) = res7_l2norm;
    fprintf('\t%d/%d\n', i, length(testList));

    st = st + size(datamat.imgMat,4);
end
%% grouping results to visualize
num = 25; % the total number of example to show
M = 5; % the number of rows in the panel, each row shows N images as below
N = 5; % the number of columns in the panel, each column shows M images as above
% h = 28;
% w = 28;
panelSZ = round(64/2)*2; % the size (height/width) of the small square image
% stepSize = (panelSZ-h)/2;

% image
showcaseSamples = stitchImage2panel(imgMat, panelSZ, M, N, 1, num);

% ground-truth semantic mask
showcaseSemanticMask= semanticMaskMat(:,:,:,1:num);
showcaseSemanticMask = reshape(showcaseSemanticMask, [numel(showcaseSemanticMask(:,:,1,1)), num]);
showcaseSemanticMask = showStitch(showcaseSemanticMask, [panelSZ panelSZ], M, N, 'whitelines', 'linewidth', 5);

% ground-truth instance mask
showcaseInstanceMask= instanceMaskMat(:,:,:,1:num);
showcaseInstanceMask = reshape(showcaseInstanceMask, [numel(showcaseInstanceMask(:,:,1,1)), num]);
showcaseInstanceMask = showStitch(showcaseInstanceMask, [panelSZ panelSZ], M, N, 'whitelines', 'linewidth', 5);

predInstanceMaskMat_loop0 = stitchImage2panel(predInstanceMaskMat_loop0, panelSZ, M, N, 1, num);
predInstanceMaskMat_loop0 = predInstanceMaskMat_loop0 - min(predInstanceMaskMat_loop0(:));
predInstanceMaskMat_loop0 = predInstanceMaskMat_loop0 ./ max(predInstanceMaskMat_loop0(:));

predInstanceMask = {};
for loopIdx = 1:5
    predInstanceMask{loopIdx} = stitchImage2panel(predInstanceMaskMat{loopIdx}, panelSZ, M, N, 1, num);
    predInstanceMask{loopIdx} = predInstanceMask{loopIdx} - min(predInstanceMask{loopIdx}(:));
    predInstanceMask{loopIdx} = predInstanceMask{loopIdx} ./ max(predInstanceMask{loopIdx}(:));
end
%% save the results
figFolder = './figFolder';
if ~isdir(figFolder)
    mkdir(figFolder);
end
a = strfind(modelName,'epoch');
path_to_save = fullfile(figFolder, [strrep(saveFolder,'/','') '_' modelName(a(1):end-4)]);
if ~isdir(path_to_save)
    mkdir(path_to_save);
end

% summary vidualization
imgFig2 = figure(1);
subWindowH = 2;
subWindowW = 2;
set(imgFig2, 'Position', [100 100 1400 900]) % [1 1 width height]
windowID = 1;
subplot(subWindowH, subWindowW, windowID); imagesc(showcaseSamples); axis off image; title('random samples'); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(showcaseSemanticMask); axis off image; title('showcase SemanticMask'); caxis([0, 11]); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(showcaseInstanceMask); axis off image; title('showcase instanceMask'); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(predInstanceMaskMat_loop0); axis off image; title('predInstanceMask'); colorbar; windowID = windowID + 1;
if flagSaveFig
    export_fig( sprintf('%s/fig00_visualization.jpg', path_to_save) );
end

imgFig2 = figure(2);
subWindowH = 2;
subWindowW = 3;
set(imgFig2, 'Position', [100 100 1400 900]) % [1 1 width height]
windowID = 1;
subplot(subWindowH, subWindowW, windowID); imagesc(predInstanceMaskMat_loop0); axis off image; title('loop0'); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(predInstanceMask{1}); axis off image; title('loop1'); caxis([0, 11]); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(predInstanceMask{2}); axis off image; title('loop2'); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(predInstanceMask{3}); axis off image; title('loop3'); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(predInstanceMask{4}); axis off image; title('loop4'); colorbar; windowID = windowID + 1;
subplot(subWindowH, subWindowW, windowID); imagesc(predInstanceMask{5}); axis off image; title('loop5'); colorbar; windowID = windowID + 1;
if flagSaveFig
    export_fig( sprintf('%s/fig01_visualization_looping.jpg', path_to_save) );
end

if flagSaveFig
    showcaseSemanticMask = uint8(showcaseSemanticMask * 255/11);
    showcaseInstanceMask = showcaseInstanceMask*255/5;

    imwrite(showcaseSamples, fullfile(path_to_save, sprintf('fig02_showcaseSamples.bmp')));
    imwrite(showcaseSemanticMask, fullfile(path_to_save, sprintf('fig03_showcaseSemanticMask.bmp')));
    imwrite(showcaseInstanceMask, fullfile(path_to_save, sprintf('fig04_showcaseInstanceMask.bmp')));
    imwrite(predInstanceMaskMat_loop0, fullfile(path_to_save, sprintf('fig05_predInstanceMask-loop0.bmp')));
    imwrite(predInstanceMask{1}, fullfile(path_to_save, sprintf('fig06_predInstanceMask-loop1.bmp')));
    imwrite(predInstanceMask{2}, fullfile(path_to_save, sprintf('fig07_predInstanceMask-loop2.bmp')));
    imwrite(predInstanceMask{3}, fullfile(path_to_save, sprintf('fig08_predInstanceMask-loop3.bmp')));
    imwrite(predInstanceMask{4}, fullfile(path_to_save, sprintf('fig09_predInstanceMask-loop4.bmp')));
    imwrite(predInstanceMask{5}, fullfile(path_to_save, sprintf('fig10_predInstanceMask-loop5.bmp')));

    save(sprintf('%s/results.mat', path_to_save), 'imgMat', 'instanceMaskMat', 'semanticMaskMat', 'predInstanceMaskMat');
end

%% leaving blank


function sF = power(sF1,sF2)
%
% Syntax
%   sF = sF1.^a
%

sF = power@S2FunHarmonic(sF1,sF2);

% try to preserve symmetry
sym = [];
if isnumeric(sF1)
  sym = sF2.CS;
elseif isnumeric(sF2)
  sym = sF1.CS;
elseif isa(sF1,'S2FunHarmonicSym') && isa(sF2,'S2FunHarmonicSym')
  sym = disjoint(sF1.CS,sF2.CS);
end

if ~isempty(sym), sF = S2FunHarmonicSym(sF.fhat,sym); end

end


function varargout=bss_decomp_tvgain(varargin)

% decompose an estimated source into target/interference/noise/artefacts components, assuming the admissible distortion is a time-varying gain.
%
% Usage:
%
% [s_target,e_interf[,e_noise],e_artif]=bss_decomp_tvgain(se,index,S[,N],tvshape,tvstep)
%
% Input:
%   - se: row vector of length T containing the estimated source,
%   - index: points which component of S se has to be compared to,
%   - S: n x T matrix containing the original sources,
%   - N: m x T matrix containing the noise on the obseravtions (if any).
%   - tvshape : row vector of length V at most T containing the shape of the elementary
%     allowed time variations of the gain
%   - tvstep  : hop size (in number of samples) between two consecutive
%     variations of the gain
%
% Output:
%   - s_target: row vector of length T containing the target source(s)
%   contribution,
%   - e_interf: row vector of length T containing the interferences
%   contribution,
%   - e_noise: row vector of length T containing the noise contribution (if
%   any),
%   - e_artif: row vector of length T containing the artifacts
%   contribution.
%
% Developers:  - Cedric Fevotte (cf269@cam.ac.uk) - Emmanuel Vincent
% (vincent@ircam.fr) - Remi Gribonval (remi.gribonval@irisa.fr)

switch nargin
    case 5
        [varargout{1},varargout{2},varargout{3}]=bss_decomp_tvfilt(varargin{1},varargin{2},varargin{3},varargin{4},varargin{5},0);
    case 6
        [varargout{1},varargout{2},varargout{3},varargout{4}]=bss_decomp_filt(varargin{1},varargin{2},varargin{3},varargin{4},varargin{5},varargin{6},0);
    otherwise
        disp('Wrong number of arguments.')
end


%% Example
% real time control of the KUKA iiwa 7 R 800
% the impedence control is turned on
% Moving first joint of the robot, using a sinisoidal function

% The external torques are plotted in real-time during the test
% the feedback from the measured torques can be used to for a closed loop
% control

% An example script, it is used to show how to use the different
% functions of the KUKA Sunrise matlab toolbox

% First start the server on the KUKA iiwa controller
% Then run this script using Matlab

% Copyright: Mohammad SAFEEA, 13th of March 2018

% Important: Be careful when runnning the script, be sure that no human, nor obstacles
% are around the robot

close all,clear all;clc;

warning('off')

ip='172.31.1.147'; % The IP of the controller
% start a connection with the server
global t_Kuka;
t_Kuka=net_establishConnection( ip );

if ~exist('t_Kuka','var') || isempty(t_Kuka) || strcmp(t_Kuka.Status,'closed')
  warning('Connection could not be establised, script aborted');
  return;
else

      %% Move point to point to an initial position
        jPos={0,0,0,-pi/2,0,pi/2,0};
      setBlueOff(t_Kuka); % turn Off blue light

      relVel=0.15;
      movePTPJointSpace( t_Kuka , jPos, relVel); % move to initial configuration
     %% Pause for 3 seocnds
     pause(3);
        %% Start direct servo in joint space
        massOfTool=0.5; % the mass of the tool attached to flange in Kg
        cOMx=0; % X coordinate of the center of mass of the tool in (mm)
        cOMy=0; % Y coordinate of the center of mass of the tool in (mm)
        cOMz=40; % Z coordinate of the center of mass of the tool in (mm)
        cStiness=900; % cartizian stifness
        rStifness=80; % rotational stifness
        nStifness=50; % null space stifness

        % Start the realtime control with impedence
        realTime_startImpedanceJoints(t_Kuka,massOfTool,cOMx,cOMy,cOMz,...
        cStiness,rStifness,nStifness);

       w=0.6; % motion constants, frequency rad/sec
       A=0.2; % motion constants, amplitude of motion

       a=datevec(now);
       t0=a(6)+a(5)*60+a(4)*60*60; % calculate initial time

     dt=0;
     tstart=t0;
     counter=0;
     duration=20*60; %1 minutes
     %% Control loop
     % real time plot handles
         colors={'k','b','r','g','c','m','y'};
         figureHandle=figure('Units','inches','Position',[0 0 5 3.75]);
         numOfSamples=120; % number of samples to show in the plot
         timeVec=1:numOfSamples;
         tawVec=zeros(7,numOfSamples);
         plotHandle=[];
         for i=1:7
            plotHandle=[plotHandle,plot(timeVec,tawVec(i,:),colors{i},'LineWidth',2)];
            hold on;
         end
        % format the plot
        ylim([0,8]);
        temp=title('External torques');
        set(temp,'FontSize',16);
        temp=xlabel('Time (seconds)');
        set(temp,'FontSize',14);
        temp=ylabel('Distance (m)');
        set(temp,'FontSize',14);

       while(dt<duration)
         %% perform trajectory calculation here
          a=datevec(now);
          time=a(6)+a(5)*60+a(4)*60*60;
          dt=time-t0;

          temp=A*(1-cos(w*dt));
            for dacount=1:7
                jPosCommand{dacount}=jPos{dacount}+temp;
            end
          counter=counter+1;
          %% Send joint positions to robot
          taw=sendJointsPositionsExTorque( t_Kuka ,jPosCommand);

          tawTemp=zeros(7,1);
          for i=1:7
              tawTemp(i)=taw{i};
              torque_array(i,counter)=taw{i};
          end
          % delete first measurment
          tawVec(:,1)=[];
          % add new measurment to end of arrays
          tawVec=[tawVec,tawTemp];
          % update plots data
          for i=1:6
            set(plotHandle(i),'YData',tawVec(i,:));
          end
          set(plotHandle(7),'YData',tawVec(i,:));
          % show plot data
          set(figureHandle,'Visible','on');

       end

       %% Stop the realtime control with impedence motion
       realTime_stopImpedanceJoints( t_Kuka );

       fprintf('\n Motion stopped \n');
       pause(2);
       %% turn off light
       setBlueOff(t_Kuka);

      %% turn off the server
       net_turnOffServer( t_Kuka );

       fclose(t_Kuka);

end
warning('on')


function gpmf = gpmf_constant(varargin)
%GPMF_CONSTANT  Create a constant mean function
%
%  Description
%    GPMF = GPMF_CONSTANT('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates constant mean function structure in which the named
%    parameters have the specified values. Any unspecified
%    parameters are set to default values.
%
%    GPMF = GPMF_CONSTANT(GPMF,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a mean function structure with the named parameters
%    altered with the specified values.
%
%    Parameters for constant mean function
%      constant          - constant value for the constant
%                          base function (default 1)
%      prior_mean        - prior mean (scalar or vector) for base
%                          functions' weight prior (default 0)
%      prior_cov         - prior covariances (scalar or vector)
%                          for base functions' prior corresponding
%                          each selected input dimension. In
%                          multiple dimension case prior_cov is a
%                          struct containing scalars or vectors.
%                          The covariances must all be either
%                          scalars (diagonal cov.matrix) or
%                          vectors (for non-diagonal cov.matrix)
%                          (default 100)
%
%  See also
%    GP_SET, GPMF_LINEAR, GPMF_SQUARED
%
% Copyright (c) 2010 Tuomas Nikoskinen
% Copyright (c) 2011 Jarno Vanhatalo

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

  ip=inputParser;
  ip.FunctionName = 'GPMF_CONSTANT';
  ip.addOptional('gpmf', [], @isstruct);
  ip.addParamValue('constant',1, @(x) isvector(x));
  ip.addParamValue('prior_mean',0, @(x) isvector(x));
  ip.addParamValue('prior_cov',100, @(x) isvector(x));
  ip.addParamValue('mean_prior', [], @isstruct);
  ip.addParamValue('cov_prior', [], @isstruct);
  ip.parse(varargin{:});
  gpmf=ip.Results.gpmf;

  if isempty(gpmf)
    % Initialize a mean function
    init=true;
    gpmf.type = 'gpmf_constant';
  else
    % Modify a mean function
    if ~isfield(gpmf,'type') && isequal(gpmf.type,'gpmf_constant')
      error('First argument does not seem to be a constant mean function')
    end
    init=false;
  end

  % Initialize parameters
  if init || ~ismember('type',ip.UsingDefaults)
    gpmf.constant = ip.Results.constant;
  end
  if init || ~ismember('prior_mean',ip.UsingDefaults)
    gpmf.b=ip.Results.prior_mean(:)';
  end
  if init || ~ismember('prior_mean',ip.UsingDefaults)
    gpmf.B=ip.Results.prior_cov(:)';
  end
  if init || ~ismember('mean_prior',ip.UsingDefaults)
    gpmf.p.b=ip.Results.mean_prior;
  end
  if init || ~ismember('cov_prior',ip.UsingDefaults)
    gpmf.p.B=ip.Results.cov_prior;
  end
  if init
    % Set the function handles to the nested functions
    gpmf.fh.geth = @gpmf_geth;
    gpmf.fh.pak = @gpmf_pak;
    gpmf.fh.unpak = @gpmf_unpak;
    gpmf.fh.lp = @gpmf_lp;
    gpmf.fh.lpg = @gpmf_lpg;
    gpmf.fh.recappend = @gpmf_recappend;
  end

end

function h = gpmf_geth(gpmf, x)
%GPMF_GETH  Calculate the base function values for a given input.
%
%  Description
%    H = GPMF_GETH(GPMF,X) takes in a mean function structure
%    GPMF and inputs X. The function returns a row vector of
%    length(X) containing the constant value which is by default
%    1.

  constant=gpmf.constant;
  if isscalar(constant)
    h = repmat(constant,1,length(x(:,1)));
  else
    h = constant(:)';
  end

end

function [w, s, h] = gpmf_pak(gpmf, w)
%GPMF_PAK  Combine GP mean function parameters into one vector
%
%  Description
%    W = GPMF_PAK(GPMF) takes a mean function
%    structure GPMF and combines the mean function
%    parameters and their hyperparameters into a single row
%    vector W.
%
%       w = [ log(gpmf.b)
%             (hyperparameters of gpmf.b)
%             log(gpmf.B)
%             (hyperparameters of gpmf.B)]'
%
%  See also
%    GPMF_UNPAK

  w = []; s = {}; h=[];
  if ~isempty(gpmf.p.b)
    w = gpmf.b;
    if numel(gpmf.b)>1
      s = [s; sprintf('gpmf_constant.b x %d',numel(gpmf.b))];
    else
      s = [s; 'gpmf_constant.b'];
    end
    h = [h -1.*ones(1,numel(gpmf.b))];
    % Hyperparameters of b
    [wh, sh, hh] = gpmf.p.b.fh.pak(gpmf.p.b);
    w = [w wh];
    s = [s; sh];
    h = [h -1-hh];
  end

  if ~isempty(gpmf.p.B)
    w = [w log(gpmf.B)];
    if numel(gpmf.B)>1
      s = [s; sprintf('log(gpmf_constant.B x %d)',numel(gpmf.B))];
    else
      s = [s; 'log(gpmf_constant.B)'];
    end
    h = [h -1.*ones(1,numel(gpmf.B))];
    % Hyperparameters of B
    [wh, sh, hh] = gpmf.p.B.fh.pak(gpmf.p.B);
    w = [w wh];
    s = [s; sh];
    h = [h -1-hh];
  end

end

function [gpmf, w] = gpmf_unpak(gpmf, w)
%GPMF_UNPAK  Sets the mean function parameters into the structure
%
%  Description
%    [GPMF, W] = GPMF_UNPAK(GPMF, W) takes a covariance
%    function structure GPMF and a hyper-parameter vector W, and
%    returns a mean function structure identical to the
%    input, except that the covariance hyper-parameters have been
%    set to the values in W. Deletes the values set to GPMF from
%    W and returns the modified W.
%
%    Assignment is inverse of
%       w = [ log(gpmf.b)
%             (hyperparameters of gpmf.b)
%             log(gpmf.B)
%             (hyperparameters of gpmf.B)]'
%
%  See also
%   GPMF_PAK

  gpp=gpmf.p;

  if ~isempty(gpp.b)
    i2=length(gpmf.b);
    i1=1;
    gpmf.b = w(i1:i2);
    w = w(i2+1:end);

    % Hyperparameters of b
    [p, w] = gpmf.p.b.fh.unpak(gpmf.p.b, w);
    gpmf.p.b = p;
  end

  if ~isempty(gpp.B)
    i2=length(gpmf.B);
    i1=1;
    gpmf.B = exp(w(i1:i2));
    w = w(i2+1:end);

    % Hyperparameters of B
    [p, w] = gpmf.p.B.fh.unpak(gpmf.p.B, w);
    gpmf.p.B = p;
  end

end

function lp = gpmf_lp(gpmf)
%GPMF_SEXP_LP  Evaluate the log prior of mean function parameters
%
%  Description
%
%  See also

% Evaluate the prior contribution to the error. The parameters that
% are sampled are transformed, e.g., W = log(w) where w is all
% the "real" samples. On the other hand errors are evaluated in
% the W-space so we need take into account also the Jacobian of
% transformation, e.g., W -> w = exp(W). See Gelman et al. (2013),
% Bayesian Data Analysis, third edition, p. 21.
  lp = 0;
  gpp=gpmf.p;

  if ~isempty(gpmf.p.b)
    lp = lp + gpp.b.fh.lp(gpmf.b, ...
                   gpp.b);
  end

  if ~isempty(gpp.B)
    lp = lp + gpp.B.fh.lp(gpmf.B, ...
                   gpp.B) +sum(log(gpmf.B));
  end
end

function [lpg_b, lpg_B] = gpmf_lpg(gpmf)
%GPMF_SEXP_LPG  Evaluate gradient of the log prior with respect
%               to the parameters.
%
%  Description
%    LPG = GPMF_SEXP_LPG(GPMF) takes a mean function
%    structure GPMF and returns LPG = d log (p(th))/dth, where th
%    is the vector of parameters.
%
%  See also
%    GPMF_SEXP_PAK, GPMF_SEXP_UNPAK, GPMF_SEXP_LP, GP_G

  lpg_b=[]; lpg_B=[];
  gpp=gpmf.p;

  if ~isempty(gpmf.p.b)
    lll = length(gpmf.b);
    lpgs = gpp.b.fh.lpg(gpmf.b, gpp.b);
    lpg_b = [lpgs(1:lll) lpgs(lll+1:end)];  %.*gpmf.b+1
  end

  if ~isempty(gpmf.p.B)
    lll = length(gpmf.B);
    lpgs = gpp.B.fh.lpg(gpmf.B, gpp.B);
    lpg_B = [lpgs(1:lll).*gpmf.B+1 lpgs(lll+1:end)];
  end
end

function recmf = gpmf_recappend(recmf, ri, gpmf)
%RECAPPEND  Record append
%
%  Description
%
%  See also
%    GP_MC and GP_MC -> RECAPPEND

% Initialize record
  if nargin == 2
    recmf.type = 'gpmf_constant';

    % Initialize parameters
    recmf.b= [];
    recmf.B = [];

    % Set the function handles
    recmf.fh.geth = @gpmf_geth;
    recmf.fh.pak = @gpmf_pak;
    recmf.fh.unpak = @gpmf_unpak;
    recmf.fh.lp = @gpmf_lp;
    recmf.fh.lpg = @gpmf_lpg;
    recmf.fh.recappend = @gpmf_recappend;

    recmf.p=[];
    recmf.p.b=[];
    recmf.p.B=[];
    if isfield(ri.p,'b') && ~isempty(ri.p.b)
      recmf.p.b = ri.p.b;
    end
    if ~isempty(ri.p.B)
      recmf.p.B = ri.p.B;
    end
    return
  end

  gpp = gpmf.p;

  % record magnSigma2
  if ~isempty(gpmf.b)
    recmf.b(ri,:)=gpmf.b;
    if ~isempty(recmf.p.b)
      recmf.p.b = gpp.b.fh.recappend(recmf.p.b, ri, gpmf.p.b);
    end
  elseif ri==1
    recmf.b=[];
  end

  if ~isempty(gpmf.B)
    recmf.B(ri,:)=gpmf.B;
    if ~isempty(recmf.p.B)
      recmf.p.B = gpp.B.fh.recappend(recmf.p.B, ri, gpmf.p.B);
    end
  elseif ri==1
    recmf.B=[];
  end

end


function [theta,cmethod,interpolation,preconditioner] = amgoption(option)
%% AMGOPTIONS default options for algebraic multigrid solver
%
%   theta = 0.025;	cmethod = 'C'; interpolation = 'S'; preconditioner = 'W';
%
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if isfield(option,'theta')
    theta = option.theta;
else
    theta = 0.025;
end

if isfield(option,'cmethod')
    cmethod = option.cmethod;
else
    cmethod = 'C';
end

if isfield(option,'interpolation')
    interpolation = option.interpolation;
else
    interpolation = 'T';
end
if isfield(option,'preconditioner')
    preconditioner = option.preconditioner;
else
    preconditioner = 'W';
end


function dt6_new = dtiResliceTensorAffine(dt6, xform, mmPerVoxIn, boundingBox, mmPerVoxOut)
% dt6_new = dtiResliceTensorAffine(dt6, xform, mmPerVoxIn, [boundingBox], [mmPerVoxOut])
%
% Applies an affine transform to a tensor volume in dt6 format.
%
% REQUIRES:
% spm99 or spm2 in the path.
%
% HISTORY:
% 2004.02.01 RFD & ASH Wrote it.
% 2004.08.06 RFD & ASH- we now use the Finite Strain method, which means
% that we no longer ignore shears.
% 2005.03.25 RFD- fixed the bug that was introduced with the last edit
% (2004.08.06). The tensor rotation was the inverse of what it should have
% been! I also think that applying the tensor rotation after the
% interpolation was introducing some subtle artifacts, so I switched back
% to rotating the tensors before interpolating (the order really shouldn't
% matter, so we should look into this more to understand why it does.)
%
% example:
% sn3dParams = load('dti_analyses/may021126_B0_sn3d.mat');
% xformToB0 = computeCannonicalXformFromIfile('dti/B0.001');
% xformToTensor = inv(xformToB0);
% [eigVec,eigVal,mm] = dtiLoadTensor('/snarp/u1/dti/adultData/may021126/dti/Vectors.float');
% dt6 = dtiRebuildTensor(eigVec, eigVal);
% dt6_new = dtiResliceTensor(dt6, sn3dParams, mmPerVoxOut, xformToTensor);
% [newVec, newVal] = dtiSplitTensor(dt6_new);
% fa = dtiComputeFA(newVal);
% figure; imagesc(fa(:,:,23)); axis equal; colormap gray; axis xy;

if(~exist('mmPerVoxOut') | isempty(mmPerVoxOut))
    mmPerVoxOut = [1 1 1];
end

if(~exist('boundingBox') | isempty(boundingBox))
    % create a default bounding box for the resliced data.
    % The following is the spm default bounding box. Note that the box is
    % defined in Talairach space (units = mm).
    boundingBox = [-78 -120 -60;
                    78  80   85];
end

x = (boundingBox(1,1):mmPerVoxOut(1):boundingBox(2,1));
y = (boundingBox(1,2):mmPerVoxOut(2):boundingBox(2,2));
z = (boundingBox(1,3):mmPerVoxOut(3):boundingBox(2,3));

[X,Y,Z] = meshgrid(x, y, z);
clear x y z;

talCoords = [X(:) Y(:) Z(:)];
newSize = size(X);
clear X Y Z;

% Does it matter if tensor rotation is done before or after rotation?
% ROTATE THE TENSORS
% NOTE: we want to apply inv(xform) to the tensors, since xform maps from
% the new space to the old (the correct mapping for the interpolation,
% since we interpolate by creating a grid in the new space and fill it by
% pulling data from the old space.)
rigidXform = dtiFiniteStrainDecompose(inv(xform));
% Apply just the rigid rotation component of the xform to the tensors
dt6 = dtiXformTensors(dt6, rigidXform);

imgCoords = mrAnatXformCoords(xform, talCoords)';
%imgCoords = imgCoords./repmat(mmPerVoxIn',1,size(imgCoords,2));

% the imgCoords are specified in mm, as an offset from the origin. So, we
% convert the 1-indexed imgCoords from mrAnatXformCoords to zero-indexed and
% scale to real mm.
imgCoords = imgCoords - 1;
imgCoords(1,:) = imgCoords(1,:).*mmPerVoxIn(1);
imgCoords(2,:) = imgCoords(2,:).*mmPerVoxIn(2);
imgCoords(3,:) = imgCoords(3,:).*mmPerVoxIn(3);

% We need to mask away the crazy values outside the head.
% Also, the Pajevic algorithm will fold the data over to deal with the
% edges. That might be nice for computing edge values, but it makes for
% ugly images and crazy fiber tacings. The following will build a mask of
% the good image values (ie. values inside the head), interpolate it to the
% new volume size, and then invert it so it marks bad values, those
% outside the head.
dt6_mask = sum(abs(dt6),4)>0.001;
dt6_mask = dtiResliceVolume(double(dt6_mask), xform, boundingBox, mmPerVoxOut);
dt6_mask = dt6_mask>0.5;
dt6_mask = dtiCleanImageMask(dt6_mask);
%figure; imagesc(makeMontage(dt6_mask)); axis equal off; colormap gray;
dt6_mask = ~dt6_mask;

% *** HACK so that matlab can find the shared library
old = pwd;
cd(fileparts(which('dtiTensorInterp_Pajevic')));
dt6_new = dtiTensorInterp_Pajevic(dt6, [imgCoords(1,:);imgCoords(2,:);imgCoords(3,:)]', mmPerVoxIn, 1, mmPerVoxIn./2);
cd(old);
dt6_new(repmat(dt6_mask(:),1,6)) = 0;
dt6_new = reshape(dt6_new, [newSize, 6]);

return;

%
% DEBUGGING:
[vec,val] = dtiSplitTensor(dt6_new);
val(val<100) = 0;
fa_interp = dtiComputeFA(val);
figure; imagesc(fa_interp(:,:,20)); axis equal xy tight off; colorbar;
b0 = loadAnalyze('dti_analyses/may021126_nB0.img');
b0 = permute(b0, [2 1 3]);
figure; imagesc(b0(:,:,20)); axis equal xy tight off; colorbar;
mask = b0>500;
fa_interp(~mask) = 0;
figure; imagesc(fa_interp(:,:,20)); axis equal xy tight off; colorbar;
m = makeMontage(fa_interp);
figure; imagesc(m); axis equal xy tight off; colorbar;


mkdir image_256
p = dir('./image/');
m1 = [];
for i = 3:numel(p)  %remove . & ..
    str = [ './image/',p(i).name,'/*.jpg'];
    pp = dir(str);
    if(numel(pp)==0)
        str = [ './image/',p(i).name,'/*.bmp'];
        pp = dir(str);
    end
    if(numel(pp)==0)
        str = [ './image/',p(i).name,'/*.png'];
        pp = dir(str);
    end
    for j = 1:numel(pp)
        img_str = [ './image/',p(i).name,'/',pp(j).name];
        wimg_str = [ './image_256/',p(i).name,'/',pp(j).name(1:end-3),'jpg'];
        wdir_str = ['./image_256/',p(i).name];
        mkdir(wdir_str);
        im = imread(img_str);
        sz = size(im);
        if(numel(sz)==2)
           im = repmat(im,1,1,3);
        end
        if(sz(1)<sz(2))
           im = imresize(im,[256, round(sz(2)/sz(1)*256)]);
        else
           im = imresize(im,[round(sz(1)/sz(2)*256),256]);
        end
        imwrite(im,wimg_str);
        m = mean(mean(im,1),2);
        m1 = cat(4,m1,m);
    end
end

mm = mean(m1,4);


% ------------------------------------------------------------------------
%  Copyright (C)
%  Universitat Politecnica de Catalunya BarcelonaTech (UPC) - Spain
%  University of California Berkeley (UCB) - USA
%
%  Jordi Pont-Tuset <jordi.pont@upc.edu>
%  Pablo Arbelaez <arbelaez@berkeley.edu>
%  June 2014
% ------------------------------------------------------------------------
% This file is part of the MCG package presented in:
%    Arbelaez P, Pont-Tuset J, Barron J, Marques F, Malik J,
%    "Multiscale Combinatorial Grouping,"
%    Computer Vision and Pattern Recognition (CVPR) 2014.
% Please consider citing the paper if you use this code.
% ------------------------------------------------------------------------
function bbox = mask2box( mask )
    tmp = regionprops(double(mask),'BoundingBox'); % Double to force a single bbox
    bbox(1) = tmp.BoundingBox(2)+0.5;
    bbox(2) = tmp.BoundingBox(1)+0.5;
    bbox(3) = tmp.BoundingBox(2)+tmp.BoundingBox(4)-0.5;
    bbox(4) = tmp.BoundingBox(1)+tmp.BoundingBox(3)-0.5;
end


function [Rob,Sen,Lmk,Obs] = correctKnownLmks(Rob, Sen, Raw, Lmk, Obs, Opt)

% CORRECTKNOWNLMKS  Correct known landmarks.
%   [Rob,Sen,Lmk,Obs] = correctKnownLmks(Rob, Sen, Raw, Lmk, Obs, Opt)
%   performs EKF corrections to a selection of observations, updating Map,
%   Rob, Sen, Lmk and Obs structures.
%
%   Input/output structures:
%       Rob:  the robot
%       Sen:  the sensor
%       Raw:  the raw data issued from Sen
%       Lmk:  the set of landmarks
%       Obs:  the set of observations for the sensor Sen
%       Opt:  the algorithm options
%
%   The selection of the landmarks to observe is based on active-search
%   procedures and individual compatibility tests to reject outliers.
%
%   This function takes care of:
%       1. Selection of observations to correct
%       2. Feature matching
%       3. EKF correction
%       4. Correction of landmark parameters out of the EKF
%       5. Landmark re-parametrization
%       6. Landmark deletion in case of corruption
%
%   The algorithm is configurable through a number of options in structure
%   Opt.correct. Edit USERDATA to access and modify these options.
%
%   See also PROJECTLMK, PROJEUCPNTINTOPINHOLEONROB, SMARTDELETELMK.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

% steps in this function
% 0- update Rob and Sen info from Map
% 1- project all landmarks
% 2- select landmarks to correct. For each one:
% 3- do feature matching. If feature found:
% 4- compute innovation.
% 5- perform consistency test. If it is OK:
% 6- do correction
% 7- eventually reparametrize landmark
% 8- eventually delete corrupted landmarks
% 9- force covariance symmetry

global Map

% 0. UPDATE ROB AND SEN INFO FROM MAP
Rob = map2rob(Rob);
Sen = map2sen(Sen);

% 1. PROJECT ALL LMKS - get all expectations
for lmk = find([Lmk.used])

    Obs(lmk) = projectLmk(Rob,Sen,Lmk(lmk),Obs(lmk),Opt);

end  % --- all landmarks are now projected.

vis = [Obs.vis];

if any(vis) % Consider only visible observations

    % 2. SELECT LMKS TO CORRECT
    [lmksToObs,lmksToSkip] = selectLmksToObserve(Obs(vis),Opt.correct.nUpdates);

    % lmks to skip, update Obs info
    [Obs(lmksToSkip).measured] = deal(false);
    [Obs(lmksToSkip).matched]  = deal(false);
    [Obs(lmksToSkip).updated]  = deal(false);

    for lmk = lmksToObs % for each landmark to correct

        % Update Lmk search counter
        Lmk(lmk).nSearch = Lmk(lmk).nSearch + 1;

        % 3. TRY TO MATCH FEATURE
        Obs(lmk) = matchFeature(Sen,Raw,Obs(lmk));

        if Obs(lmk).matched

            % Update Lmk matched counter
            Lmk(lmk).nMatch = Lmk(lmk).nMatch + 1;

            % 4. COMPUTE INNOVATIONS
            Rob = map2rob(Rob);
            Sen = map2sen(Sen);

            if Opt.correct.reprojectLmks
                % re-project landmark for improved Jacobians
                Obs(lmk) = projectLmk(Rob,Sen,Lmk(lmk),Obs(lmk),Opt);
            end

            Obs(lmk) = observationInnovation(Obs(lmk),Opt.correct.lines.innType);

            % 5. CHECK CONSISTENCE
            if Obs(lmk).inn.MD2 < Opt.correct.MD2th

                % Update Lmk inlier counter
                Lmk(lmk).nInlier = Lmk(lmk).nInlier + 1;

                % 6. LANDMARK CORRECTION AND 7. REPARAMETRIZATION
                % fully correct landmark - EKF, reparam. and off-filter
                [Rob,Sen,Lmk(lmk),Obs(lmk)] = correctLmk(...
                    Rob,      ...
                    Sen,      ...
                    Lmk(lmk), ...
                    Obs(lmk), ...
                    Opt          );

            else % obs is inconsistent - do not update

                Obs(lmk).updated = false;

            end % if consistent

        end % if matched

    end % for lmk = lmkList

    % 8. LANDMARK DELETION -- loop all visible
    for lmk = find([Obs.vis])

        [Lmk(lmk),Obs(lmk)] = smartDeleteLmk(Lmk(lmk),Obs(lmk));

    end

    % 9. FORCE COVARIANCE SYMMETRY
    Map.P(Map.used,Map.used) = (Map.P(Map.used,Map.used) + Map.P(Map.used,Map.used)')/2;

end % if any(vis)

end % function

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


classdef LvdGeometry < matlab.mixin.SetGet
    %LvdGeometry Summary of this class goes here
    %   Detailed explanation goes here

    properties
        points GeometricPointSet = GeometricPointSet.empty(1,0)
        vectors GeometricVectorSet = GeometricVectorSet.empty(1,0)
        coordSyses GeometricCoordSysSet = GeometricCoordSysSet.empty(1,0)
        refFrames GeometricRefFrameSet = GeometricRefFrameSet.empty(1,0)
        angles GeometricAngleSet = GeometricAngleSet.empty(1,0);
        planes GeometricPlaneSet = GeometricPlaneSet.empty(1,0);

        lvdData LvdData
    end

    methods
        function obj = LvdGeometry(lvdData)
            obj.lvdData = lvdData;

            obj.points = GeometricPointSet(lvdData);
            obj.vectors = GeometricVectorSet(lvdData);
            obj.coordSyses = GeometricCoordSysSet(lvdData);
            obj.refFrames = GeometricRefFrameSet(lvdData);
            obj.angles = GeometricAngleSet(lvdData);
            obj.planes = GeometricPlaneSet(lvdData);
        end

        function tf = usesGroundObj(obj, groundObj)
            tf = false;
            tf = tf || obj.points.usesGroundObj(groundObj);
            tf = tf || obj.vectors.usesGroundObj(groundObj);
            tf = tf || obj.coordSyses.usesGroundObj(groundObj);
            tf = tf || obj.refFrames.usesGroundObj(groundObj);
            tf = tf || obj.angles.usesGroundObj(groundObj);
        end

        function tf = usesGeometricPoint(obj, point)
            tf = false;
            tf = tf || obj.points.usesGeometricPoint(point);
            tf = tf || obj.vectors.usesGeometricPoint(point);
            tf = tf || obj.coordSyses.usesGeometricPoint(point);
            tf = tf || obj.refFrames.usesGeometricPoint(point);
            tf = tf || obj.angles.usesGeometricPoint(point);
        end

        function tf = usesGeometricVector(obj, vector)
            tf = false;
            tf = tf || obj.points.usesGeometricVector(vector);
            tf = tf || obj.vectors.usesGeometricVector(vector);
            tf = tf || obj.coordSyses.usesGeometricVector(vector);
            tf = tf || obj.refFrames.usesGeometricVector(vector);
            tf = tf || obj.angles.usesGeometricVector(vector);
        end

        function tf = usesGeometricCoordSys(obj, coordSys)
            tf = false;
            tf = tf || obj.points.usesGeometricCoordSys(coordSys);
            tf = tf || obj.vectors.usesGeometricCoordSys(coordSys);
            tf = tf || obj.coordSyses.usesGeometricCoordSys(coordSys);
            tf = tf || obj.refFrames.usesGeometricCoordSys(coordSys);
            tf = tf || obj.angles.usesGeometricCoordSys(coordSys);
        end

        function tf = usesGeometricRefFrame(obj, refFrame)
            tf = false;
            tf = tf || obj.points.usesGeometricRefFrame(refFrame);
            tf = tf || obj.vectors.usesGeometricRefFrame(refFrame);
            tf = tf || obj.coordSyses.usesGeometricRefFrame(refFrame);
            tf = tf || obj.refFrames.usesGeometricRefFrame(refFrame);
            tf = tf || obj.angles.usesGeometricRefFrame(refFrame);
        end

        function tf = usesGeometricAngle(obj, angle)
            tf = false;
            tf = tf || obj.points.usesGeometricAngle(angle);
            tf = tf || obj.vectors.usesGeometricAngle(angle);
            tf = tf || obj.coordSyses.usesGeometricAngle(angle);
            tf = tf || obj.refFrames.usesGeometricAngle(angle);
            tf = tf || obj.angles.usesGeometricAngle(angle);
        end

        function tf = usesGeometricPlane(obj, plane)
            tf = false;
            tf = tf || obj.points.usesGeometricPlane(plane);
            tf = tf || obj.vectors.usesGeometricPlane(plane);
            tf = tf || obj.coordSyses.usesGeometricPlane(plane);
            tf = tf || obj.refFrames.usesGeometricPlane(plane);
            tf = tf || obj.angles.usesGeometricPlane(plane);
            tf = tf || obj.planes.usesGeometricPlane(plane);
        end
    end

    methods(Static)
        function obj = loadobj(obj)
            if(isempty(obj.points))
                obj.points = GeometricPointSet(obj.lvdData);
            end

            if(isempty(obj.vectors))
                obj.vectors = GeometricVectorSet(obj.lvdData);
            end

            if(isempty(obj.coordSyses))
                obj.coordSyses = GeometricCoordSysSet(obj.lvdData);
            end

            if(isempty(obj.refFrames))
                obj.refFrames = GeometricRefFrameSet(obj.lvdData);
            end

            if(isempty(obj.angles))
                obj.angles = GeometricAngleSet(obj.lvdData);
            end

            if(isempty(obj.planes))
                obj.planes = GeometricPlaneSet(obj.lvdData);
            end
        end
    end
end


function out = isfinite(f)
%ISFINITE   Test if a CLASSICFUN is bounded.
%   ISFINITE(F) returns FALSE if F has any infinite values and TRUE otherwise.
%
% See also ISINF, ISNAN.

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

% Check if the ONEFUN of f is finite:
out = isfinite(f.onefun);

end


function mpc = case14
%CASE14    Power flow data for IEEE 14 bus test case.
%   Please see CASEFORMAT for details on the case file format.
%   This data was converted from IEEE Common Data Format
%   (ieee14cdf.txt) on 15-Oct-2014 by cdf2matp, rev. 2393
%   See end of file for warnings generated during conversion.
%
%   Converted from IEEE CDF file from:
%       https://labs.ece.uw.edu/pstca/
%
%  08/19/93 UW ARCHIVE           100.0  1962 W IEEE 14 Bus Test Case

%   MATPOWER

%% MATPOWER Case Format : Version 2
mpc.version = '2';

%%-----  Power Flow Data  -----%%
%% system MVA base
mpc.baseMVA = 100;

%% bus data
%	bus_i	type	Pd	Qd	Gs	Bs	area	Vm	Va	baseKV	zone	Vmax	Vmin
mpc.bus = [
	1	3	0	0	0	0	1	1.06	0	0	1	1.06	0.94;
	2	2	21.7	12.7	0	0	1	1.045	-4.98	0	1	1.06	0.94;
	3	2	94.2	19	0	0	1	1.01	-12.72	0	1	1.06	0.94;
	4	1	47.8	-3.9	0	0	1	1.019	-10.33	0	1	1.06	0.94;
	5	1	7.6	1.6	0	0	1	1.02	-8.78	0	1	1.06	0.94;
	6	2	11.2	7.5	0	0	1	1.07	-14.22	0	1	1.06	0.94;
	7	1	0	0	0	0	1	1.062	-13.37	0	1	1.06	0.94;
	8	2	0	0	0	0	1	1.09	-13.36	0	1	1.06	0.94;
	9	1	29.5	16.6	0	19	1	1.056	-14.94	0	1	1.06	0.94;
	10	1	9	5.8	0	0	1	1.051	-15.1	0	1	1.06	0.94;
	11	1	3.5	1.8	0	0	1	1.057	-14.79	0	1	1.06	0.94;
	12	1	6.1	1.6	0	0	1	1.055	-15.07	0	1	1.06	0.94;
	13	1	13.5	5.8	0	0	1	1.05	-15.16	0	1	1.06	0.94;
	14	1	14.9	5	0	0	1	1.036	-16.04	0	1	1.06	0.94;
];

%% generator data
%	bus	Pg	Qg	Qmax	Qmin	Vg	mBase	status	Pmax	Pmin	Pc1	Pc2	Qc1min	Qc1max	Qc2min	Qc2max	ramp_agc	ramp_10	ramp_30	ramp_q	apf
mpc.gen = [
	1	232.4	-16.9	10	0	1.06	100	1	332.4	0	0	0	0	0	0	0	0	0	0	0	0;
	2	40	42.4	50	-40	1.045	100	1	140	0	0	0	0	0	0	0	0	0	0	0	0;
	3	0	23.4	40	0	1.01	100	1	100	0	0	0	0	0	0	0	0	0	0	0	0;
	6	0	12.2	24	-6	1.07	100	1	100	0	0	0	0	0	0	0	0	0	0	0	0;
	8	0	17.4	24	-6	1.09	100	1	100	0	0	0	0	0	0	0	0	0	0	0	0;
];

%% branch data
%	fbus	tbus	r	x	b	rateA	rateB	rateC	ratio	angle	status	angmin	angmax
mpc.branch = [
	1	2	0.01938	0.05917	0.0528	0	0	0	0	0	1	-360	360;
	1	5	0.05403	0.22304	0.0492	0	0	0	0	0	1	-360	360;
	2	3	0.04699	0.19797	0.0438	0	0	0	0	0	1	-360	360;
	2	4	0.05811	0.17632	0.034	0	0	0	0	0	1	-360	360;
	2	5	0.05695	0.17388	0.0346	0	0	0	0	0	1	-360	360;
	3	4	0.06701	0.17103	0.0128	0	0	0	0	0	1	-360	360;
	4	5	0.01335	0.04211	0	0	0	0	0	0	1	-360	360;
	4	7	0	0.20912	0	0	0	0	0.978	0	1	-360	360;
	4	9	0	0.55618	0	0	0	0	0.969	0	1	-360	360;
	5	6	0	0.25202	0	0	0	0	0.932	0	1	-360	360;
	6	11	0.09498	0.1989	0	0	0	0	0	0	1	-360	360;
	6	12	0.12291	0.25581	0	0	0	0	0	0	1	-360	360;
	6	13	0.06615	0.13027	0	0	0	0	0	0	1	-360	360;
	7	8	0	0.17615	0	0	0	0	0	0	1	-360	360;
	7	9	0	0.11001	0	0	0	0	0	0	1	-360	360;
	9	10	0.03181	0.0845	0	0	0	0	0	0	1	-360	360;
	9	14	0.12711	0.27038	0	0	0	0	0	0	1	-360	360;
	10	11	0.08205	0.19207	0	0	0	0	0	0	1	-360	360;
	12	13	0.22092	0.19988	0	0	0	0	0	0	1	-360	360;
	13	14	0.17093	0.34802	0	0	0	0	0	0	1	-360	360;
];

%%-----  OPF Data  -----%%
%% generator cost data
%	1	startup	shutdown	n	x1	y1	...	xn	yn
%	2	startup	shutdown	n	c(n-1)	...	c0
mpc.gencost = [
	2	0	0	3	0.0430292599	20	0;
	2	0	0	3	0.25	20	0;
	2	0	0	3	0.01	40	0;
	2	0	0	3	0.01	40	0;
	2	0	0	3	0.01	40	0;
];

%% bus names
mpc.bus_name = {
	'Bus 1     HV';
	'Bus 2     HV';
	'Bus 3     HV';
	'Bus 4     HV';
	'Bus 5     HV';
	'Bus 6     LV';
	'Bus 7     ZV';
	'Bus 8     TV';
	'Bus 9     LV';
	'Bus 10    LV';
	'Bus 11    LV';
	'Bus 12    LV';
	'Bus 13    LV';
	'Bus 14    LV';
};

% Warnings from cdf2matp conversion:
%
% ***** check the title format in the first line of the cdf file.
% ***** Qmax = Qmin at generator at bus    1 (Qmax set to Qmin + 10)
% ***** MVA limit of branch 1 - 2 not given, set to 0
% ***** MVA limit of branch 1 - 5 not given, set to 0
% ***** MVA limit of branch 2 - 3 not given, set to 0
% ***** MVA limit of branch 2 - 4 not given, set to 0
% ***** MVA limit of branch 2 - 5 not given, set to 0
% ***** MVA limit of branch 3 - 4 not given, set to 0
% ***** MVA limit of branch 4 - 5 not given, set to 0
% ***** MVA limit of branch 4 - 7 not given, set to 0
% ***** MVA limit of branch 4 - 9 not given, set to 0
% ***** MVA limit of branch 5 - 6 not given, set to 0
% ***** MVA limit of branch 6 - 11 not given, set to 0
% ***** MVA limit of branch 6 - 12 not given, set to 0
% ***** MVA limit of branch 6 - 13 not given, set to 0
% ***** MVA limit of branch 7 - 8 not given, set to 0
% ***** MVA limit of branch 7 - 9 not given, set to 0
% ***** MVA limit of branch 9 - 10 not given, set to 0
% ***** MVA limit of branch 9 - 14 not given, set to 0
% ***** MVA limit of branch 10 - 11 not given, set to 0
% ***** MVA limit of branch 12 - 13 not given, set to 0
% ***** MVA limit of branch 13 - 14 not given, set to 0


function Y=GeneLableY(TrainLabel,Num)

N=length(TrainLabel);
Y=zeros(Num,N);

for i=1:N
    Y(TrainLabel(:,i),i)=1;
end
end


function test_bug1956

% WALLTIME 02:00:00
% MEM 3gb
% DEPENDENCY ft_volumesegment ft_prepare_sourcemodel volumesmooth

mri = ft_read_mri(dccnpath('/home/common/matlab/fieldtrip/data/ftp/test/ctf/Subject01.mri'));
mri.coordsys = 'ctf'; % this can also be determined with ft_determine_coordsys

cfg = [];
tpm = ft_volumesegment(cfg,mri);  % gray, white, csf tissue prob. map

% there are some spm functions which change their own input

cfg = [];
cfg.mri = tpm;
grid01 = ft_prepare_sourcemodel(cfg);

cfg = [];
cfg.mri = tpm;
grid02 = ft_prepare_sourcemodel(cfg);

cfg = [];
cfg.mri = tpm;
grid03 = ft_prepare_sourcemodel(cfg);

grid01 = rmfield(grid01, 'cfg');
grid02 = rmfield(grid02, 'cfg');
grid03 = rmfield(grid03, 'cfg');

% make sure that they are the same, and not super-smoohted
assert(isequal(grid01,grid02))
assert(isequal(grid01,grid03))


caffe.reset_all();
caffe.set_mode_gpu();
gpu_id = 0;  % we will use the first gpu in this demo
caffe.set_device(gpu_id);

batch_size = 50;
feature_dim = 512;
mean_value = 128;
scale = 0.0078125;
ROIx = 1:96;
ROIy = 1:112;
height = length(ROIx);
width = length(ROIy);

feature_suffix = '_hard64nofd';
aligned_image_folder = 'F:\datasets\MegaFace\FaceScrub\aligned\';
feature_folder = ['F:\datasets\MegaFace\FaceScrub\feature' feature_suffix '\'];
if exist(feature_folder, 'dir')==0
    mkdir(feature_folder);
end;

test_list_file = 'E:\datasets\MegaFace\devkit\templatelists\facescrub_uncropped_features_list.json';
json_string = fileread(test_list_file);
json_string = json_string(strfind(json_string,'path')+8:end);
test_list = regexp(json_string(8:end), '"(.*?)"','tokens');
test_list_raw = cell(length(test_list),1);
for i=1:length(test_list)
    test_list_raw{i} = [aligned_image_folder test_list{i}{1}];
    dot_pos = strfind(test_list{i}{1},'.');
    if isempty(dot_pos)||(dot_pos(end) ~= length(test_list{i}{1}) - 3&&dot_pos(end) ~= length(test_list{i}{1}) - 4)
%         disp(test_list{i}{1});
        test_list{i}{1} = [test_list{i}{1} '.jpg'];
    end;
    if ~isempty(dot_pos) && strcmp(test_list{i}{1}(dot_pos(end):end), '.gif')
        test_list{i}{1} = [test_list{i}{1}(1:end-3) 'jpg'];
    end;
    test_list{i} = [aligned_image_folder test_list{i}{1}];
end;

total_image = length(test_list);
total_iter = ceil(total_image / batch_size);

net = caffe.Net('D:\face project\experiment\96_112_l2_distance\face_deploy_64.prototxt','D:\face project\norm_face_model\hard-margin-64-nofd.caffemodel', 'test');%face_model_my

image_p = 1;
for i=1:total_iter
    fprintf('%d/%d\n',i, total_iter);
    J = zeros(height,width,3,batch_size,'single');
    feature_names = cell(batch_size,1);
    for j = 1 : batch_size
        if image_p <= total_image
            I = imread(test_list{image_p});
            I = permute(I,[2 1 3]);
            I = I(:,:,[3 2 1]);
            I = I(ROIx,ROIy,:);
            I = single(I) - mean_value;
            J(:,:,:,j) = I*scale;
            feature_names{j} = [strrep(test_list_raw{image_p},aligned_image_folder, feature_folder) feature_suffix '.bin'];
            image_p = image_p + 1;
        end;
    end;
    f1 = net.forward({J});
    feature = squeeze(f1{1});
    for j = 1 : batch_size
        if ~isempty(feature_names{j})
            [file_folder, file_name, file_ext] = fileparts(feature_names{j});
            if exist(file_folder,'dir')==0
                mkdir(file_folder);
            end;
            fp = fopen(feature_names{j},'wb');
            fwrite(fp, [feature_dim 1 4 5], 'int32');
            fwrite(fp, feature(:,j), 'float32');
            fclose(fp);
        end;
    end;
end;


function [ripples,sd,bad] = FindRipples(filtered,varargin)

%FindRipples - Find hippocampal ripples (100~200Hz oscillations).
%
%  USAGE
%
%    [ripples,stdev,noise] = FindRipples(filtered,<options>)
%
%    Ripples are detected using the normalized squared signal (NSS) by
%    thresholding the baseline, merging neighboring events, thresholding
%    the peaks, and discarding events with excessive duration.
%    Thresholds are computed as multiples of the standard deviation of
%    the NSS. Alternatively, one can use explicit values, typically obtained
%    from a previous call.
%
%    filtered       ripple-band filtered LFP (one channel).
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'thresholds'  thresholds for ripple beginning/end and peak, in multiples
%                   of the stdev (default = [2 5])
%     'durations'   minimum inter-ripple interval, and minimum and maximum
%                   ripple durations, in ms (default = [30 20 100])
%     'baseline'    interval used to compute normalization (default = all)
%     'restrict'    same as 'baseline' (for backwards compatibility)
%     'frequency'   sampling rate (in Hz) (default = 1250Hz)
%     'stdev'       reuse previously computed stdev
%     'show'        plot results (default = 'off')
%     'noise'       noisy ripple-band filtered channel used to exclude ripple-
%                   like noise (events also present on this channel are
%                   discarded)
%    =========================================================================
%
%  OUTPUT
%
%    ripples        for each ripple, [start_t peak_t end_t peakNormalizedPower]
%    stdev          standard deviation of the NSS (can be reused subsequently)
%    noise          ripple-like activity recorded simultaneously on the noise
%                   channel (for debugging info)
%
%  SEE
%
%    See also FilterLFP, RippleStats, SaveRippleEvents, PlotRippleStats.

% Copyright (C) 2004-2011 by Michaël Zugaro, initial algorithm by Hajime Hirase
%
% 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.

% Default values
frequency = 1250;
show = 'off';
restrict = [];
sd = [];
lowThresholdFactor = 2; % Ripple envoloppe must exceed lowThresholdFactor*stdev
highThresholdFactor = 5; % Ripple peak must exceed highThresholdFactor*stdev
minInterRippleInterval = 30; % in ms
minRippleDuration = 20; % in ms
maxRippleDuration = 100; % in ms
noise = [];

% Check number of parameters
if nargin < 1 | mod(length(varargin),2) ~= 0,
  error('Incorrect number of parameters (type ''help FindRipples'' for details).');
end

% Check parameter sizes
if ~isdmatrix(filtered) | size(filtered,2) ~= 2,
	error('Parameter ''filtered'' is not a Nx2 matrix (type ''help FindRipples'' for details).');
end

% Parse parameter list
for i = 1:2:length(varargin),
	if ~ischar(varargin{i}),
		error(['Parameter ' num2str(i+2) ' is not a property (type ''help FindRipples'' for details).']);
	end
	switch(lower(varargin{i})),
		case 'thresholds',
			thresholds = varargin{i+1};
			if ~isdvector(thresholds,'#2','>0'),
				error('Incorrect value for property ''thresholds'' (type ''help FindRipples'' for details).');
			end
			lowThresholdFactor = thresholds(1);
			highThresholdFactor = thresholds(2);
		case 'durations',
			durations = varargin{i+1};
			if ~isdvector(durations,'#2','>0') && ~isdvector(durations,'#3','>0'),
				error('Incorrect value for property ''durations'' (type ''help FindRipples'' for details).');
			end
			if length(durations) == 2,
				minInterRippleInterval = durations(1);
				maxRippleDuration = durations(2);
			else
				minInterRippleInterval = durations(1);
				minRippleDuration = durations(2);
				maxRippleDuration = durations(3);
			end
		case 'frequency',
			frequency = varargin{i+1};
			if ~isdscalar(frequency,'>0'),
				error('Incorrect value for property ''frequency'' (type ''help FindRipples'' for details).');
			end
		case 'show',
			show = varargin{i+1};
			if ~isstring(show,'on','off'),
				error('Incorrect value for property ''show'' (type ''help FindRipples'' for details).');
			end
		case {'baseline','restrict'},
			restrict = varargin{i+1};
			if ~isempty(restrict) & ~isdvector(restrict,'#2','<'),
				error('Incorrect value for property ''restrict'' (type ''help FindRipples'' for details).');
			end
		case 'stdev',
			sd = varargin{i+1};
			if ~isdscalar(sd,'>0'),
				error('Incorrect value for property ''stdev'' (type ''help FindRipples'' for details).');
			end
		case 'noise',
			noise = varargin{i+1};
			if ~isdmatrix(noise) | size(noise,1) ~= size(filtered,1) | size(noise,2) ~= 2,
				error('Incorrect value for property ''noise'' (type ''help FindRipples'' for details).');
			end
		otherwise,
			error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help FindRipples'' for details).']);
	end
end

% Parameters
windowLength = round(frequency/1250*11);

% Square and normalize signal
signal = filtered(:,2);
squaredSignal = signal.^2;
window = ones(windowLength,1)/windowLength;
keep = [];
if ~isempty(restrict),
	keep = filtered(:,1)>=restrict(1)&filtered(:,1)<=restrict(2);
end

[normalizedSquaredSignal,sd] = unity(Filter0(window,sum(squaredSignal,2)),sd,keep);

% Detect ripple periods by thresholding normalized squared signal
thresholded = normalizedSquaredSignal > lowThresholdFactor;
start = find(diff(thresholded)>0);
stop = find(diff(thresholded)<0);
% Exclude last ripple if it is incomplete
if length(stop) == length(start)-1,
	start = start(1:end-1);
end
% Exclude first ripple if it is incomplete
if length(stop)-1 == length(start),
    stop = stop(2:end);
end
% Correct special case when both first and last ripples are incomplete
if start(1) > stop(1),
	stop(1) = [];
	start(end) = [];
end
firstPass = [start,stop];
if isempty(firstPass),
	disp('Detection by thresholding failed');
	return
else
	disp(['After detection by thresholding: ' num2str(length(firstPass)) ' events.']);
end

% Merge ripples if inter-ripple period is too short
minInterRippleSamples = minInterRippleInterval/1000*frequency;
secondPass = [];
ripple = firstPass(1,:);
for i = 2:size(firstPass,1)
	if firstPass(i,1) - ripple(2) < minInterRippleSamples,
		% Merge
		ripple = [ripple(1) firstPass(i,2)];
	else
		secondPass = [secondPass ; ripple];
		ripple = firstPass(i,:);
	end
end
secondPass = [secondPass ; ripple];
if isempty(secondPass),
	disp('Ripple merge failed');
	return
else
	disp(['After ripple merge: ' num2str(length(secondPass)) ' events.']);
end

% Discard ripples with a peak power < highThresholdFactor
thirdPass = [];
peakNormalizedPower = [];
for i = 1:size(secondPass,1)
	[maxValue,maxIndex] = max(normalizedSquaredSignal([secondPass(i,1):secondPass(i,2)]));
	if maxValue > highThresholdFactor,
		thirdPass = [thirdPass ; secondPass(i,:)];
		peakNormalizedPower = [peakNormalizedPower ; maxValue];
	end
end
if isempty(thirdPass),
	disp('Peak thresholding failed.');
	return
else
	disp(['After peak thresholding: ' num2str(length(thirdPass)) ' events.']);
end

% Detect negative peak position for each ripple
peakPosition = zeros(size(thirdPass,1),1);
for i=1:size(thirdPass,1),
	[minValue,minIndex] = min(signal(thirdPass(i,1):thirdPass(i,2)));
	peakPosition(i) = minIndex + thirdPass(i,1) - 1;
end

% Discard ripples that are way too short
time = filtered(:,1);
ripples = [time(thirdPass(:,1)) time(peakPosition) time(thirdPass(:,2)) peakNormalizedPower];
duration = ripples(:,3)-ripples(:,1);
ripples(duration<minRippleDuration/1000,:) = [];
disp(['After min duration test: ' num2str(size(ripples,1)) ' events.']);

% Discard ripples that are way too long
duration = ripples(:,3)-ripples(:,1);
ripples(duration>maxRippleDuration/1000,:) = [];
disp(['After max duration test: ' num2str(size(ripples,1)) ' events.']);

% If a noisy channel was provided, find ripple-like events and exclude them
bad = [];
if ~isempty(noise),
	% Square and pseudo-normalize (divide by signal stdev) noise
	squaredNoise = noise(:,2).^2;
	window = ones(windowLength,1)/windowLength;
	normalizedSquaredNoise = unity(Filter0(window,sum(squaredNoise,2)),sd,[]);
	excluded = logical(zeros(size(ripples,1),1));
	% Exclude ripples when concomittent noise crosses high detection threshold
	previous = 1;
	for i = 1:size(ripples,1),
		j = FindInInterval(noise,[ripples(i,1),ripples(i,3)],previous);
		previous = j(2);
		if any(normalizedSquaredNoise(j(1):j(2))>highThresholdFactor),
			excluded(i) = 1;
		end
	end
	bad = ripples(excluded,:);
	ripples = ripples(~excluded,:);
	disp(['After noise removal: ' num2str(size(ripples,1)) ' events.']);
end

% Optionally, plot results
if strcmp(show,'on'),
	figure;
	if ~isempty(noise),
		MultiPlotXY([time signal],[time squaredSignal],[time normalizedSquaredSignal],[time noise(:,2)],[time squaredNoise],[time normalizedSquaredNoise]);
		nPlots = 6;
		subplot(nPlots,1,3);
 		ylim([0 highThresholdFactor*1.1]);
		subplot(nPlots,1,6);
  		ylim([0 highThresholdFactor*1.1]);
	else
		MultiPlotXY([time signal],[time squaredSignal],[time normalizedSquaredSignal]);
%  		MultiPlotXY(time,signal,time,squaredSignal,time,normalizedSquaredSignal);
		nPlots = 3;
		subplot(nPlots,1,3);
  		ylim([0 highThresholdFactor*1.1]);
	end
	for i = 1:nPlots,
		subplot(nPlots,1,i);
		hold on;
  		yLim = ylim;
		for j=1:size(ripples,1),
			plot([ripples(j,1) ripples(j,1)],yLim,'g-');
			plot([ripples(j,2) ripples(j,2)],yLim,'k-');
			plot([ripples(j,3) ripples(j,3)],yLim,'r-');
			if i == 3,
				plot([ripples(j,1) ripples(j,3)],[ripples(j,4) ripples(j,4)],'k-');
			end
		end
		for j=1:size(bad,1),
			plot([bad(j,1) bad(j,1)],yLim,'k-');
			plot([bad(j,2) bad(j,2)],yLim,'k-');
			plot([bad(j,3) bad(j,3)],yLim,'k-');
			if i == 3,
				plot([bad(j,1) bad(j,3)],[bad(j,4) bad(j,4)],'k-');
			end
		end
		if mod(i,3) == 0,
			plot(xlim,[lowThresholdFactor lowThresholdFactor],'k','linestyle','--');
			plot(xlim,[highThresholdFactor highThresholdFactor],'k-');
		end
	end
end

function y = Filter0(b,x)

if size(x,1) == 1,
	x = x(:);
end

if mod(length(b),2) ~= 1,
	error('filter order should be odd');
end

shift = (length(b)-1)/2;

[y0 z] = filter(b,1,x);

y = [y0(shift+1:end,:) ; z(1:shift,:)];

function [U,stdA] = unity(A,sd,restrict)

if ~isempty(restrict),
	meanA = mean(A(restrict));
	stdA = std(A(restrict));
else
	meanA = mean(A);
	stdA = std(A);
end
if ~isempty(sd),
	stdA = sd;
end

U = (A - meanA)/stdA;


function [ Iframe, Annotation, I, bbox_orig, bbox, OrigPoints ] = getImageFrame( options, idx, annotation_struct )
%GETIMAGEFRAME return normalized frame and its annotation
%   Given image from db and detected frame of face on it this function
%   expands detected frame by options.bw_margin and than crops image to
%   this frame and rescales it to size of options.bw (base window)
%
% INPUT
%
%   options     ...     structure containing parameters - bw (base_window),
%                       bw_margin (base window margin), components (marix with bboxes of components),
%                       image_path (path to directory containing jpg files of faces)
%   idx         ...     index of image in db
%   image       ...     cell image with filename and  detected frame
%
% OUTPUT
%
%   Iframe      ...     detected frame normalized to bw size after
%                       expansion of bw_margin in both dimensions
%   Annotation  ...     structure with annotation in similar format as
%                       original XML annotation
%   I           ...     original Image (rgb)
%   bbox_orig   ...     original bbox of detected face
%   bbox        ...     extended bbox by options.bw_margin
%   OrigPoints  ...     original landmarks (in unnormalized image)
%
% 02-08-10 Michal Uricar
% 20-10-10 Michal Uricar, normalized image frame get by getNormalizedFrame.m
% 12-07-11 Michal Uricar, corners dataset
% 21-03-12 Michal Uricar, LFW annotation in one file

    M = 10;
    fname = strtok(annotation_struct.names{idx}, '.');
    bbox_orig = double(annotation_struct.bbox(idx, :));
    filename = [options.image_path annotation_struct.names{idx}];
    I = rgb2gray(imread(filename));

    % get normalized image frame
    try
        [Iframe, bbox] = getNormalizedFrame(I, bbox_orig, options);
    catch
        Iframe = [];
        Annotation = [];
        bbox = [];
        OrigPoints = [];
        return;
    end;

    % compute coordinates of landmarks in normalized frame
    OrigPoints = [annotation_struct.eye_r(idx, :)' annotation_struct.eye_l(idx, :)' ,...
        annotation_struct.canthus_rr(idx, :)' annotation_struct.canthus_rl(idx, :)' ,...
        annotation_struct.canthus_lr(idx, :)' annotation_struct.canthus_ll(idx, :)' ,...
        annotation_struct.mouth(idx, :)' annotation_struct.mouth_corner_r(idx, :)'  ,...
        annotation_struct.mouth_corner_l(idx, :)' annotation_struct.nose(idx, :)'];
    O = [bbox(1); bbox(2)];
    scalefac = [ options.bw(1)/(bbox(3)-bbox(1)+1); options.bw(2)/(bbox(4)-bbox(2)+1) ];

    if (size(OrigPoints, 2) ~= M)
        error('??? Count of landmarks does not equal M. Corrupted xml annotation?');
        Annotation = [];
        fprintf('Bad annotation found.\n');
        return;
    end;

    % save annotation
    Points = zeros(2, M);
    for j = 1 : M
        Points(:, j) = (OrigPoints(:, j) - O) .* scalefac;
    end;

    if (numel(Points(Points < 0)) > 0)
        Annotation = [];
        fprintf('Bad annotation found.\n');
        return;
    end;

    [h w d] = size(Iframe);

    Annotation.P = round(Points);
    Annotation.image.filename = [fname '.jpg'];
    Annotation.image.width = w;
    Annotation.image.height = h;
end


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: May 27th, 2015
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: This is where you can perform data analysis of a simulation
%           from your stored viz_IB2d information
%
%     Note:
%           (1) USER-DEFINED functions should be made made to analyze specific
%               data sets, regions of interest, etc. (see
%               EXAMPLE_FOR_DATA_ANALYSIS for an example of this)
%           (2) MUST make sure to 'addpath' to where DA_Blackbox is, i.e.,
%               line 56
%           (3) MUST make sure to set path to desired dataset, i.e., in line
%               59
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Insect_Analysis()

% TEMPORAL INFO FROM input2d %
dt = 2.5e-5;      % Time-step
Tfinal = 0.05;   % Final time in simulation
pDump=40;        % Note: 'print_dump' should match from input2d

% DATA ANALYSIS INFO %
start=1;                             % 1ST interval # included in data analysis
finish=25;                           % LAST interval # included in data analysis
dump_Times = (start:1:finish)*pDump; % Time vector when data was printed in analysis

% SET PATH TO DESIRED viz_IB2d DATA %
pathMain = '/Users/nick_battista/Desktop/IB2d/matIB2d/Examples/Example_Insect_Flight/Insect_1024x1024';

% SET PATH TO DA_BLACKBOX %
addpath('/Users/nick_battista/Desktop/IB2d/data_analysis/analysis_in_matlab/DA_Blackbox');

pathViz = [pathMain '/viz_IB2d/'];
pathForce = [pathMain '/hier_IB2d_data'];

%simVec = {'32x32','64x64','96x96','128x128','256x256','512x512','768x768','1024x1024'};

for i=start:1:finish

    % Points to desired data viz_IB2d data file
    if i<10
       numSim = ['000', num2str(i) ];
    elseif i<100
       numSim = ['00', num2str(i) ];
    elseif i<1000
       numSim = ['0', num2str(i)];
    else
       numSim = num2str(i);
    end

    % Imports immersed boundary positions %
    %[xLag,yLag] = give_Lag_Positions(pathViz,numSim);

    % Imports (x,y) grid values and ALL Eulerian Data %
    %                      DEFINITIONS
    %          x: x-grid                y: y-grid
    %       Omega: vorticity           P: pressure
    %    uMag: mag. of velocity
    %    uX: mag. of x-Velocity   uY: mag. of y-Velocity
    %    U: x-directed velocity   V: y-directed velocity
    %    Fx: x-directed Force     Fy: y-directed Force
    %
    %  Note: U(j,i): j-corresponds to y-index, i to the x-index
    %
    %[x,y,Omega,P,uMag,uX,uY,U,V,Fx,Fy] = import_Eulerian_Data(pathViz,numSim);

    % Imports Lagrangian Pt. FORCE (magnitude) DATA %
    %                      DEFINITIONS
    %
    %      fX_Lag: forces in x-direction on boundary
    %      fY_Lag: forces in y-direction on boundary
    %       fLagMag: magnitude of force at boundary
    %   fLagNorm: magnitude of NORMAL force at boundary
    %   fLagTan: magnitude of TANGENT force at boundary
    %
    [fX_Lag,fY_Lag] = import_Lagrangian_Force_Data_Insect(pathForce,numSim);

    fx_Mat(i,1) = mean( fX_Lag );
    fy_Mat(i,1) = mean( fY_Lag );

end

cd ..;

strName = 'fX_1024';
print_Matrix_To_Txt_File(fx_Mat,strName)

strName = 'fY_1024';
print_Matrix_To_Txt_File(fy_Mat,strName)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION prints matrix to file
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Matrix_To_Txt_File(a,strName)

nameTxt = [strName '.txt'];

fid = fopen(nameTxt, 'wt'); % Open for writing
for i=1:size(a,1)
   fprintf(fid, '%d ', a(i,:));
   fprintf(fid, '\n');
end
fclose(fid);


% model_setup_ui
% Run this to specify a design for optimization with GA2
%
% The script creates two struct variables, mspec and conditions,
% which are then used as inputs to ga2.m
%
% mspec and conditions contain fields, accessed with mspec.fieldname,
% that contain information about the design.
% Numbers in these variables describe timing info in TRs! (except the runtime and TR fields, which are in s)
%
% This function is good for:
%   1 - creating a design matrix with combinations of convolved, unconvolved, and shape-free elements
%   2 - creating designs AT THE RESOLUTION OF THE TR
%   3 - creating designs for optimization of temporal jitter and estimation of parts of a single trial
%   4 - creating designs with completely independent conditions (can overlap, etc.)
%
% This function is NOT as good for:
%   1 - optimizing the ordering of stimulus presentation for multiple trial types
%   2 - creating designs that have 1 or 2 events in some conditions.  confuses the program.
%   3 - creating designs in which trial types are mutually exclusive (present A or B at time t, but not both)

% scanlength, tr

% event, epoch, block
% convolve
% regular / random

% each condition is a trial type
% each trial type can be
%   event
%   epoch
%   multi-part trial

% each condition has these fields:
%   onsets      vector, for fixed onsets, times in s
%               [onset range; trial length range] for random onsets, in s
%               0 is first second of run
%
%   parts       epochs within a trial, for multipart trials
%               for each part, specify onsets (vector or range)
%               onsets can be an integer, for fixed onsets rel to trial or prev part.

% for multi-part
%   no of parts
%

clear mspec, clear conditions

conditions(1).onsets = 1;
i = 1;

disp(['Model setup user-interface: for specifying a design matrix.'])
disp(['by Tor Wager, version a1'])
disp(['___________________________________________________________'])

mspec.TR = input('Enter TR in s ');
mspec.runtime = input('Enter run length in s ');
mspec.hrf = spm_hrf(mspec.TR); mspec.hrf = mspec.hrf ./ max(mspec.hrf);
mspec.numframes = mspec.runtime ./ mspec.TR;

tmp = input('All trials at least N TRs apart? (Enter N or return to skip.) ');
if ~isempty(tmp), mspec.toverlap = [1 tmp];,end

disp(['Condition setup:'])
disp(['Press return when asked for onsets for a condition to exit.'])
disp(['___________________________________________________________'])

while ~isempty(conditions(i).onsets)

    conditions(i).onsets = input(['Onsets for condition ' num2str(i) ' in s ']);
    conditions(i).onsets = conditions(i).onsets ./ mspec.TR;

    if isempty(conditions(i).onsets), break, end

    conditions(i).parts = input(['Parts for condition ' num2str(i) ': enter 1 or num parts ']);
    % if this is a multi-part trial, parts > 1

        for j = 1:conditions(i).parts

            rel_to = input(['Onsets for condition ' num2str(i) ' part ' num2str(j) ' relative to: 1 for start of trial, 2 for prev. part ']);
            if rel_to == 1, str = ' start of trial ';, elseif rel_to == 2, str = ' previous part ';, else, error('Choose 1 or 2'), end

            if rel_to == 2 & j == 1, error('Cannot specify onset of 1st trial part relative to previous part.'),end
            conditions(i).subcond(j).rel_to = rel_to;
            conditions(i).subcond(j).onsets = input(['Onsets in s for condition ' num2str(i) ' part ' num2str(j) ' from' str]);
            if length(conditions(i).subcond(j).onsets) == 1,
                conditions(i).subcond(j).onsets = repmat(conditions(i).subcond(j).onsets,1,length(conditions(i).onsets));
            end
            conditions(i).subcond(j).onsets = conditions(i).subcond(j).onsets ./ mspec.TR;

            conditions(i).subcond(j).stimlength = input(['Stimulation length in TRs for condition ' num2str(i) ' part ' num2str(j) ' ']);

            conditions(i).subcond(j).convolve = input(['Convolve condition ' num2str(i) ' part ' num2str(j) ' with canonical HRF? 1/0 ']);
            if ~conditions(i).subcond(j).convolve
                conditions(i).subcond(j).hrfest = input(['Estimate shape-free HRF for condition ' num2str(i) ' part ' num2str(j) ' ? 1 or TP to estimate ']);
            else
                conditions(i).subcond(j).hrfest = [];
            end

        end

    if conditions(i).parts == 1, conditions(i).stimlength = conditions(i).subcond(1).stimlength;, end

    i = i + 1;
    conditions(i).onsets = 1;
end

conditions = conditions(1:end-1);