%  internal function

%  'xform_nii.m' is an internal function called by "load_nii.m", so
%  you do not need run this program by yourself. It does simplified
%  NIfTI sform/qform affine transform, and supports some of the
%  affine transforms, including translation, reflection, and
%  orthogonal rotation (N*90 degree).
%
%  For other affine transforms, e.g. any degree rotation, shearing
%  etc. you will have to use the included 'reslice_nii.m' program
%  to reslice the image volume. 'reslice_nii.m' is not called by
%  any other program, and you have to run 'reslice_nii.m' explicitly
%  for those NIfTI files that you want to reslice them.
%
%  Since 'xform_nii.m' does not involve any interpolation or any
%  slice change, the original image volume is supposed to be
%  untouched, although it is translated, reflected, or even
%  orthogonally rotated, based on the affine matrix in the
%  NIfTI header.
%
%  However, the affine matrix in the header of a lot NIfTI files
%  contain slightly non-orthogonal rotation. Therefore, optional
%  input parameter 'tolerance' is used to allow some distortion
%  in the loaded image for any non-orthogonal rotation or shearing
%  of NIfTI affine matrix. If you set 'tolerance' to 0, it means
%  that you do not allow any distortion. If you set 'tolerance' to
%  1, it means that you do not care any distortion. The image will
%  fail to be loaded if it can not be tolerated. The tolerance will
%  be set to 0.1 (10%), if it is default or empty.
%
%  Because 'reslice_nii.m' has to perform 3D interpolation, it can
%  be slow depending on image size and affine matrix in the header.
%
%  After you perform the affine transform, the 'nii' structure
%  generated from 'xform_nii.m' or new NIfTI file created from
%  'reslice_nii.m' will be in RAS orientation, i.e. X axis from
%  Left to Right, Y axis from Posterior to Anterior, and Z axis
%  from Inferior to Superior.
%
%  NOTE: This function should be called immediately after load_nii.
%
%  Usage: [ nii ] = xform_nii(nii, [tolerance], [preferredForm])
%
%  nii	- NIFTI structure (returned from load_nii)
%
%  tolerance (optional) - distortion allowed for non-orthogonal rotation
%	or shearing in NIfTI affine matrix. It will be set to 0.1 (10%),
%	if it is default or empty.
%
%  preferredForm (optional)  -  selects which transformation from voxels
%	to RAS coordinates; values are s,q,S,Q.  Lower case s,q indicate
%	"prefer sform or qform, but use others if preferred not present".
%	Upper case indicate the program is forced to use the specificied
%	tranform or fail loading.  'preferredForm' will be 's', if it is
%	default or empty.	- Jeff Gunter
%
%  NIFTI data format can be found on: http://nifti.nimh.nih.gov
%
%  - Jimmy Shen (jimmy@rotman-baycrest.on.ca)
%
function nii = xform_nii(nii, tolerance, preferredForm)

   %  save a copy of the header as it was loaded.  This is the
   %  header before any sform, qform manipulation is done.
   %
   nii.original.hdr = nii.hdr;

   if ~exist('tolerance','var') | isempty(tolerance)
      tolerance = 0.1;
   elseif(tolerance<=0)
      tolerance = eps;
   end

   if ~exist('preferredForm','var') | isempty(preferredForm)
      preferredForm= 's';				% Jeff
   end

   %  if scl_slope field is nonzero, then each voxel value in the
   %  dataset should be scaled as: y = scl_slope * x + scl_inter
   %  I bring it here because hdr will be modified by change_hdr.
   %
   if nii.hdr.dime.scl_slope ~= 0 & ...
	ismember(nii.hdr.dime.datatype, [2,4,8,16,64,256,512,768]) & ...
	(nii.hdr.dime.scl_slope ~= 1 | nii.hdr.dime.scl_inter ~= 0)

      nii.img = ...
	nii.hdr.dime.scl_slope * double(nii.img) + nii.hdr.dime.scl_inter;

      if nii.hdr.dime.datatype == 64

         nii.hdr.dime.datatype = 64;
         nii.hdr.dime.bitpix = 64;
      else
         nii.img = single(nii.img);

         nii.hdr.dime.datatype = 16;
         nii.hdr.dime.bitpix = 32;
      end

      nii.hdr.dime.glmax = max(double(nii.img(:)));
      nii.hdr.dime.glmin = min(double(nii.img(:)));

      %  set scale to non-use, because it is applied in xform_nii
      %
      nii.hdr.dime.scl_slope = 0;

   end

   %  However, the scaling is to be ignored if datatype is DT_RGB24.

   %  If datatype is a complex type, then the scaling is to be applied
   %  to both the real and imaginary parts.
   %
   if nii.hdr.dime.scl_slope ~= 0 & ...
	ismember(nii.hdr.dime.datatype, [32,1792])

      nii.img = ...
	nii.hdr.dime.scl_slope * double(nii.img) + nii.hdr.dime.scl_inter;

      if nii.hdr.dime.datatype == 32
         nii.img = single(nii.img);
      end

      nii.hdr.dime.glmax = max(double(nii.img(:)));
      nii.hdr.dime.glmin = min(double(nii.img(:)));

      %  set scale to non-use, because it is applied in xform_nii
      %
      nii.hdr.dime.scl_slope = 0;

   end

   %  There is no need for this program to transform Analyze data
   %
   if nii.filetype == 0 & exist([nii.fileprefix '.mat'],'file')
      load([nii.fileprefix '.mat']);	% old SPM affine matrix
      R=M(1:3,1:3);
      T=M(1:3,4);
      T=R*ones(3,1)+T;
      M(1:3,4)=T;
      nii.hdr.hist.qform_code=0;
      nii.hdr.hist.sform_code=1;
      nii.hdr.hist.srow_x=M(1,:);
      nii.hdr.hist.srow_y=M(2,:);
      nii.hdr.hist.srow_z=M(3,:);
   elseif nii.filetype == 0
      nii.hdr.hist.rot_orient = [];
      nii.hdr.hist.flip_orient = [];
      return;				% no sform/qform for Analyze format
   end

   hdr = nii.hdr;

   [hdr,orient]=change_hdr(hdr,tolerance,preferredForm);

   %  flip and/or rotate image data
   %
   if ~isequal(orient, [1 2 3])

      old_dim = hdr.dime.dim([2:4]);

      %  More than 1 time frame
      %
      if ndims(nii.img) > 3
         pattern = 1:prod(old_dim);
      else
         pattern = [];
      end

      if ~isempty(pattern)
         pattern = reshape(pattern, old_dim);
      end

      %  calculate for rotation after flip
      %
      rot_orient = mod(orient + 2, 3) + 1;

      %  do flip:
      %
      flip_orient = orient - rot_orient;

      for i = 1:3
         if flip_orient(i)
            if ~isempty(pattern)
               pattern = flipdim(pattern, i);
            else
               nii.img = flipdim(nii.img, i);
            end
         end
      end

      %  get index of orient (rotate inversely)
      %
      [tmp rot_orient] = sort(rot_orient);

      new_dim = old_dim;
      new_dim = new_dim(rot_orient);
      hdr.dime.dim([2:4]) = new_dim;

      new_pixdim = hdr.dime.pixdim([2:4]);
      new_pixdim = new_pixdim(rot_orient);
      hdr.dime.pixdim([2:4]) = new_pixdim;

      %  re-calculate originator
      %
      tmp = hdr.hist.originator([1:3]);
      tmp = tmp(rot_orient);
      flip_orient = flip_orient(rot_orient);

      for i = 1:3
         if flip_orient(i) & ~isequal(tmp(i), 0)
            tmp(i) = new_dim(i) - tmp(i) + 1;
         end
      end

      hdr.hist.originator([1:3]) = tmp;
      hdr.hist.rot_orient = rot_orient;
      hdr.hist.flip_orient = flip_orient;

      %  do rotation:
      %
      if ~isempty(pattern)
         pattern = permute(pattern, rot_orient);
         pattern = pattern(:);

         if hdr.dime.datatype == 32 | hdr.dime.datatype == 1792 | ...
		hdr.dime.datatype == 128 | hdr.dime.datatype == 511

            tmp = reshape(nii.img(:,:,:,1), [prod(new_dim) hdr.dime.dim(5:8)]);
            tmp = tmp(pattern, :);
            nii.img(:,:,:,1) = reshape(tmp, [new_dim       hdr.dime.dim(5:8)]);

            tmp = reshape(nii.img(:,:,:,2), [prod(new_dim) hdr.dime.dim(5:8)]);
            tmp = tmp(pattern, :);
            nii.img(:,:,:,2) = reshape(tmp, [new_dim       hdr.dime.dim(5:8)]);

            if hdr.dime.datatype == 128 | hdr.dime.datatype == 511
               tmp = reshape(nii.img(:,:,:,3), [prod(new_dim) hdr.dime.dim(5:8)]);
               tmp = tmp(pattern, :);
               nii.img(:,:,:,3) = reshape(tmp, [new_dim       hdr.dime.dim(5:8)]);
            end

         else
            nii.img = reshape(nii.img, [prod(new_dim) hdr.dime.dim(5:8)]);
            nii.img = nii.img(pattern, :);
            nii.img = reshape(nii.img, [new_dim       hdr.dime.dim(5:8)]);
         end
      else
         if hdr.dime.datatype == 32 | hdr.dime.datatype == 1792 | ...
		hdr.dime.datatype == 128 | hdr.dime.datatype == 511

            nii.img(:,:,:,1) = permute(nii.img(:,:,:,1), rot_orient);
            nii.img(:,:,:,2) = permute(nii.img(:,:,:,2), rot_orient);

            if hdr.dime.datatype == 128 | hdr.dime.datatype == 511
               nii.img(:,:,:,3) = permute(nii.img(:,:,:,3), rot_orient);
            end
         else
            nii.img = permute(nii.img, rot_orient);
         end
      end
   else
      hdr.hist.rot_orient = [];
      hdr.hist.flip_orient = [];
   end

   nii.hdr = hdr;

   return;					% xform_nii

%-----------------------------------------------------------------------
function [hdr, orient] = change_hdr(hdr, tolerance, preferredForm)

   orient = [1 2 3];
   affine_transform = 1;

   %  NIFTI can have both sform and qform transform. This program
   %  will check sform_code prior to qform_code by default.
   %
   %  If user specifys "preferredForm", user can then choose the
   %  priority.					- Jeff
   %
   useForm=[];					% Jeff

   if isequal(preferredForm,'S')
       if isequal(hdr.hist.sform_code,0)
           error('User requires sform, sform not set in header');
       else
           useForm='s';
       end
   end						% Jeff

   if isequal(preferredForm,'Q')
       if isequal(hdr.hist.qform_code,0)
           error('User requires qform, qform not set in header');
       else
           useForm='q';
       end
   end						% Jeff

   if isequal(preferredForm,'s')
       if hdr.hist.sform_code > 0
           useForm='s';
       elseif hdr.hist.qform_code > 0
           useForm='q';
       end
   end						% Jeff

   if isequal(preferredForm,'q')
       if hdr.hist.qform_code > 0
           useForm='q';
       elseif hdr.hist.sform_code > 0
           useForm='s';
       end
   end						% Jeff

   if isequal(useForm,'s')
      R = [hdr.hist.srow_x(1:3)
           hdr.hist.srow_y(1:3)
           hdr.hist.srow_z(1:3)];

      T = [hdr.hist.srow_x(4)
           hdr.hist.srow_y(4)
           hdr.hist.srow_z(4)];

      if det(R) == 0 | ~isequal(R(find(R)), sum(R)')
         hdr.hist.old_affine = [ [R;[0 0 0]] [T;1] ];
         R_sort = sort(abs(R(:)));
         R( find( abs(R) < tolerance*min(R_sort(end-2:end)) ) ) = 0;
         hdr.hist.new_affine = [ [R;[0 0 0]] [T;1] ];

         if det(R) == 0 | ~isequal(R(find(R)), sum(R)')
            msg = [char(10) char(10) '   Non-orthogonal rotation or shearing '];
            msg = [msg 'found inside the affine matrix' char(10)];
            msg = [msg '   in this NIfTI file. You have 3 options:' char(10) char(10)];
            msg = [msg '   1. Using included ''reslice_nii.m'' program to reslice the NIfTI' char(10)];
            msg = [msg '      file. I strongly recommand this, because it will not cause' char(10)];
            msg = [msg '      negative effect, as long as you remember not to do slice' char(10)];
            msg = [msg '      time correction after using ''reslice_nii.m''.' char(10) char(10)];
            msg = [msg '   2. Using included ''load_untouch_nii.m'' program to load image' char(10)];
            msg = [msg '      without applying any affine geometric transformation or' char(10)];
            msg = [msg '      voxel intensity scaling. This is only for people who want' char(10)];
            msg = [msg '      to do some image processing regardless of image orientation' char(10)];
            msg = [msg '      and to save data back with the same NIfTI header.' char(10) char(10)];
            msg = [msg '   3. Increasing the tolerance to allow more distortion in loaded' char(10)];
            msg = [msg '      image, but I don''t suggest this.' char(10) char(10)];
            msg = [msg '   To get help, please type:' char(10) char(10) '   help reslice_nii.m' char(10)];
            msg = [msg '   help load_untouch_nii.m' char(10) '   help load_nii.m'];
            error(msg);
         end
      end

   elseif isequal(useForm,'q')
      b = hdr.hist.quatern_b;
      c = hdr.hist.quatern_c;
      d = hdr.hist.quatern_d;

      if 1.0-(b*b+c*c+d*d) < 0
         if abs(1.0-(b*b+c*c+d*d)) < 1e-5
            a = 0;
         else
            error('Incorrect quaternion values in this NIFTI data.');
         end
      else
         a = sqrt(1.0-(b*b+c*c+d*d));
      end

      qfac = hdr.dime.pixdim(1);
      if qfac==0, qfac = 1; end
      i = hdr.dime.pixdim(2);
      j = hdr.dime.pixdim(3);
      k = qfac * hdr.dime.pixdim(4);

      R = [a*a+b*b-c*c-d*d     2*b*c-2*a*d        2*b*d+2*a*c
           2*b*c+2*a*d         a*a+c*c-b*b-d*d    2*c*d-2*a*b
           2*b*d-2*a*c         2*c*d+2*a*b        a*a+d*d-c*c-b*b];

      T = [hdr.hist.qoffset_x
           hdr.hist.qoffset_y
           hdr.hist.qoffset_z];

      %  qforms are expected to generate rotation matrices R which are
      %  det(R) = 1; we'll make sure that happens.
      %
      %  now we make the same checks as were done above for sform data
      %  BUT we do it on a transform that is in terms of voxels not mm;
      %  after we figure out the angles and squash them to closest
      %  rectilinear direction. After that, the voxel sizes are then
      %  added.
      %
      %  This part is modified by Jeff Gunter.
      %
      if det(R) == 0 | ~isequal(R(find(R)), sum(R)')

         %  det(R) == 0 is not a common trigger for this ---
         %  R(find(R)) is a list of non-zero elements in R; if that
         %  is straight (not oblique) then it should be the same as
         %  columnwise summation. Could just as well have checked the
         %  lengths of R(find(R)) and sum(R)' (which should be 3)
         %
         hdr.hist.old_affine = [ [R * diag([i j k]);[0 0 0]] [T;1] ];
         R_sort = sort(abs(R(:)));
         R( find( abs(R) < tolerance*min(R_sort(end-2:end)) ) ) = 0;
         R = R * diag([i j k]);
         hdr.hist.new_affine = [ [R;[0 0 0]] [T;1] ];

         if det(R) == 0 | ~isequal(R(find(R)), sum(R)')
            msg = [char(10) char(10) '   Non-orthogonal rotation or shearing '];
            msg = [msg 'found inside the affine matrix' char(10)];
            msg = [msg '   in this NIfTI file. You have 3 options:' char(10) char(10)];
            msg = [msg '   1. Using included ''reslice_nii.m'' program to reslice the NIfTI' char(10)];
            msg = [msg '      file. I strongly recommand this, because it will not cause' char(10)];
            msg = [msg '      negative effect, as long as you remember not to do slice' char(10)];
            msg = [msg '      time correction after using ''reslice_nii.m''.' char(10) char(10)];
            msg = [msg '   2. Using included ''load_untouch_nii.m'' program to load image' char(10)];
            msg = [msg '      without applying any affine geometric transformation or' char(10)];
            msg = [msg '      voxel intensity scaling. This is only for people who want' char(10)];
            msg = [msg '      to do some image processing regardless of image orientation' char(10)];
            msg = [msg '      and to save data back with the same NIfTI header.' char(10) char(10)];
            msg = [msg '   3. Increasing the tolerance to allow more distortion in loaded' char(10)];
            msg = [msg '      image, but I don''t suggest this.' char(10) char(10)];
            msg = [msg '   To get help, please type:' char(10) char(10) '   help reslice_nii.m' char(10)];
            msg = [msg '   help load_untouch_nii.m' char(10) '   help load_nii.m'];
            error(msg);
         end

      else
         R = R * diag([i j k]);
      end					% 1st det(R)

   else
      affine_transform = 0;	% no sform or qform transform
   end

   if affine_transform == 1
      voxel_size = abs(sum(R,1));
      inv_R = inv(R);
      originator = inv_R*(-T)+1;
      orient = get_orient(inv_R);

      %  modify pixdim and originator
      %
      hdr.dime.pixdim(2:4) = voxel_size;
      hdr.hist.originator(1:3) = originator;

      %  set sform or qform to non-use, because they have been
      %  applied in xform_nii
      %
      hdr.hist.qform_code = 0;
      hdr.hist.sform_code = 0;
   end

   %  apply space_unit to pixdim if not 1 (mm)
   %
   space_unit = get_units(hdr);

   if space_unit ~= 1
      hdr.dime.pixdim(2:4) = hdr.dime.pixdim(2:4) * space_unit;

      %  set space_unit of xyzt_units to millimeter, because
      %  voxel_size has been re-scaled
      %
      hdr.dime.xyzt_units = char(bitset(hdr.dime.xyzt_units,1,0));
      hdr.dime.xyzt_units = char(bitset(hdr.dime.xyzt_units,2,1));
      hdr.dime.xyzt_units = char(bitset(hdr.dime.xyzt_units,3,0));
   end

   hdr.dime.pixdim = abs(hdr.dime.pixdim);

   return;					% change_hdr

%-----------------------------------------------------------------------
function orient = get_orient(R)

   orient = [];

   for i = 1:3
      switch find(R(i,:)) * sign(sum(R(i,:)))
      case 1
         orient = [orient 1];		% Left to Right
      case 2
         orient = [orient 2];		% Posterior to Anterior
      case 3
         orient = [orient 3];		% Inferior to Superior
      case -1
         orient = [orient 4];		% Right to Left
      case -2
         orient = [orient 5];		% Anterior to Posterior
      case -3
         orient = [orient 6];		% Superior to Inferior
      end
   end

   return;					% get_orient

%-----------------------------------------------------------------------
function [space_unit, time_unit] = get_units(hdr)

   switch bitand(hdr.dime.xyzt_units, 7)	% mask with 0x07
   case 1
      space_unit = 1e+3;		% meter, m
   case 3
      space_unit = 1e-3;		% micrometer, um
   otherwise
      space_unit = 1;			% millimeter, mm
   end

   switch bitand(hdr.dime.xyzt_units, 56)	% mask with 0x38
   case 16
      time_unit = 1e-3;			% millisecond, ms
   case 24
      time_unit = 1e-6;			% microsecond, us
   otherwise
      time_unit = 1;			% second, s
   end

   return;					% get_units


function [p,x_min,upper] = initializesolution(p,x_min,upper)

if p.options.warmstart
    x = p.x0;
    z = evaluate_nonlinear(p,x);
    residual = constraint_residuals(p,z);
    relaxed_feasible = all(residual(1:p.K.f)>=-p.options.bmibnb.eqtol) & all(residual(1+p.K.f:end)>=-p.options.bmibnb.pdtol);
    if relaxed_feasible
        % upper_ = p.f+p.c'*z+z'*p.Q*z;
        upper_ = computecost(p.f,p.c,p.Q,z,p);
        if upper_ <= upper
            upper = upper_;
            x_min = z;
        end
    end
else
    x0 = p.x0;
    p.x0 = zeros(length(p.c),1);
    % Avoid silly warnings
    if ~isempty(p.evalMap)
        for i = 1:length(p.evalMap)
            if (isequal(p.evalMap{i}.fcn,'log') | isequal(p.evalMap{i}.fcn,'log2') | isequal(p.evalMap{i}.fcn,'log10'))
                p.x0(p.evalMap{i}.variableIndex) = (p.lb(p.evalMap{i}.variableIndex) +  p.ub(p.evalMap{i}.variableIndex))/2;
            end
        end
    end
    p = correctEXPConeClosureInitial(p);
    x = p.x0;
    z = evaluate_nonlinear(p,x);
    z = propagateAuxilliary(p,z);

    residual = constraint_residuals(p,z);
    relaxed_feasible = all(residual(1:p.K.f)>=-p.options.bmibnb.eqtol) & all(residual(1+p.K.f:end)>=-p.options.bmibnb.pdtol);
    if relaxed_feasible
        for i = 1:length(p.evalMap)
            if ~isempty(p.evalMap{i}.properties.forbidden)
                if z(p.evalMap{i}.variableIndex) > p.evalMap{i}.properties.forbidden(1) && z(p.evalMap{i}.variableIndex) < p.evalMap{i}.properties.forbidden(2)
                    relaxed_feasible = 0;
                end
            end
        end
    end
    if relaxed_feasible
        infs = isinf(z);
        if ~any(infs)
            % upper_ = p.f+p.c'*z+z'*p.Q*z;
            upper_ = computecost(p.f,p.c,p.Q,z,p);
            if upper_ <= upper
                upper = upper_;
                x_min = z;
                x0 = x_min;
            end
        else
            % Allow inf solutions if variables aren't used in objective
            if all(p.c(infs)==0) & nnz(p.Q(infs,:))==0
                ztemp = z;ztemp(infs)=0;
                %upper_ = p.f+p.c'*ztemp+ztemp'*p.Q*ztemp;
                upper_ = computecost(p.f,p.c,p.Q,ztemp,p);
                if upper_ <= upper
                    upper = upper_;
                    x_min = z;
                    x0 = x_min;
                end
            end
        end
    end
    p.x0 = (p.lb + p.ub)/2;
    p.x0(isinf(p.ub))=p.lb(isinf(p.ub));
    p.x0(isinf(p.lb))=p.ub(isinf(p.lb));
    p.x0(isinf(p.lb) & isinf(p.ub))=0;
    if ~isempty(p.integer_variables)
        p.x0(p.integer_variables) = round(p.x0(p.integer_variables));
    end
    if ~isempty(p.binary_variables)
        p.x0(p.binary_variables) = round(p.x0(p.binary_variables));
    end
    p = correctEXPConeClosureInitial(p);
    x = p.x0;
    x(isinf(x))=eps;
    x(isnan(x))=eps;
    z = evaluate_nonlinear(p,x);
    z = propagateAuxilliary(p,z);
    residual = constraint_residuals(p,z);
    relaxed_feasible = all(residual(1:p.K.f)>=-p.options.bmibnb.eqtol) & all(residual(1+p.K.f:end)>=p.options.bmibnb.pdtol);
    if relaxed_feasible
        tempcost = computecost(p.f,p.c,p.Q,z,p);
        if tempcost < upper
            upper = tempcost;
            x_min = z;
            x0 = x_min;
        end
    end
    p.x0 = x0;
end

function z = propagateAuxilliary(p,z)

try
    % New feature. If we introduce new variables xx = f(x) to be used
    % in a nonlinear operator, we can derive its value when x is chosen

    if ~isempty(p.aux_variables)
        if p.K.f > 1
            A = p.F_struc(1:p.K.f,2:end);
            b = p.F_struc(1:p.K.f,1);
            for i = 1:length(p.aux_variables)
                j = find(A(:,p.aux_variables(i)));
                if length(j)==1
                    if A(j,p.aux_variables(i))==1
                        z(p.aux_variables(i)) = -b(j)-A(j,:)*z;
                    end
                end
            end
            z = evaluate_nonlinear(p,z);
        end
    end
catch
end


function mriCoord = cs_scs2mri(MRI, scsCoord)
% CS_SCS2MRI: Transform SCS point coordinates (in mm) to MRI coordinate system (in mm)
%
% DEPRECATED: Replace with cs_convert()

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@

disp('BST> WARNING: Deprecated function cs_scs2mri(), use cs_convert() instead.');

mriCoord = cs_convert(MRI, 'scs', 'mri', scsCoord' / 1000)' * 1000;


function msg = mpc_init(s,c)
    apm(s,c,'clear all');

    % load model and data
    apm_load(s,c,'control.apm');
    csv_load(s,c,'control.csv');

    % configure MV / CV
    apm_info(s,c,'MV','Q1');
    apm_info(s,c,'MV','Q2');
    apm_info(s,c,'CV','TC1');
    apm_info(s,c,'CV','TC2');

    % dynamic control
    apm_option(s,c,'apm.imode',6);
    apm_option(s,c,'apm.solver',3);
    apm_option(s,c,'apm.hist_hor',600);

    % tune MV
    % delta MV movement penalty
    apm_option(s,c,'Q1.dcost',1.0e-4);
    apm_option(s,c,'Q2.dcost',1.0e-4);
    % penalize energy use
    apm_option(s,c,'Q1.cost',0.0);
    apm_option(s,c,'Q2.cost',0.0);
    % limit MV movement each cycle
    apm_option(s,c,'Q1.dmax',50);
    apm_option(s,c,'Q2.dmax',50);
    % MV limits
    apm_option(s,c,'Q1.upper',100);
    apm_option(s,c,'Q1.lower',0);
    apm_option(s,c,'Q2.upper',100);
    apm_option(s,c,'Q2.lower',0);

    % tune CV
    % how fast to reach setpoint
    apm_option(s,c,'TC1.tau',10);
    apm_option(s,c,'TC2.tau',10);
    % trajectory type
    apm_option(s,c,'TC1.tr_init',2);
    apm_option(s,c,'TC2.tr_init',2);

    % let optimizer use MV
    apm_option(s,c,'Q1.status',1);
    apm_option(s,c,'Q2.status',1);
    % include CV in objective function
    apm_option(s,c,'TC1.status',1);
    apm_option(s,c,'TC2.status',1);

    % feedback status (whether we have measurements)
    apm_option(s,c,'Q1.fstatus',0);
    apm_option(s,c,'Q2.fstatus',0);
    apm_option(s,c,'TC1.fstatus',1);
    apm_option(s,c,'TC2.fstatus',1);

    % web-viewer option, update every second
    apm_option(s,c,'apm.web_plot_freq',3);

    msg = 'initialization complete';
end


function model = setup_fmincon_params(model)

monomtable = model.monomtable;
nonlinearindicies = model.nonlinearindicies;
linearindicies = model.linearindicies;

model.evalinobjective  = 0;

if ~isempty(model.evalMap)
    [i,j,k] = find(model.monomtable(:,model.evalVariables));
    evalInvolved = union(model.evalVariables,i);
    model.evalinobjective = any(model.c(evalInvolved)) | any(model.Q(evalInvolved,1));
    if size(model.Anonlineq,1)>0
        if nnz(model.Anonlineq(:,evalInvolved))>0
            model.evalinconstraint = 1;
        end
    end
    if size(model.Anonlinineq,1)>0
        if nnz(model.Anonlinineq(:,evalInvolved))>0
            model.evalinconstraint = 1;
        end
    end
    if any(model.K.q)
        if nnz(model.F_struc(1+model.K.f + model.K.l:end,1+evalInvolved)) > 0
            model.evalinconstraint = 1;
        end
    end
    % Speed up callbacks by removing last marker argument (only used in
    % expandmodel etc) and figuring out what R^m -> R^n operators computes
    for i = 1:length(model.evalMap)
        model.evalMap{i}.prearg = {model.evalMap{i}.fcn,model.evalMap{i}.arg{1:end-1}};
        model.evalMap{i}.prearg{1+model.evalMap{i}.argumentIndex} = [];
        if ~isfield(model.evalMap{i},'computes')
            model.evalMap{i}.computes = model.evalVariables(i);
        end
    end
end

% Figure out if YALMIP easily can compute the gradient of the objective
% This will done completely general later
model.SimpleLinearObjective = 0;    % Linear
model.SimpleQuadraticObjective = 0; % Quadratic
model.SimpleNonlinearObjective = 1; % Polynomial
if isempty(model.evalMap)
    if nnz(model.c(nonlinearindicies)) == 0
        if (nnz(model.Q)==0)
            model.SimpleLinearObjective = 1;
        else
            if nnz(model.Q(nonlinearindicies,nonlinearindicies))==0
                model.SimpleQuadraticObjective = 1;
            end
        end
    end
elseif ~model.evalinobjective
    if nnz(model.c(nonlinearindicies)) == 0
        if (nnz(model.Q)==0)
            model.SimpleLinearObjective = 1;
        else
            if nnz(model.Q(nonlinearindicies,nonlinearindicies))==0
                model.SimpleQuadraticObjective = 1;
            end
        end
    elseif nnz(model.Q)==0
        r = find(model.c);
        if all(model.variabletype(r)<=2)
            if isempty(intersect(r,model.evalVariables))
                % Simple quadratic function
                for i = r(:)'
                    if model.variabletype(i)==2
                        j = find(model.monomtable(i,:));
                        model.Q(j,j) = model.c(i);
                        model.c(i) = 0;
                    elseif model.variabletype(i)==1
                        j = find(model.monomtable(i,:));
                        model.Q(j(1),j(2)) = model.c(i)/2;
                        model.Q(j(2),j(1)) = model.c(i)/2;
                        model.c(i) = 0;
                    end
                end
                model.SimpleQuadraticObjective = 1;
            end
        end
    end
else
    model.SimpleNonlinearObjective = 0;
end

model.linearconstraints = isempty(model.Anonlinineq) & isempty(model.Anonlineq) & nnz(model.K.q)==0;
model.nonlinearinequalities = ~isempty(model.Anonlinineq);
model.nonlinearequalities = ~isempty(model.Anonlineq);
if any(model.K.q)
    if nnz(model.F_struc(1+model.K.f + model.K.l:end,1+model.nonlinearindicies)) > 0
        model.nonlinearcones = 1;
    else
        model.nonlinearcones = 0;
    end
else
    model.nonlinearcones = 0;
end

% Structure for fast evaluation of monomial terms in differentiation
 if isempty(model.evalMap) & (model.nonlinearinequalities | model.nonlinearequalities | model.nonlinearcones) & ~isfield(model,'fastdiff')
    allA = [model.Anonlineq;model.Anonlinineq];
    dgAll = [];
    n = length(model.c);
    linearindicies = model.linearindicies;
    mtNonlinear = model.monomtable(model.nonlinearindicies,:);

    allDerivemt = [];
    news = [];
    c = [];
    mtNonlinear = mtNonlinear(:,linearindicies);
    mtNonlinear = mtNonlinear';

    [jj,ii,val] = find(mtNonlinear');
    for k = 1:length(ii)
        i = ii(k);
        j = jj(k);
        s=mtNonlinear(:,j);
        c = [c;s((i))];
        s((i)) = s((i))-1;
        allDerivemt = [allDerivemt s(:)];
        news = [news;j i];
    end

    allDerivemt = allDerivemt';

    model.fastdiff.news = news;
    model.fastdiff.allDerivemt = allDerivemt;
    model.fastdiff.c = c;
    model.fastdiff.univariateDifferentiates = 0;

    a1 =  model.fastdiff.news(1:length(model.fastdiff.c),2);
    a2 =  model.nonlinearindicies(model.fastdiff.news(1:length(model.fastdiff.c),1))';
    zzz = [ones(length(a1),1)];
    nn = max(max(length(model.linearindicies)),max(news(:,2)));
    mm = max(max(linearindicies),max(model.nonlinearindicies(news(:,1))));
    a1f = [a1(:);(1:length(model.linearindicies))'];
    a2f = [a2(:);model.linearindicies(:)];
    zzzf = [zzz;ones(length(linearindicies),1)];

    %model.fastdiff.newdxx = sparse(a1f,a2f,zzzf,nn,mm);
    %model.fastdiff.linear_in_newdxx = sub2ind([nn mm],a1(:),a2(:));
    model.fastdiff.newdxx = sparse(a1f,a2f,zzzf,nn,mm)';
    model.fastdiff.linear_in_newdxx = sub2ind([mm nn],a2(:),a1(:));
    model.fastdiff.newdxx(model.fastdiff.linear_in_newdxx)=1;

    if all(sum(allDerivemt | allDerivemt,2)==1)
        model.fastdiff.univariateDifferentiates = 1;
        [i,j,k] = find(allDerivemt');
        if any(diff(j)<0)
            error;
        end
        model.fastdiff.univariateDiffMonom = i(:);
        model.fastdiff.univariateDiffPower = k(:);
    end
 else
      allA = [model.Anonlineq;model.Anonlinineq];
      if any(model.K.q)
          allA = [allA;model.F_struc(1+model.K.f + model.K.f:end,2:end)];
      end
      requested = any(allA',2);
      [i,j,k] = find((model.deppattern(find(requested),:)));
      requested(j) = 1;
      model.fastdiff.requested = requested;
 end


port = int16(2000);
client = py.carla.Client('localhost', port);
client.set_timeout(10.0);
world = client.get_world();
blueprint_library = world.get_blueprint_library();
actor_list = py.list(world.get_actors());

id = 0;

for i=1:length(actor_list)
    k = actor_list{i}.type_id;

    if k(1:7) == "vehicle"
        id = i - 1;
    end
end

if (id == 0)
    fprintf('ERROR: Could not find any vehicle!\n');
    fprintf('The program will now exit!\n');
    return;
end

carla_is_running = true;
yolo_detection = true;

% Sensor 1
blueprint = world.get_blueprint_library().find('sensor.camera.rgb');
blueprint.set_attribute('image_size_x', '1920')
blueprint.set_attribute('image_size_y', '1080')
% blueprint.set_attribute('sensor_tick', '.1');
transform = py.carla.Transform(py.carla.Location(pyargs('x',0.8, 'z',1.7)));
sensor = world.spawn_actor(blueprint, transform, pyargs('attach_to',actor_list{id}));

pyModule = py.importlib.import_module('senFunc');
py.senFunc.getData(sensor);

pause(.5);
% data = uint8(py.array.array('d',py.numpy.nditer(ml)));

% Load YOLO model
if exist('yoloml','var') ~= 1
    disp('loading modified network')
    load('yoloml.mat')
end

% Display Initial Frame
image = py.getattr(pyModule, 'array');
% image = py.numpy.ascontiguousarray(image);
imMat = uint8(image);

% Set Yolo parameters
probThresh = 0.175;
iouThresh = 0.5;

myImageSize = size(imMat);

classLabels = ["aeroplane",	"bicycle",	"bird"	,"boat",	"bottle"	,"bus"	,"car",...
"cat",	"chair"	,"cow"	,"diningtable"	,"dog"	,"horse",	"motorbike",	"person",	"pottedplant",...
"sheep",	"sofa",	"train",	"tvmonitor"];

imageHandle = imshow(imMat);

% Detect After how many frames
totalFramesElapsed = 0;
predictEvery = 2;

while carla_is_running
%     pause(1);
%     clc;

     image = py.getattr(pyModule, 'array');
     imMat = uint8(image);

     % Detect Cars
%      [bboxes,scores] = detect(detector,imMat);
%      imMat = insertObjectAnnotation(imMat,'rectangle',bboxes,1);

     if yolo_detection
         redImage = single(imresize(imMat,[448 448]))/255;

         if ~rem(totalFramesElapsed, predictEvery)
            out = predict(yoloml,redImage,'ExecutionEnvironment','gpu');
            % fprintf('%d\n', totalFramesElapsed);
         end
         class = out(1:980);
         boxProbs = out(981:1078);
         boxDims = out(1079:1470);

         outArray = zeros(7,7,30);

         for j = 0:6
             for i = 0:6
                 outArray(i+1,j+1,1:20) = class(i*20*7+j*20+1:i*20*7+j*20+20);
                 outArray(i+1,j+1,21:22) = boxProbs(i*2*7+j*2+1:i*2*7+j*2+2);
                 outArray(i+1,j+1,23:30) = boxDims(i*8*7+j*8+1:i*8*7+j*8+8);
             end
         end

         [cellProb, cellIndex] = max(outArray(:,:,21:22),[],3);
         contain = max(outArray(:,:,21:22),[],3)>probThresh;

         [classMax, classMaxIndex] = max(outArray(:,:,1:20),[],3);

         boxes = [];

         counter = 0;
         for i = 1:7
             for j = 1:7
                 if contain(i,j) == 1
                     counter = counter+1;

                     % Bounding box center relative to cell
                     x = outArray(i,j,22+1+(cellIndex(i,j)-1)*4);
                     y = outArray(i,j,22+2+(cellIndex(i,j)-1)*4);

                     % Yolo outputs the square root of the width and height of the
                     % bounding boxes (subtle detail in paper that took me forver to realize).
                     % Relative to image size.
                     w = (outArray(i,j,22+3+(cellIndex(i,j)-1)*4))^2;
                     h = (outArray(i,j,22+4+(cellIndex(i,j)-1)*4))^2;

                     %absolute values scaled to image size
                     wS = w*448;
                     hS = h*448;
                     xS = (j-1)*448/7+x*448/7-wS/2;
                     yS = (i-1)*448/7+y*448/7-hS/2;

                     % this array will be used for drawing bounding boxes in Matlab
                     boxes(counter).coords = [xS yS wS hS];

                    %save cell indices in the structure
                     boxes(counter).cellIndex = [i,j];

                    %save classIndex to structure
                     boxes(counter).classIndex = classMaxIndex(i,j);

                    % save cell proability to structure
                     boxes(counter).cellProb = cellProb(i,j);

                    % put in a switch for non max which we will use later
                     boxes(counter).nonMax = 1;
                 end
             end
         end

         for i = 1:length(boxes)
             for j = i+1:length(boxes)
                 %calculate intersection over union (can also use bboxOverlapRatio
                 %with proper toolbox
                 intersect = rectint(boxes(i).coords,boxes(j).coords);
                 union = boxes(i).coords(3)*boxes(i).coords(4)+boxes(j).coords(3)*boxes(j).coords(4)-intersect;
                 iou(i,j) = intersect/union;
                 if boxes(i).classIndex == boxes(j).classIndex && iou(i,j) > iouThresh
                     [value(i), dropIndex(i)] = min([boxes(i).cellProb boxes(j).cellProb]);
                     if dropIndex(i) == 1
                         boxes(i).nonMax=0;
                     elseif dropIndex(i) == 2
                         boxes(j).nonMax=0;
                     end
                 end
             end
         end

         %imagesc(imMat);

         for i = 1:length(boxes)
             if boxes(i).nonMax == 1
                 textStr = convertStringsToChars(classLabels(boxes(i).classIndex));

                 if sum(textStr == [ "bicycle", "car", "motorbike",	"person"])
                     position = [(boxes(i).cellIndex(2)-1)*myImageSize(2)/7 (boxes(i).cellIndex(1)-1)*myImageSize(1)/7];
                     %text(position(1),position(2),textStr,'Color',[0 1 0],'fontWeight','bold','fontSize',12);
                     transformedCoor = boxes(i).coords .* [myImageSize(2) myImageSize(1) myImageSize(2) myImageSize(1)] / 448;

                     imMat = insertText(imMat, transformedCoor([1, 2]), textStr, 'AnchorPoint','LeftBottom', 'FontSize', 36);
                     imMat = insertShape(imMat, 'Rectangle', transformedCoor, 'LineWidth',5);

                     % rectangle('Position',transformedCoor, 'EdgeColor','green','LineWidth',2);
                 end
             end
         end
     end

     % Write directly to Cdata. Consideribly faster than imshow
     set(imageHandle,'Cdata',imMat);
     drawnow;

     totalFramesElapsed = totalFramesElapsed + 1;

     pause(0.001);

      % Directly write to Cdata, Noticibly faster than just imshow
     % rectangle('Position',[1 1 30 30], 'EdgeColor','green','LineWidth',2);
     %imagesc(imMat);
     %pause(.05);
     %py.cv2.imshow('Carla', image);
     % pause(0.05);
%      data = uint8(py.array.array('d',py.numpy.nditer(image)));
%
%      data = reshape(data, 4, []);
%
%      imMat(:,:,1) = reshape(data(3,:), 1920, 1080)';
%      imMat(:,:,2) = reshape(data(2,:), 1920, 1080)';
%      imMat(:,:,3) = reshape(data(1,:), 1920, 1080)';
%
%      imshow(imMat);

%      for n=0:1080*1920-1
%         i = fix(n/1920) + 1;
%         j = rem(n,1920) + 1;
%
%         imMat(i, j, 3) = data(4*n + 1);
%         imMat(i, j, 2) = data(4*n + 2);
%         imMat(i, j, 1) = data(4*n + 3);
%      end

% %     currentLocation = actor_list{id}.get_location();
% %     fprintf('Location: [m]\n')
% %     fprintf('x: %.2f \t y: %.2f \t z: %.2f \n\n',currentLocation.x, currentLocation.y, currentLocation.z);
% %
% %     fprintf('Velocity: [m/s]\n')
% %     currentVelocity = actor_list{id}.get_velocity();
% %     fprintf('x: %.2f \t y: %.2f \t z: %.2f \n\n',currentVelocity.x, currentVelocity.y, currentVelocity.z);
% %
% %     fprintf('Acceleration: [m/s^2]\n')
% %     currentAcceleration = actor_list{id}.get_acceleration();
% %     fprintf('x: %.2f \t y: %.2f \t z: %.2f \n\n',currentAcceleration.x, currentAcceleration.y, currentAcceleration.z);
% %
% %     omega = char (hex2dec ( '03C9' ));
% %     fprintf('Angular Velocity: [%s]\n', omega);
% %     currentAngVelocity = actor_list{id}.get_angular_velocity();
% %     fprintf('x: %.2f \t y: %.2f \t z: %.2f \n\n',currentAngVelocity.x, currentAngVelocity.y, currentAngVelocity.z);
%
end


function out = spm_deformations(job)
% Various deformation field utilities
% FORMAT out = spm_deformations(job)
% job - a job created via spm_cfg_deformations.m
% out - a struct with fields
%       .def    - file name of created deformation field
%       .warped - file names of warped images
%
% See spm_cfg_deformations.m for more information.
%__________________________________________________________________________
% Copyright (C) 2005-2015 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_deformations.m 6577 2015-10-15 15:22:11Z volkmar $

[Def,mat] = get_comp(job.comp);
out = struct('def',{{}},'warped',{{}},'surf',{{}},'jac',{{}});
for i=1:numel(job.out)
    fn = fieldnames(job.out{i});
    fn = fn{1};
    switch fn
    case 'savedef'
        out.def    = [out.def;    save_def(Def,mat,job.out{i}.(fn))];
    case 'pull'
        out.warped = [out.warped; pull_def(Def,mat,job.out{i}.(fn))];
    case 'push'
        out.warped = [out.warped; push_def(Def,mat,job.out{i}.(fn))];
    case 'surf'
        out.surf   = [out.surf;   surf_def(Def,mat,job.out{i}.(fn))];
    case 'savejac'
        out.jac    = [out.jac;    jac_def(Def,mat,job.out{i}.(fn))];
    otherwise
        error('Unknown option');
    end
end

%==========================================================================
% function [Def,mat] = get_comp(job)
%==========================================================================
function [Def,mat] = get_comp(job)
% Return the composition of a number of deformation fields.

if isempty(job)
    error('Empty list of jobs in composition');
end
[Def,mat] = get_job(job{1});
for i=2:numel(job)
    Def1         = Def;
    mat1         = mat;
    [Def,mat]    = get_job(job{i});
    M            = inv(mat1);
    tmp          = zeros(size(Def),'single');
    tmp(:,:,:,1) = M(1,1)*Def(:,:,:,1)+M(1,2)*Def(:,:,:,2)+M(1,3)*Def(:,:,:,3)+M(1,4);
    tmp(:,:,:,2) = M(2,1)*Def(:,:,:,1)+M(2,2)*Def(:,:,:,2)+M(2,3)*Def(:,:,:,3)+M(2,4);
    tmp(:,:,:,3) = M(3,1)*Def(:,:,:,1)+M(3,2)*Def(:,:,:,2)+M(3,3)*Def(:,:,:,3)+M(3,4);
    Def(:,:,:,1) = single(spm_diffeo('bsplins',Def1(:,:,:,1),tmp,[1,1,1,0,0,0]));
    Def(:,:,:,2) = single(spm_diffeo('bsplins',Def1(:,:,:,2),tmp,[1,1,1,0,0,0]));
    Def(:,:,:,3) = single(spm_diffeo('bsplins',Def1(:,:,:,3),tmp,[1,1,1,0,0,0]));
    clear tmp
end

%==========================================================================
% function [Def,mat] = get_job(job)
%==========================================================================
function [Def,mat] = get_job(job)
% Determine what is required, and pass the relevant bit of the
% job out to the appropriate function.

fn = fieldnames(job);
fn = fn{1};
switch fn
    case {'comp'}
        [Def,mat] = get_comp(job.(fn));
    case {'def'}
        [Def,mat] = get_def(job.(fn));
    case {'dartel'}
        [Def,mat] = get_dartel(job.(fn));
    case {'sn2def'}
        [Def,mat] = get_sn2def(job.(fn));
    case {'inv'}
        [Def,mat] = get_inv(job.(fn));
    case {'id'}
        [Def,mat] = get_id(job.(fn));
    case {'idbbvox'}
        [Def,mat] = get_idbbvox(job.(fn));
    otherwise
        error('Unrecognised job type');
end

%==========================================================================
% function [Def,mat] = get_sn2def(job)
%==========================================================================
function [Def,mat] = get_sn2def(job)
% Convert a SPM _sn.mat file into a deformation field, and return it.

vox = job.vox;
bb  = job.bb;
sn  = load(job.matname{1});

if any(isfinite(bb(:))) || any(isfinite(vox))
    [bb0, vox0] = spm_get_bbox(sn.VG(1));
    if any(~isfinite(vox)), vox = vox0; end
    if any(~isfinite(bb)),  bb  = bb0;  end
    bb  = sort(bb);
    vox = abs(vox);

    % Adjust bounding box slightly - so it rounds to closest voxel.
    bb(:,1) = round(bb(:,1)/vox(1))*vox(1);
    bb(:,2) = round(bb(:,2)/vox(2))*vox(2);
    bb(:,3) = round(bb(:,3)/vox(3))*vox(3);

    M   = sn.VG(1).mat;
    vxg = sqrt(sum(M(1:3,1:3).^2));
    ogn = M\[0 0 0 1]';
    ogn = ogn(1:3)';

    % Convert range into range of voxels within template image
    x   = (bb(1,1):vox(1):bb(2,1))/vxg(1) + ogn(1);
    y   = (bb(1,2):vox(2):bb(2,2))/vxg(2) + ogn(2);
    z   = (bb(1,3):vox(3):bb(2,3))/vxg(3) + ogn(3);

    og  = -vxg.*ogn;
    of  = -vox.*(round(-bb(1,:)./vox)+1);
    M1  = [vxg(1) 0 0 og(1) ; 0 vxg(2) 0 og(2) ; 0 0 vxg(3) og(3) ; 0 0 0 1];
    M2  = [vox(1) 0 0 of(1) ; 0 vox(2) 0 of(2) ; 0 0 vox(3) of(3) ; 0 0 0 1];
    mat = sn.VG(1).mat*(M1\M2);
    % dim = [length(x) length(y) length(z)];
else
    dim = sn.VG(1).dim;
    x   = 1:dim(1);
    y   = 1:dim(2);
    z   = 1:dim(3);
    mat = sn.VG(1).mat;
end

[X,Y] = ndgrid(x,y);

st = size(sn.Tr);

if (prod(st) == 0)
    affine_only = true;
    basX = 0;
    basY = 0;
    basZ = 0;
else
    affine_only = false;
    basX = spm_dctmtx(sn.VG(1).dim(1),st(1),x-1);
    basY = spm_dctmtx(sn.VG(1).dim(2),st(2),y-1);
    basZ = spm_dctmtx(sn.VG(1).dim(3),st(3),z-1);
end

Def = zeros([numel(x),numel(y),numel(z),3],'single');

for j=1:length(z)
    if (~affine_only)
        tx = reshape( reshape(sn.Tr(:,:,:,1),st(1)*st(2),st(3)) *basZ(j,:)', st(1), st(2) );
        ty = reshape( reshape(sn.Tr(:,:,:,2),st(1)*st(2),st(3)) *basZ(j,:)', st(1), st(2) );
        tz = reshape( reshape(sn.Tr(:,:,:,3),st(1)*st(2),st(3)) *basZ(j,:)', st(1), st(2) );

        X1 = X    + basX*tx*basY';
        Y1 = Y    + basX*ty*basY';
        Z1 = z(j) + basX*tz*basY';
    end

    Mult = sn.VF.mat*sn.Affine;
    if (~affine_only)
        X2= Mult(1,1)*X1 + Mult(1,2)*Y1 + Mult(1,3)*Z1 + Mult(1,4);
        Y2= Mult(2,1)*X1 + Mult(2,2)*Y1 + Mult(2,3)*Z1 + Mult(2,4);
        Z2= Mult(3,1)*X1 + Mult(3,2)*Y1 + Mult(3,3)*Z1 + Mult(3,4);
    else
        X2= Mult(1,1)*X + Mult(1,2)*Y + (Mult(1,3)*z(j) + Mult(1,4));
        Y2= Mult(2,1)*X + Mult(2,2)*Y + (Mult(2,3)*z(j) + Mult(2,4));
        Z2= Mult(3,1)*X + Mult(3,2)*Y + (Mult(3,3)*z(j) + Mult(3,4));
    end

    Def(:,:,j,1) = single(X2);
    Def(:,:,j,2) = single(Y2);
    Def(:,:,j,3) = single(Z2);
end

%==========================================================================
% function [Def,mat] = get_def(job)
%==========================================================================
function [Def,mat] = get_def(job)
% Load a deformation field saved as an image
Nii = nifti(job{1});
Def = single(Nii.dat(:,:,:,1,:));
d   = size(Def);
if d(4)~=1 || d(5)~=3, error('Deformation field is wrong!'); end
Def = reshape(Def,[d(1:3) d(5)]);
mat = Nii.mat;

%==========================================================================
% function [Def,mat] = get_dartel(job)
%==========================================================================
function [Def,mat] = get_dartel(job)

Nii    = nifti(job.flowfield{1});

if ~isempty(job.template{1})
   %Nt        = nifti(job.template{1});
    [pth,nam] = fileparts(job.template{1});
    if exist(fullfile(pth,[nam '_2mni.mat']),'file')
        load(fullfile(pth,[nam '_2mni.mat']),'mni');
    else
        % Affine registration of Dartel Template with MNI space.
        %------------------------------------------------------------------
        fprintf('** Affine registering "%s" with MNI space **\n', nam);
        tpm        = fullfile(spm('Dir'),'tpm','TPM.nii');
        Mmni       = spm_get_space(tpm);
        Nt         = nifti(job.template{1});
        mni.affine = Mmni/spm_klaff(Nt,tpm);
        mni.code   = 'MNI152';
        save(fullfile(pth,[nam '_2mni.mat']),'mni', spm_get_defaults('mat.format'));
    end
    Mat    = mni.affine;
   %do_aff = true;
else
    Mat    = Nii.mat;
   %do_aff = false;
end

% Integrate a Dartel flow field
y0      = spm_dartel_integrate(Nii.dat,job.times,job.K);
if all(job.times == [0 1]),
    mat = Nii.mat0;
    Def = affine(y0,single(Mat));
else
    mat = Mat;
    Def = affine(y0,single(Nii.mat0));
end

%==========================================================================
% function [Def,mat] = get_id(job)
%==========================================================================
function [Def,mat] = get_id(job)
% Get an identity transform based on an image volume
[mat, dim] = spm_get_matdim(job.space{1});
Def = identity(dim, mat);

%==========================================================================
% function [Def,mat] = get_idbbvox(job)
%==========================================================================
function [Def,mat] = get_idbbvox(job)
% Get an identity transform based on bounding box and voxel size.
% This will produce a transversal image.
[mat, dim] = spm_get_matdim('', job.vox, job.bb);
Def = identity(dim, mat);

%==========================================================================
% function [Def,mat] = get_inv(job)
%==========================================================================
function [Def,mat] = get_inv(job)
% Invert a deformation field (derived from a composition of deformations)

NT          = nifti(job.space{:});
[Def0,mat0] = get_comp(job.comp);
M0          = mat0;
M1          = inv(NT.mat);
M0(4,:)     = [0 0 0 1];
M1(4,:)     = [0 0 0 1];
Def         = spm_diffeo('invdef',Def0,NT.dat.dim(1:3),M1,M0);
mat         = NT.mat;
Def         = spm_extrapolate_def(Def,mat);

%==========================================================================
% function fname = save_def(Def,mat,job)
%==========================================================================
function fname = save_def(Def,mat,job)
% Save a deformation field as an image

ofname = job.ofname;
if isempty(ofname), fname = {}; return; end;

[pth,nam] = fileparts(ofname);
if isfield(job.savedir,'savepwd')
    wd = pwd;
elseif isfield(job.savedir,'saveusr')
    wd = job.savedir.saveusr{1};
else
    wd = pwd;
end

fname = {fullfile(wd,['y_' nam '.nii'])};
dim   = [size(Def,1) size(Def,2) size(Def,3) 1 3];
dtype = 'FLOAT32-LE';
off   = 0;
scale = 1;
inter = 0;
dat   = file_array(fname{1},dim,dtype,off,scale,inter);

N      = nifti;
N.dat  = dat;
N.mat  = mat;
N.mat0 = mat;
N.mat_intent  = 'Aligned';
N.mat0_intent = 'Aligned';
N.intent.code = 'VECTOR';
N.intent.name = 'Mapping';
N.descrip     = 'Deformation field';
create(N);
N.dat(:,:,:,1,1) = Def(:,:,:,1);
N.dat(:,:,:,1,2) = Def(:,:,:,2);
N.dat(:,:,:,1,3) = Def(:,:,:,3);

%==========================================================================
% function fname = jac_def(Def,mat,job)
%==========================================================================
function fname = jac_def(Def,mat,job)
% Save Jacobian determinants of deformation field

ofname = job.ofname;
if isempty(ofname), fname = {}; return; end;

[pth,nam] = fileparts(ofname);
if isfield(job.savedir,'savepwd')
    wd = pwd;
elseif isfield(job.savedir,'saveusr')
    wd = job.savedir.saveusr{1};
else
    wd = pwd;
end

Dets  = spm_diffeo('def2det',Def)/det(mat(1:3,1:3));
Dets(:,:,[1 end]) = NaN;
Dets(:,[1 end],:) = NaN;
Dets([1 end],:,:) = NaN;

fname = {fullfile(wd,['j_' nam '.nii'])};
dim   = [size(Def,1) size(Def,2) size(Def,3) 1 1];
dtype = 'FLOAT32-LE';
off   = 0;
scale = 1;
inter = 0;
dat   = file_array(fname{1},dim,dtype,off,scale,inter);

N      = nifti;
N.dat  = dat;
N.mat  = mat;
N.mat0 = mat;
N.mat_intent  = 'Aligned';
N.mat0_intent = 'Aligned';
N.intent.code = 0;
N.intent.name = 'Mapping';
N.descrip     = 'Jacobian Determinants';
create(N);
N.dat(:,:,:,1,1) = Dets;

%==========================================================================
% function out = pull_def(Def,mat,job)
%==========================================================================
function out = pull_def(Def,mat,job)

PI      = job.fnames;
intrp   = job.interp;
intrp   = [intrp*[1 1 1], 0 0 0];
out     = cell(numel(PI),1);

if numel(PI)==0, return; end

if job.mask
    oM  = zeros(4,4);
    odm = zeros(1,3);
    dim = size(Def);
    msk = true(dim);
    for m=1:numel(PI)
        [pth,nam,ext,num] = spm_fileparts(PI{m});
        NI = nifti(fullfile(pth,[nam ext]));
        dm = NI.dat.dim(1:3);
        if isempty(num)
            j_range = 1:size(NI.dat,4);
        else
            num     = sscanf(num,',%d');
            j_range = num(1);
        end
        for j=j_range

            M0 = NI.mat;
            if ~isempty(NI.extras) && isstruct(NI.extras) && isfield(NI.extras,'mat')
                M1 = NI.extras.mat;
                if size(M1,3) >= j && sum(sum(M1(:,:,j).^2)) ~=0
                    M0 = M1(:,:,j);
                end
            end
            M   = inv(M0);
            if ~all(M(:)==oM(:)) || ~all(dm==odm)
                tmp = affine(Def,M);
                msk = tmp(:,:,:,1)>=1 & tmp(:,:,:,1)<=size(NI.dat,1) ...
                    & tmp(:,:,:,2)>=1 & tmp(:,:,:,2)<=size(NI.dat,2) ...
                    & tmp(:,:,:,3)>=1 & tmp(:,:,:,3)<=size(NI.dat,3);
            end
            oM  = M;
            odm = dm;
        end
    end
end

oM = zeros(4,4);
spm_progress_bar('Init',numel(PI),'Resampling','volumes completed');
for m=1:numel(PI)

    % Generate headers etc for output images
    %----------------------------------------------------------------------
    [pth,nam,ext,num] = spm_fileparts(PI{m});
    NI = nifti(fullfile(pth,[nam ext]));

    j_range = 1:size(NI.dat,4);
    k_range = 1:size(NI.dat,5);
    l_range = 1:size(NI.dat,6);
    if ~isempty(num)
        num = sscanf(num,',%d');
        if numel(num)>=1, j_range = num(1); end
        if numel(num)>=2, k_range = num(2); end
        if numel(num)>=3, l_range = num(3); end
    end

    NO = NI;
    if isfield(job.savedir,'savepwd')
        wd = pwd;
    elseif isfield(job.savedir,'saveusr')
        wd = job.savedir.saveusr{1};
    elseif isfield(job.savedir,'savesrc')
        wd = pth;
    else
        wd = pwd;
    end

    if sum(job.fwhm.^2)==0
        newprefix  = spm_get_defaults('normalise.write.prefix');
        NO.descrip = sprintf('Warped');
    else
        newprefix  = [spm_get_defaults('smooth.prefix') spm_get_defaults('normalise.write.prefix')];
        NO.descrip = sprintf('Smoothed (%gx%gx%g subopt) warped',job.fwhm);
    end
    if isfield(job,'prefix') && ~isempty(job.prefix)
        NO.dat.fname = fullfile(wd,[job.prefix nam ext]);
    else
        NO.dat.fname = fullfile(wd,[newprefix nam ext]);
    end
    dim            = size(Def);
    dim            = dim(1:3);
    NO.dat.dim     = [dim NI.dat.dim(4:end)];
    NO.dat.offset  = 0; % For situations where input .nii images have an extension.
    NO.mat         = mat;
    NO.mat0        = mat;
    NO.mat_intent  = 'Aligned';
    NO.mat0_intent = 'Aligned';
    if isempty(num)
        out{m}     = NO.dat.fname;
    else
        out{m}     = [NO.dat.fname, ',', num2str(num(1))];
    end
    NO.extras      = [];
    create(NO);

    % Smoothing settings
    vx  = sqrt(sum(mat(1:3,1:3).^2));
    krn = max(job.fwhm./vx,0.25);

    % Loop over volumes within the file
    %----------------------------------------------------------------------
    %fprintf('%s',nam);
    for j=j_range

        M0 = NI.mat;
        if ~isempty(NI.extras) && isstruct(NI.extras) && isfield(NI.extras,'mat')
            M1 = NI.extras.mat;
            if size(M1,3) >= j && sum(sum(M1(:,:,j).^2)) ~=0
                M0 = M1(:,:,j);
            end
        end
        M  = inv(M0);
        if ~all(M(:)==oM(:))
            % Generate new deformation (if needed)
            Y     = affine(Def,M);
        end
        oM = M;
        % Write the warped data for this time point
        %------------------------------------------------------------------
        for k=k_range
            for l=l_range
                C   = spm_diffeo('bsplinc',single(NI.dat(:,:,:,j,k,l)),intrp);
                dat = spm_diffeo('bsplins',C,Y,intrp);
                if job.mask
                    dat(~msk) = NaN;
                end
                if sum(job.fwhm.^2)~=0
                    spm_smooth(dat,dat,krn); % Side effects
                end
                NO.dat(:,:,:,j,k,l) = dat;
                %fprintf('\t%d,%d,%d', j,k,l);
            end
        end
    end
    %fprintf('\n');
    spm_progress_bar('Set',m);
end
spm_progress_bar('Clear');

%==========================================================================
% function out = push_def(Def,mat,job)
%==========================================================================
function out = push_def(Def,mat,job)
% Generate deformation, which is the inverse of the usual one (it is for "pushing"
% rather than the usual "pulling"). This deformation is affine transformed to
% allow for different voxel sizes and bounding boxes, and also to incorporate
% the affine mapping between MNI space and the population average shape.
%--------------------------------------------------------------------------

% Deal with desired bounding box and voxel sizes.
%--------------------------------------------------------------------------
if isfield(job.fov,'file')
    N1   = nifti(job.fov.file);
    mat0 = N1.mat;
    dim  = N1.dat.dim(1:3);
else
    bb   = job.fov.bbvox.bb;
    vox  = job.fov.bbvox.vox;
    [mat0, dim] = spm_get_matdim('', vox, bb);
end

M   = inv(mat0);
y0  = affine(Def,M);

if isfield(job,'weight') && ~isempty(job.weight) && ~isempty(job.weight{1})
    wfile = job.weight{1};
    Nw    = nifti(wfile);
    Mw    = Nw.mat;
    wt    = Nw.dat(:,:,:,1,1,1);
else
    wt    = [];
end

odm = zeros(1,3);
oM  = zeros(4,4);
PI  = job.fnames;
out = cell(numel(PI),1);
for m=1:numel(PI)

    % Generate headers etc for output images
    %----------------------------------------------------------------------
    [pth,nam,ext,num] = spm_fileparts(PI{m});
    NI = nifti(fullfile(pth,[nam ext]));
    j_range = 1:size(NI.dat,4);
    k_range = 1:size(NI.dat,5);
    l_range = 1:size(NI.dat,6);
    if ~isempty(num)
        num = sscanf(num,',%d');
        if numel(num)>=1, j_range = num(1); end
        if numel(num)>=2, k_range = num(2); end
        if numel(num)>=3, l_range = num(3); end
    end

    if isfield(job.savedir,'savepwd')
        wd = pwd;
    elseif isfield(job.savedir,'saveusr')
        wd = job.savedir.saveusr{1};
    elseif isfield(job.savedir,'savesrc')
        wd = pth;
    else
        wd = pwd;
    end

    NO = NI;
    if job.preserve
        NO.dat.scl_slope = 1.0;
        NO.dat.scl_inter = 0.0;
        NO.dat.dtype     = 'float32-le';
        if sum(job.fwhm.^2)==0
            newprefix  = [spm_get_defaults('deformations.modulate.prefix') spm_get_defaults('normalise.write.prefix')];
            NO.descrip = sprintf('Warped & Jac scaled');
        else
            newprefix  = [spm_get_defaults('smooth.prefix') spm_get_defaults('deformations.modulate.prefix') spm_get_defaults('normalise.write.prefix')];
            NO.descrip = sprintf('Smoothed (%gx%gx%g) warped Jac scaled',job.fwhm);
        end
    else
        if sum(job.fwhm.^2)==0
            newprefix  = spm_get_defaults('normalise.write.prefix');
            NO.descrip = sprintf('Warped');
        else
            newprefix  = [spm_get_defaults('smooth.prefix') spm_get_defaults('normalise.write.prefix')];
            NO.descrip = sprintf('Smoothed (%gx%gx%g opt) warped',job.fwhm);
        end
    end
    if isfield(job,'prefix') && ~isempty(job.prefix)
        NO.dat.fname = fullfile(wd,[job.prefix nam ext]);
    else
        NO.dat.fname = fullfile(wd,[newprefix nam ext]);
    end
    NO.dat.dim     = [dim NI.dat.dim(4:end)];
    NO.dat.offset  = 0; % For situations where input .nii images have an extension.
    NO.mat         = mat0;
    NO.mat0        = mat0;
    NO.mat_intent  = 'Aligned';
    NO.mat0_intent = 'Aligned';

    if isempty(num)
        out{m}     = NO.dat.fname;
    else
        out{m}     = [NO.dat.fname, ',', num2str(num(1))];
    end

    NO.extras      = [];
    create(NO);

    % Smoothing settings
    vx  = sqrt(sum(mat0(1:3,1:3).^2));
    krn = max(job.fwhm./vx,0.25);

    % Loop over volumes within the file
    %----------------------------------------------------------------------
    fprintf('%s',nam); drawnow;
    for j=j_range

        % Need to resample the mapping by an affine transform
        % so that it maps from voxels in the native space image
        % to voxels in the spatially normalised image.
        %------------------------------------------------------------------
        M0 = NI.mat;
        if ~isempty(NI.extras) && isstruct(NI.extras) && isfield(NI.extras,'mat')
            M1 = NI.extras.mat;
            if size(M1,3) >= j && sum(sum(M1(:,:,j).^2)) ~=0
                M0 = M1(:,:,j);
            end
        end

        M   = mat\M0;
        dm  = [size(NI.dat),1,1,1,1];
        if ~all(dm(1:3)==odm) || ~all(M(:)==oM(:))
            % Generate new deformation (if needed)
            y   = zeros([dm(1:3),3],'single');
            for d=1:3
                yd = y0(:,:,:,d);
                for x3=1:size(y,3)
                    y(:,:,x3,d) = single(spm_slice_vol(yd,M*spm_matrix([0 0 x3]),dm(1:2),[1 NaN]));
                end
            end
        end

        odm = dm(1:3);
        oM  = M;
        % Write the warped data for this time point.
        %------------------------------------------------------------------
        for k=k_range
            for l=l_range
                f  = single(NI.dat(:,:,:,j,k,l));
                if isempty(wt)
                    if ~job.preserve
                        % Unmodulated - note the slightly novel procedure
                        [f,c] = spm_diffeo('push',f,y,dim);
                        spm_smooth(f,f,krn); % Side effects
                        spm_smooth(c,c,krn); % Side effects
                        f = f./(c+0.001);
                    else
                        % Modulated, by pushing
                        scal = abs(det(NI.mat(1:3,1:3))/det(NO.mat(1:3,1:3))); % Account for vox sizes
                        f    = spm_diffeo('push',f,y,dim)*scal;
                        spm_smooth(f,f,krn); % Side effects
                    end
                else
                    if isequal(size(wt),size(f)) && sum((Mw(:)-M0(:)).^2)<1e-6
                        wtw = single(wt);
                        f   = single(f.*wt);
                    else
                        wtw = zeros(size(f),'single');
                        for z=1:size(wt,3)
                            Mz = Mw\M0*[1 0 0 0; 0 1 0 0; 0 0 1 z; 0 0 0 1];
                            wtw(:,:,z) = single(spm_slice_vol(wt,Mz,[size(f,1),size(f,2)],1));
                        end
                    end
                    if ~job.preserve
                        % Unmodulated - note the slightly novel procedure
                        f = spm_diffeo('push',f.*wtw,y,dim);
                        c = spm_diffeo('push',wtw,y,dim);
                        spm_smooth(f,f,krn); % Side effects
                        spm_smooth(c,c,krn); % Side effects
                        f = f./(c+0.001);
                    else
                        % Modulated, by pushing
                        scal = abs(det(NI.mat(1:3,1:3))/det(NO.mat(1:3,1:3))); % Account for vox sizes
                        f    = spm_diffeo('push',f.*wtw,y,dim)*scal;
                        spm_smooth(f,f,krn); % Side effects
                    end
                    clear wtw
                end
                NO.dat(:,:,:,j,k,l) = f;
                fprintf('\t%d,%d,%d', j,k,l); drawnow;
            end
        end
    end
    fprintf('\n'); drawnow;
end

%==========================================================================
% function out = surf_def(Def,mat,job)
%==========================================================================
function out = surf_def(Def,mat,job)
filenames = job.surface;
out       = cell(numel(filenames),1);
for i=1:numel(filenames)
    fname     = deblank(job.surface{i});
    [pth,nam] = fileparts(fname);
    fprintf('%s\n', nam);
    d      = size(Def);
    tmp    = double(reshape(Def,[d(1:3) 1 d(4)]));
    Tmesh  = spm_swarp(fname, tmp,mat);
    if isfield(job.savedir,'savepwd')
        wd = pwd;
    elseif isfield(job.savedir,'saveusr')
        wd = job.savedir.saveusr{1};
    elseif isfield(job.savedir,'savesrc')
        wd = pth;
    else
        wd = pwd;
    end
    filename = fullfile(wd,[nam,'_warped', '.gii']);
    save(gifti(Tmesh), filename);
    out{i}   = filename;
end

%==========================================================================
% function Def = affine(y,M)
%==========================================================================
function Def = affine(y,M)
Def          = zeros(size(y),'single');
Def(:,:,:,1) = y(:,:,:,1)*M(1,1) + y(:,:,:,2)*M(1,2) + y(:,:,:,3)*M(1,3) + M(1,4);
Def(:,:,:,2) = y(:,:,:,1)*M(2,1) + y(:,:,:,2)*M(2,2) + y(:,:,:,3)*M(2,3) + M(2,4);
Def(:,:,:,3) = y(:,:,:,1)*M(3,1) + y(:,:,:,2)*M(3,2) + y(:,:,:,3)*M(3,3) + M(3,4);

%==========================================================================
% function Def = identity(d,M)
%==========================================================================
function Def = identity(d,M)
[y1,y2]   = ndgrid(single(1:d(1)),single(1:d(2)));
Def       = zeros([d 3],'single');
for y3=1:d(3)
    Def(:,:,y3,1) = y1*M(1,1) + y2*M(1,2) + (y3*M(1,3) + M(1,4));
    Def(:,:,y3,2) = y1*M(2,1) + y2*M(2,2) + (y3*M(2,3) + M(2,4));
    Def(:,:,y3,3) = y1*M(3,1) + y2*M(3,2) + (y3*M(3,3) + M(3,4));
end


% LFDispVidCirc - visualize a 4D light field animating a circular path through two dimensions
%
% Usage:
%     FigureHandle = LFDispVidCirc( LF )
%     FigureHandle = LFDispVidCirc( LF, [RenderOptions], [ScaleFactor] )
%
%
% Inputs:
%
%     LF : a colour or single-channel light field, and can a floating point or integer format. For
%          display, it is converted to 8 bits per channel. If LF contains more than three colour
%          channels, as is the case when a weight channel is present, only the first three are used.
%
% Optional Inputs:
%
%     RenderOptions : struct controlling rendering
%        .PathRadius_percent : radius of the circular path taken by the viewpoint. Values that are
%                              too high can result in collision with the edges of the lenslet image,
%                              while values that are too small result in a less impressive
%                              visualization. The default value is 60%.
%        .FrameDelay         : delay between frames, in seconds. Default 1/30.
%        .NumCycles          : how many times the circular path should repeat. Default inf.
%        .FramesPerCycle     : how many frames along a single cycle. Default 20.
%
%     ScaleFactor : Adjusts the size of the display -- 1 means no change, 2 means twice as
%                   big, etc. Integer values are recommended to avoid scaling artifacts.
%
% Outputs:
%
%     FigureHandle
%
%
% User guide: LFToolbox.pdf
% See also:  LFDisp, LFDispMousePan, LFDispLawnmower, LFDispProj, LFDispTiles

% Copyright (c) 2013-2020 Donald G. Dansereau

function FigureHandle = LFDispVidCirc( LF, RenderOptions, ScaleFactor )

%---Defaults---
ScaleFactor = LFDefaultVal('ScaleFactor', 1);
RenderOptions = LFDefaultField( 'RenderOptions', 'PathRadius_percent', 60 );
RenderOptions = LFDefaultField( 'RenderOptions', 'FrameDelay', 1/30 );
RenderOptions = LFDefaultField( 'RenderOptions', 'NumCycles', inf );
RenderOptions = LFDefaultField( 'RenderOptions', 'FramesPerCycle', 20 );

%---Check for mono and clip off the weight channel if present---
Mono = (ndims(LF) == 4);
if( ~Mono )
    LF = LF(:,:,:,:,1:3);
end

%---Rescale for 8-bit display---
if( isfloat(LF) )
    LF = uint8(LF ./ max(LF(:)) .* 255);
else
    LF = uint8(LF.*(255 / double(intmax(class(LF)))));
end

%---Setup the motion path---
[TSize,SSize, ~,~] = size(LF(:,:,:,:,1));
TCent = (TSize-1)/2 + 1;
SCent = (SSize-1)/2 + 1;

t = 0;
RotRad = TCent*RenderOptions.PathRadius_percent/100;
NumFrames = RenderOptions.NumCycles * RenderOptions.FramesPerCycle;

%---Setup the display---
TIdx = round(TCent + RotRad);
SIdx = round(SCent);
[ImageHandle,FigureHandle] = LFDispSetup( squeeze(LF(TIdx,SIdx,:,:,:)), ScaleFactor );

while(1)
    TVal = TCent + RotRad * cos( 2*pi*t/RenderOptions.FramesPerCycle );
    SVal = SCent + RotRad * sin( 2*pi*t/RenderOptions.FramesPerCycle );

    SIdx = round(SVal);
    TIdx = round(TVal);

    CurFrame =  squeeze(LF( TIdx, SIdx, :,:,: ));
    set(ImageHandle,'cdata', CurFrame );

    pause(RenderOptions.FrameDelay)
    t = t + 1;

	if( t > NumFrames )
		break;
	end
end


function [stimset,stim_full] = StimulusKeyPreprocessing(frame_turn,i_fish,isplotting)
% [stimset,stim_full] = StimulusKeyPreprocessing(frame_turn,i_fish,'isplotting');

if exist('isplotting','var'),
    isPlotting = true; % plot parsing process for manual inspection!
else
    isPlotting = false;
end
% Manual inputs needed for this function:
% - protocol sequence and naming, as in 'block_raw' and 'stimset' initialization
% - substitution array 'M_range', to substitute original stimulus code to a
% tighter range that is better defined for plotting (both for plotting values and stimulus bar)

%% Load stimulus

st = frame_turn(:,17)'; % from raw file

% cap abnormally high values in raw values
temp = sort(st,'descend');
thr = temp(round(length(st)/100)); % arb. thr % used /10 up till 1/27/16 for all fish except Fish12
st(st>thr) = thr+1; % assign cap value to be thr+1

xv = 0:thr+1; % x-vector for histogram bins

% Manual inspection
% figure;
% hist(st,xv);

% (continue by default) find bins with significantly high counts, i.e.
% whole range/set of raw stimulus keys, store in 'M_range_raw'
% use to manually define standardized 'M_range'
counts = hist(st,xv);
thr_counts = round(length(st)/500);
M_range_raw = xv(counts>thr_counts);
if isPlotting,
    disp(['M_range_raw = ' num2str(M_range_raw)]); % use this to adjust the substitution array, M_range, manually after this inspection!%%%%%%%%%%%%%%%%%%%%%%
end

%% Raw Stimlus Key ----------------------------------------------------------------------------- MANUAL INPUT!

% Manually input protocol sequence
% stored in 'block', based on the raw stimulus code
%% Manual: params for each fish

% write out protocol sequence for each block; write stimulus sets within block in arrays within cell
if i_fish == 8,
    nBlocks = 3;
    block_raw = cell(nBlocks,1); % number of blocks
    block_raw{1} = {[2,3,4,5],[12,13,12,14,12,15],99,};
    block_raw{2} = {[2,100,2,3,4,100,4,5],[2,100,2,3,4,100,4,5],[12,100,12,13,12,100,12,14,12,100,12,15],99};
    block_raw{3} = {[2,3,4,5],[12,13,12,14,12,15],99};

    stimset = [];
    stimset(1).name = 'PT';
    stimset(1).ij = [1,1; 3,1;];
    stimset(2).name = 'OMR';
    stimset(2).ij = [1,2; 3,2;];
    stimset(3).name = 'Spontaneous';
    stimset(3).ij = [1,3; 2,4;3,3];
    stimset(4).name = 'PT-shock';
    stimset(4).ij = [2,1; 2,2;];
    stimset(5).name = 'OMT-shock';
    stimset(5).ij = [2,3];

    % M_range_raw = [2,3,4,5,12,13,14,15,99,100]; % for Fish 8
    M_range =       [3,1,3,2, 3,10,11,12, 0,16]; % standardized

elseif i_fish >=9 && i_fish <=11,
    nBlocks = 3;
    block_raw = cell(nBlocks,1); % number of blocks
    block_raw{1} = {[2,3,4,5],[12,13,12,14,12,15],99,[23,22],[30,31,30,32],[42,43,44,45]};
    block_raw{2} = {[2,100,2,3,4,100,4,5],99,[12,100,12,13,12,100,12,14,12,100,12,15],99,[23,22],[30,31,30,32],[42,100,42,43,44,100,44,45]};
    block_raw{3} = {[2,3,4,5],[12,13,12,14,12,15],99,[23,22],[30,31,30,32],[42,43,44,45],[2,3,4,5],[12,13,12,14,12,15]};

    stimset = [];
    stimset(1).name = 'PT';
    stimset(1).ij = [1,1; 3,1; 3,7];
    stimset(2).name = 'OMR';
    stimset(2).ij = [1,2; 3,2; 3,8];
    stimset(3).name = 'Spontaneous';
    stimset(3).ij = [1,3; 2,4; 3,3];
    stimset(4).name = 'Dot/prey';
    stimset(4).ij = [1,4; 2,5; 3,4];
    stimset(5).name = 'Looming';
    stimset(5).ij = [1,5; 2,6; 3,5];
    stimset(6).name = 'Red|Blue';
    stimset(6).ij = [1,6; 3,6];
    stimset(7).name = 'PT-shock';
    stimset(7).ij = [2,1];
    stimset(8).name = 'OMR-shock';
    stimset(8).ij = [2,3];
    stimset(9).name = 'RB-shock';
    stimset(9).ij = [2,7];

    % M_range_raw = [2,3,4,5,12,13,14,15,22,23,30,31,32,42,43,44,45,99,100] % for Fish 10
    M_range =       [3,1,3,2, 3,10,11,12,13, 0, 3,14,15,23,21,23,22, 0,16]; % standardized

elseif i_fish >=12 && i_fish <=14,
    nBlocks = 2;
    block_raw = cell(nBlocks,1); % number of blocks
    block_raw{1} = {[2,3,4,5],[12,13,12,14,12,15,12,16],[0,12],99,[30,31,30,32]};
    block_raw{2} = {[2,3,4,5],[12,13,12,14,12,15,12,16],[0,12],99,[30,31,30,32]};

    stimset = [];
    stimset(1).name = 'PT';
    stimset(1).ij = [1,1; 2,1];
    stimset(2).name = 'OMR';
    stimset(2).ij = [1,2; 2,2];
    stimset(3).name = 'DF';
    stimset(3).ij = [1,3; 2,3];
    stimset(4).name = 'Spontaneous';
    stimset(4).ij = [1,4; 2,4];
    stimset(5).name = 'Looming';
    stimset(5).ij = [1,5; 2,5];

    % M_range_raw = [0,2,3,4,5,12,13,14,15,16,30,31,32,99] % for Fish 12
    M_range =       [0,3,1,3,2, 3,10, 9,11,12, 3,14,15, 4]; % standardized

% elseif i_fish == 12 || i_fish == 13 || i_fish == 14,
%     nBlocks = 1;
%     block_raw = cell(nBlocks,1); % number of blocks
%
%     block_raw{1} = {[0],[3],[4]};
%
%     stimset = [];
%     stimset(1).name = 'pre-para';
%     stimset(1).ij = [1,1];
%     stimset(2).name = 'add para';
%     stimset(2).ij = [1,2];
%     stimset(3).name = 'post-para';
%     stimset(3).ij = [1,3];
%
%     % M_range_raw = [1,3,4] % for Fish 10
%     M_range =       [0,3,4]; % standardized
elseif ismember(i_fish,[15,17,18]),
%     M_range_raw = [2   3   4   5  12  13  14  15  16  17  18  99]
    nBlocks = 2;
    block_raw = cell(nBlocks,1); % number of blocks
    block_raw{1} = {[2,3,4,5],[12,13,12,14,12,15,12,16],[17,18],99,[30,31,30,32]};
    block_raw{2} = {[2,3,4,5],[12,13,12,14,12,15,12,16],[17,18],99,[30,31,30,32]};

    stimset = [];
    stimset(1).name = 'PT';
    stimset(1).ij = [1,1; 2,1];
    stimset(2).name = 'OMR';
    stimset(2).ij = [1,2; 2,2];
    stimset(3).name = 'DF';
    stimset(3).ij = [1,3; 2,3];
    stimset(4).name = 'Spontaneous';
    stimset(4).ij = [1,4; 2,4];
    stimset(5).name = 'Looming';
    stimset(5).ij = [1,5; 2,5];

    % M_range_raw = [2,3,4,5,12,13,14,15,16,17,18,30,31,32,99] % for Fish 12
    M_range =       [3,1,3,2, 3,10, 9,11,12, 3, 0, 3,14,15, 4]; % standardized

% elseif i_fish==16,
%     %     M_range_raw = [2   3   4   5  12  13  14  15  16  17  18  99]
%     nBlocks = 1;
%     block_raw = cell(nBlocks,1); % number of blocks
%     block_raw{1} = {31,[2,3,4,5],[12,13,12,14,12,15,12,16],31};
%
%     stimset = [];
%     stimset(1).name = 'NA';
%     stimset(1).ij = [1,1; 2,1];
%     stimset(2).name = 'PT';
%     stimset(2).ij = [1,1; 2,1];
%     stimset(3).name = 'OMR';
%     stimset(3).ij = [1,2; 2,2];
%     stimset(4).name = 'NA';
%     stimset(4).ij = [1,3; 2,3];
%
%
%     % M_range_raw = [2,3,4,5,12,13,14,15,16,31] % for Fish 12
%     M_range =       [3,1,3,2, 3,10, 9,11,12,0]; % standardized

else % inspect manually to set these manual params
    % set M_range here after first inspection of M_range_raw:
    M_range =       [3,1,3,2, 9,10,11,12, 0,13, 3,14,15, 4,16]; % standardized

    M_replace = zeros(1,max(M_range_raw)+1); % starting from zero
    for i = 0:max(M_range_raw),
        ix = find(i==M_range_raw);
        if ~isempty(ix),
            M_replace(i+1) = M_range(ix);
        end
    end

    stim_full = zeros(size(st));
    for i = 1:length(M_range_raw),
        stim_full(st==M_range_raw(i)) = M_range(i);
    end

    figure;
    plot(stim_full);

    stimset = [];
    return;
end
%% Raw stimulus key:
% PT:Red/Dark
% 2,4: Red|Red
% 3: Dark|Red
% 5: Red|Dark
%
% PT:Red/Blue
% 42,44: Red|Red
% 43: Blue|Red
% 45: Red|Blue

% OMR
% 12: Red
% 13: OMR F
% 14: OMR R
% 15: OMR L
% 16: OMR?

% Spont
% 99: same as black
%
% Dot(prey)
% 22: Dot
% 23: No dot
%
% Looming
% 30: Red
% 31: Blob R
% 32: Blob L

%% Standardized stimulus key:
% (Define normalized code, e.g. same stimulus under different raw stim keys can be unified)

% 0 = all black; 1 = phototaxis R; 2 = phototaxis L; 3 = all white; 4 = all gray;
% 5 = gray/black; 6 = white/gray; 7 = black/gray; 8 = gray/white.
% 9 = backward grating
% 10 = forward grating (very slow, more for calibration)
% 11 = rightward grating
% 12 = leftward grating
% 13 = Dot
% 14 = looming L %Blob R
% 15 = looming R %Blob L

% 16 = electric shock (spike)

% 21,22,23 = 'red|blue R','blue|red L','red|red'

% % 0 = all black; 1 = black/white; 2 = white/black; 3 = all white; 4 = all gray;
% % 5 = gray/black; 6 = white/gray; 7 = black/gray; 8 = gray/white.
% % 9 = backward grating... %%OMR baseline = not moving?? grey??
% % 10 = forward grating (very slow, more for calibration)
% % 11 = rightward grating
% % 12 = leftward grating
% % 13 = Dot
% % 14 = Blob R
% % 15 = Blob L
%
% % 16 = electric shock (spike)
%
% % 21,22,23 = 'red|blue','blue|red','red|red'

%% correct transient frames
% short method: st_rounded = interp1(M_range,M_range,st,'nearest');
% but this doesn't work if a frame is different from both the one
% before and the one after. Use loop below to force a manual 'round'
% to the closer one of the 2 neighbors.
temp = diff([0,st]);
switches = find(temp);
trans = switches(diff(switches)==1);

for i = 1:length(trans),
    j = trans(i);
    if j>1,
        st(j) = st(j-1) + round((st(j)-st(j-1))/abs(st(j+1)-st(j-1))); % round to nearest of st(j-1) and st(j+1)
    end
end

% (loop until all transient ones are corrected)
temp = diff([0,st]);
switches = find(temp);
trans = switches(diff(switches)==1);
count = 0;
while ~isempty(trans),
    for i = 1:length(trans),
        j = trans(i);
        st(j) = st(j-1) + round((st(j)-st(j-1))/abs(st(j+1)-st(j-1))); % round to nearest of st(j-1) and st(j+1)
    end
    temp = diff([0,st]);
    switches = find(temp);
    trans = switches(diff(switches)==1);
    count = count+1;
    if count>5,
        disp('count>5??');
        break;
    end
end

%% Standardize stimulus keys for both 'stim' and 'block', based on 'M_range_raw'->'M_range'
% (tricky for cell arrays) custom method:
% make a replacement matrix 'M_replace' to substitude raw stimulus key in 'block_raw'
%  to normalized key, stored in new cell array 'block'
M_replace = zeros(1,max(M_range_raw)+1); % starting from zero
for i = 0:max(M_range_raw),
    ix = find(i==M_range_raw);
    if ~isempty(ix),
        M_replace(i+1) = M_range(ix);
    end
end

stim_full = zeros(size(st));
block = cell(nBlocks,1);
for i = 1:length(M_range_raw),
    stim_full(st==M_range_raw(i)) = M_range(i);
    for j = 1:nBlocks,
        block{j} = cellfun(@(x) M_replace(x+1),block_raw{j},'UniformOutput',0);
    end
end
stim_full_1 = horzcat(stim_full,-1);% set a virtual next frame to a invalid number for segmentation later

%% Stimulus set segmentation, based on 'block' (manual input of protocol sequence)
% reduce 'stim' to a sequence of keys without consecutive repeats
ix_singles = [1,find(diff(stim_full_1))+1]; % index array to map back to raw indices (frame-number)
singles = stim_full_1(ix_singles); % sequence of keys without consecutive repeats.
% '_sg' below stands for 'singles'. Manipulations below in both raw and singles, in parallel.

jump = 1; % searching for 'jumps' in 'singles'
% Based on the known protocol sequence for each set, look for first
% mismatch in 'singles', i.e. position where the next set begins, stored
% as 'block_change(_sg)' and 'set_start(_sg)'.
block_change = cell(nBlocks,1);
block_change_sg = cell(nBlocks,1);
flag_isbreakagain = false;
for I = 1:nBlocks,
    nSets = length(block{I});
    block_change{I} = zeros(1,nSets);
    block_change_sg{I} = zeros(1,nSets);
    for J = 1:nSets,
        jump_last = jump;
        pattern = block{I}{J}; % protocol sequence for this set

        % this is sort of awkward. Instead of padding the 'already-found' sets with zeros,
        % one could also search the cropped array and adjusted the index.
        % Now 'comparison' is aligned with stim, works.
        textile = repmat(pattern,1,ceil(length(singles)/length(pattern))); % tile, for use below

        template = zeros(size(singles));
        ixs_fill = jump_last:length(singles);
        template(ixs_fill) = textile(1:length(ixs_fill)); % crop the 'textile' to fill the space after jump_last

        singles_temp = singles;
        if jump_last>1,
            singles_temp(1:jump_last-1) = 0;
        end
        comparison = (singles_temp-template);
        % find the position where the next set begins
        jump = find((comparison),1,'first');

        %% contingency plan: if start of the new set is different from
        % expected protocol ('block')
        count_stuck = 0;
        count_stuck2 = 0;
        while jump-jump_last<length(pattern),
            count_stuck = count_stuck+1;
            if count_stuck>length(pattern),
                count_stuck = 0;
                jump_last = jump_last+1;
                count_stuck2 = count_stuck2+1;
                disp(['count_stuck2=' num2str(count_stuck2)]);
                if count_stuck2>10, % arb. thres
                    disp('count_stuck2>10');
                    break;
                end
            end

            pattern_ = circshift(pattern,-count_stuck,2);
            textile = repmat(pattern_,1,ceil(length(singles)/length(pattern_)));

            template = zeros(size(singles));
            ixs_fill = jump_last:length(singles);
            template(ixs_fill) = textile(1:length(ixs_fill));

            singles_temp = singles;
            if jump_last>1,
                singles_temp(1:jump_last-1) = 0;
            end
            comparison = (singles_temp-template);

            jump = find((comparison),1,'first');

        end
        %% save
        if isempty(jump), % reached end of experiment!
            flag_isbreakagain = true;
            break;
        end
        block_change_sg{I}(J) = jump;
        block_change{I}(J) = ix_singles(jump);
    end
    if flag_isbreakagain,
        break;
    end
end

% eliminate empty sets
for I = 1:nBlocks,
    nSets = length(block_change{I});
    if nSets == 0,
        block_change{I} = [];
        block_change_sg{I} = [];
        block{I} = [];
        nBlocks = I-1;
    else
        for J = 1:nSets,
            if block_change{I}(J) == 0,
                block_change{I}(J:end) = [];
                block_change_sg{I}(J:end) = [];
                if length(block{I})>= J,
                    block{I}(J:end) = [];
                end
                break;
            end
        end
    end
end

% block_change is basically start of next set. shift by one set to get 'set_start'.
set_start = cell(nBlocks,1);
set_start_sg = cell(nBlocks,1);
set_stop = cell(nBlocks,1);
set_stop_sg = cell(nBlocks,1);
for I = 1:nBlocks,
    nSets = length(block{I});
    set_start{I} = zeros(1,nSets);
    set_start_sg{I} = zeros(1,nSets);
    if I == 1,
        set_start{I}(1) = 1;
        set_start_sg{I}(1) = 1;
    else
        set_start{I}(1) = block_change{I-1}(end);
        set_start_sg{I}(1) = block_change_sg{I-1}(end);
    end
    set_start{I}(2:end) = block_change{I}(1:end-1);
    set_start_sg{I}(2:end) = block_change_sg{I}(1:end-1);
    set_stop{I} = block_change{I}-1;
    set_stop_sg{I} = block_change_sg{I}-1;
end

%% Visualize current segmentation of blocks
if isPlotting,
    figure;
    hold on;
    plot(stim_full_1);
    for I = 1:nBlocks,
        for J = 1:length(block_change{I}),
            x = block_change{I}(J);
            plot([x,x],[-1,max(M_range)+1],'r--')
            ylim([-1,max(M_range)+1]);
        end
    end
end
%% Find shift-corrections for each stim-set, used for averaging later

%% Find periods for each set-stype, and organize info by type into 'stimset'.
for i_ss = 1:length(stimset),
    % find empty sets (for this particular experiment) and delete ij's in 'stimset'
    for i_SetRep = 1:size(stimset(i_ss).ij,1),
        I = stimset(i_ss).ij(i_SetRep,1);
        J = stimset(i_ss).ij(i_SetRep,2);
        if I>length(block) || J>length(block{I}),
            stimset(i_ss).ij(i_SetRep:end,:) = [];
            break;
        end
    end
    nSetReps = size(stimset(i_ss).ij,1);
    if nSetReps == 0,
       stimset(i_ss) = [];
       break;
    end

    %% get period from first set of block
    I = stimset(i_ss).ij(1,1);
    J = stimset(i_ss).ij(1,2);
    pattern = block{I}{J}; % protocol sequence (singles) in normalized code
    stimset(i_ss).pattern = pattern;

    % find periodicity
    if length(pattern)==1, % exception, e.g. 'Spontaneous' set, single stimlus key held for full duration of set
        if J < length(block{I}),
            next_start = set_start{I}(J+1);
        elseif J == length(block{I}),
            if I < nBlocks,
                next_start = set_start{I+1}(1);
            else
                next_start = length(stim_full_1);
            end
        end
        stimset(i_ss).period = next_start - set_start{I}(J);
    else % find periodicity based on stimlus key repetition
        ix_start = set_start_sg{I}(J);
        segment = singles(ix_start:ix_start+length(pattern)-1);

        for i_segment = 1:length(segment),
            if length(find(segment==segment(i_segment)))==1, % use a stimulus that appears only once per period
                % find period
                start_s = set_start_sg{I}(J) + i_segment -1;
                stop_s = start_s + find(singles(1,start_s+1:end)==segment(i_segment),1,'first');
                stimset(i_ss).period = ix_singles(stop_s+1)-ix_singles(start_s+1); % shift by 1!! in case first stim is not intact
                break;
            end
        end
    end

    %%
    %     stimset(i_ss).period % assigned above

    stimset(i_ss).rawstarts = zeros(1,nSetReps);
    stimset(i_ss).rawstops = zeros(1,nSetReps);
    stimset(i_ss).starts = zeros(1,nSetReps);
    stimset(i_ss).stops = zeros(1,nSetReps);
    stimset(i_ss).nReps = zeros(1,nSetReps);
    for i_SetRep = 1:nSetReps,
        I = stimset(i_ss).ij(i_SetRep,1);
        J = stimset(i_ss).ij(i_SetRep,2);
        stimset(i_ss).rawstarts(i_SetRep) = set_start{I}(J);
        stimset(i_ss).rawstops(i_SetRep) = block_change{I}(J)-1;

        if length(pattern)==1, % exception, e.g. 'Spontaneous' set, single stimlus key held for full duration of set
            stimset(i_ss).starts(i_SetRep) = stimset(i_ss).rawstarts(i_SetRep);
            % length of following occurences need to match period
            % (period primitively extracted from first occurence)
%             i_start = stimset(i_ss).rawstarts(i_SetRep);
            i_stop = stimset(i_ss).rawstops(i_SetRep);

            stimset(i_ss).nReps(i_SetRep) = 1;
            ix = stimset(i_ss).starts(i_SetRep) + stimset(i_ss).period - 1;
            if ix < i_stop,
                stimset(i_ss).stops(i_SetRep) = ix;
            else
                stimset(i_ss).stops(i_SetRep) = i_stop;
            end

        else
            % circshift to find intact periods
            pattern = stimset(i_ss).pattern;

            ix_start = set_start_sg{I}(J);
            ix_stop = set_stop_sg{I}(J);

            % find 'shift' (shift = 1 means starting from 2nd in 'singles')
            segment = singles(ix_start:ix_start+length(pattern)-1);
            shift = 0;
            while ~isequal(segment,pattern),
                shift = shift+1;
                segment = singles(ix_start+shift:ix_start+length(pattern)-1+shift);
            end
            stimset(i_ss).starts(i_SetRep) = ix_singles(ix_start+shift);
            % round to period within this set
            nReps = floor((ix_stop-ix_start-shift+1)/length(pattern));
            stimset(i_ss).nReps(i_SetRep) = nReps;

            ix = stimset(i_ss).starts(i_SetRep) + nReps*stimset(i_ss).period - 1;
            if ix>stimset(i_ss).rawstops(i_SetRep),
                nReps = nReps-1;
            end
            stimset(i_ss).stops(i_SetRep) = stimset(i_ss).starts(i_SetRep) + nReps*stimset(i_ss).period - 1;
        end
    end
end

% discard very first period because of potential artifacts at beginning of
% entire experiment
if stimset(1).nReps(1)>1,
    stimset(1).nReps(1) = stimset(1).nReps(1) - 1;
    stimset(1).starts(1) = stimset(1).starts(1) + stimset(1).period;
end

end


function test_xformInplaneToVolume
%Validate transforming data from INPLANE view to VOLUME (gray) view
%
%  test_xformInplaneToVolume()
%
% Tests: initHiddenGray, loadMeanMap, loadCorAnal, ip2volParMap,
% ip2volCorAnal, ip2volTSeries
%
% INPUTS
%  No inputs
%
% RETURNS
%  No returns
%
% Example: test_getCurDataROI()
%
% See also MRVTEST
%
% Copyright Stanford team, mrVista, 2011

%% Initialize the key variables and data path
% Data directory (where the mrSession file is located)
dataDir = mrtInstallSampleData('functional','mrBOLD_01');

% This is the validation file
val = mrtGetValididationData('ipToVolumeData');

% These are the items we stored in the validation file
%
% val.codim      = size(co);
% val.comed      = nanmedian(co);
% val.cosample   = co(1000);
% val.mapdim     = size(map);
% val.mapmed     = nanmedian(map);
% val.mapsample  = map(1000);
% val.tSdim     = size(tSeries);
% val.tSsample  = tSeries(1000);
%
% save(vFile, '-struct', 'val')

%% Retain original directory, change to data directory
curDir = pwd;
cd(dataDir);

% There can be several data types - name the one you want to probe
dataType = 'Original';

% Which scan number from that data type?
scan = 1;

%% Get data structure:
ip  = initHiddenInplane(); % Foregoes interface - loads data silently
vol = initHiddenGray();

%% Set dataTYPE:
ip  = viewSet(ip, 'Current DataType', dataType); % Data type
vol = viewSet(vol, 'Current DataType', dataType); % Data type

%% Load coranal and mean map into INPLANE view
ip = loadCorAnal(ip);
ip = loadMeanMap(ip);

%% Transform data to VOLUME view
vol = ip2volParMap( ip, vol, scan, [], 'linear', 1);
vol = ip2volCorAnal(ip, vol, scan, -1);
vol = ip2volTSeries(ip, vol, scan);

co  = viewGet(vol, 'scan coherence', scan);
map = viewGet(vol, 'scan map', scan);
tSeries = loadtSeries(vol, scan, 1); %We can hardcode slice since gray view only has 1 slice

%% Go home
cd(curDir);

%% Validate..

% check the dimensions of coherence map and mean map
assertEqual(val.codim, size(co));
assertEqual(val.mapdim, size(map));
assertEqual(val.tSdim, size(tSeries));

% check the median value of the coherence map and mean map
assertElementsAlmostEqual(val.comed,nanmedian(co));
assertElementsAlmostEqual(val.mapmed,nanmedian(map));

% check the values of the mean map and coherence map for an arbitrary voxel
% (to make sure the sequence is correct)
assertElementsAlmostEqual(val.cosample, co(1000));
assertElementsAlmostEqual(val.mapsample, map(1000));

% check the values of the time series for the max and an arbitrary voxel
assertElementsAlmostEqual(val.tSsample, tSeries(1000));
assertElementsAlmostEqual(val.tSmax,nanmax(tSeries(:)));

%% Cleanup
% clean up vistadata repository because this test script wrote new data
% test_CleanUpSVN

mrvCleanWorkspace;


function vis_im = visualise_bboxes(id, objectNames, cmap, rgb_dir, detection_dir)

font_size = 25;

city = strtok(id, '_');

image_filename = fullfile(rgb_dir, city, [id '_leftImg8bit.png']);
detection_filename = fullfile(detection_dir, [id '_leftImg8bit.mat']);

rgb_im = imread(image_filename);
temp = load(detection_filename);
detections = temp.dets.annotation;
vis_im = rgb_im;

if ~isfield(detections, 'object') || isempty(detections.object)
    return;
end

rectangle_position = zeros(length(detections), 4);
colour = zeros(length(detections), 3);
text_strings = cell(length(detections), 1);
% we eventually want to display the bboxes differently depending on whether
% they are a group annotation or not. unfortunately it seems insertShape as
% it is now doesn't support changes to line style.
is_grp = zeros(length(detections), 1);

for k = length(detections.object):-1:1
    name = detections.object(k).name;
    train_id = find(strcmp(objectNames, name)) - 1;
    assert(~isempty(train_id));
    xmin = str2double(detections.object(k).bndbox.xmin) + 1;
    ymin = str2double(detections.object(k).bndbox.ymin) + 1;
    xmax = str2double(detections.object(k).bndbox.xmax) + 1;
    ymax = str2double(detections.object(k).bndbox.ymax) + 1;
    width = xmax - xmin + 1;
    height = ymax - ymin + 1;
    rectangle_position(k, :) = [xmin, ymin, width, height];

    colour(k, :) = cmap(train_id + 1,:);
    text_strings{k} = name;
    is_grp(k) = logical(str2double(detections.object(k).is_grp));
    % this is a temporary workaround
    if is_grp(k)
        text_strings{k} = [text_strings{k}, 'group'];
    end

end

text_position = rectangle_position(:,1:2);
vis_im = insertShape(vis_im, 'Rectangle', rectangle_position,...
    'LineWidth', 4, 'Color', colour*255);

% scale font size according to width of bbox for clearer visual
max_font_shrink_ratio = 2;
pixel_to_font_size_ratio = 2.6; % for three-letter word
step_size = 0.25;
width_threshes = [0, ...
    (pixel_to_font_size_ratio/max_font_shrink_ratio + step_size) : step_size : pixel_to_font_size_ratio,...
    Inf] * font_size;
for k = (numel(width_threshes) - 1):(-1):1
    dets_in_range = width_threshes(k) <= rectangle_position(:,3) & ...
        rectangle_position(:,3) < width_threshes(k+1);
    adjusted_font_size = floor(max(width_threshes(k)/pixel_to_font_size_ratio, ...
        font_size/max_font_shrink_ratio));
    if any(dets_in_range)
        vis_im = insertText(vis_im, text_position(dets_in_range, :), text_strings(dets_in_range),...
            'Font', 'UbuntuMono-R', 'FontSize', adjusted_font_size,...
            'BoxColor', colour(dets_in_range, :)*255, 'BoxOpacity', 0.5, 'TextColor', 'white', ...
            'AnchorPoint', 'LeftBottom');
    end
end
end