function [xc,good,bad,type] = cornerfinder_saddle_point(xt,I,wintx,winty,wx2,wy2);

%[xc] = cornerfinder_saddle_point(xt,I,wintx,winty);
%
%Finds the sub-pixel corners on the image I with initial guess xt
%xt and xc are 2xN matrices. The first component is the x coordinate
%(horizontal) and the second component is the y coordinate (vertical)
%
%Based on Harris corner finder method
%
%Finds corners to a precision below .1 pixel!
%Oct. 14th, 1997 - UPDATED to work with vertical and horizontal edges as well!!!
%Sept 1998 - UPDATED to handle diverged points: we keep the original points
%good is a binary vector indicating wether a feature point has been properly
%found.
%
%Add a zero zone of size wx2,wy2
%July 15th, 1999 - Bug on the mask building... fixed + change to Gaussian mask with higher
%resolution and larger number of iterations.

% California Institute of Technology
% (c) Jean-Yves Bouguet -- Oct. 14th, 1997

xt = xt';
xt = fliplr(xt);

if nargin < 4,
   winty = 5;
   if nargin < 3,
      wintx = 5;
   end;
end;

if nargin < 6,
   wx2 = -1;
   wy2 = -1;
end;

%mask = ones(2*wintx+1,2*winty+1);
mask = exp(-((-wintx:wintx)'/(wintx)).^2) * exp(-((-winty:winty)/(winty)).^2);

if (wx2>0) & (wy2>0),
   if ((wintx - wx2)>=2)&((winty - wy2)>=2),
      mask(wintx+1-wx2:wintx+1+wx2,winty+1-wy2:winty+1+wy2)= zeros(2*wx2+1,2*wy2+1);
   end;
end;

offx = [-wintx:wintx]'*ones(1,2*winty+1);
offy = ones(2*wintx+1,1)*[-winty:winty];

resolution = 0.005;

MaxIter = 10;

[nx,ny] = size(I);
N = size(xt,1);

xc = xt; % first guess... they don't move !!!

type = zeros(1,N);

for i=1:N,

   v_extra = resolution + 1; 		% just larger than resolution

   compt = 0; 				% no iteration yet

   while (norm(v_extra) > resolution) & (compt<MaxIter),

       cIx = xc(i,1); 			%
       cIy = xc(i,2); 			% Coords. of the point
       crIx = round(cIx); 		% on the initial image
       crIy = round(cIy); 		%
       itIx = cIx - crIx; 		% Coefficients
       itIy = cIy - crIy; 		% to compute
       if itIx > 0, 			% the sub pixel
           vIx = [itIx 1-itIx 0]'; 	% accuracy.
       else
           vIx = [0 1+itIx -itIx]';
       end;
       if itIy > 0,
           vIy = [itIy 1-itIy 0];
       else
           vIy = [0 1+itIy -itIy];
       end;

      % What if the sub image is not in?

      if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5;
      elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4;
      else
          xmin = crIx-wintx-2; xmax = crIx+wintx+2;
      end;

      if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5;
      elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4;
      else
          ymin = crIy-winty-2; ymax = crIy+winty+2;
      end;

      SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood
      SI = conv2(conv2(SI,vIx,'same'),vIy,'same');
      SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood

      px = cIx + offx;
      py = cIy + offy;

      if 1, %~saddle,
          [gy,gx] = gradient(SI); 		% The gradient image
          gx = gx(2:2*wintx+2,2:2*winty+2); % extraction of the useful parts only
          gy = gy(2:2*wintx+2,2:2*winty+2); % of the gradients
          gxx = gx .* gx .* mask;
          gyy = gy .* gy .* mask;
          gxy = gx .* gy .* mask;

          bb = [sum(sum(gxx .* px + gxy .* py)); sum(sum(gxy .* px + gyy .* py))];

          a = sum(sum(gxx));
          b = sum(sum(gxy));
          c = sum(sum(gyy));

          dt = a*c - b^2;

          xc2 = [c*bb(1)-b*bb(2) a*bb(2)-b*bb(1)]/dt;
      else

          SI = SI(2:2*wintx+2,2:2*winty+2);
          A =  repmat(mask(:),1,6) .* [px(:).^2 px(:).*py(:) py(:).^2 px(:) py(:) ones((2*wintx+1)*(2*winty+1),1)];
          param = inv(A'*A)*A'*( mask(:).*SI(:));
          xc2 = (-inv([2*param(1) param(2) ; param(2) 2*param(3) ]) * param(4:5))';

      end;

      v_extra = xc(i,:) - xc2;

      xc(i,:) = xc2;

      compt = compt + 1;

  end;

  if 1,

      cIx = xc(i,1); 			%
      cIy = xc(i,2); 			% Coords. of the point
      crIx = round(cIx); 		% on the initial image
      crIy = round(cIy); 		%
      itIx = cIx - crIx; 		% Coefficients
      itIy = cIy - crIy; 		% to compute
      if itIx > 0, 			% the sub pixel
          vIx = [itIx 1-itIx 0]'; 	% accuracy.
      else
          vIx = [0 1+itIx -itIx]';
      end;
      if itIy > 0,
          vIy = [itIy 1-itIy 0];
      else
          vIy = [0 1+itIy -itIy];
      end;

      % What if the sub image is not in?

      if (crIx-wintx-2 < 1), xmin=1; xmax = 2*wintx+5;
      elseif (crIx+wintx+2 > nx), xmax = nx; xmin = nx-2*wintx-4;
      else
          xmin = crIx-wintx-2; xmax = crIx+wintx+2;
      end;

      if (crIy-winty-2 < 1), ymin=1; ymax = 2*winty+5;
      elseif (crIy+winty+2 > ny), ymax = ny; ymin = ny-2*winty-4;
      else
          ymin = crIy-winty-2; ymax = crIy+winty+2;
      end;

      SI = I(xmin:xmax,ymin:ymax); % The necessary neighborhood
      SI = conv2(conv2(SI,vIx,'same'),vIy,'same');
      SI = SI(2:2*wintx+4,2:2*winty+4); % The subpixel interpolated neighborhood
      px = cIx + offx;
      py = cIy + offy;

      SI = SI(2:2*wintx+2,2:2*winty+2);
      A =  repmat(mask(:),1,6) .* [px(:).^2 px(:).*py(:) py(:).^2 px(:) py(:) ones((2*wintx+1)*(2*winty+1),1)];
      param = inv(A'*A)*A'*( mask(:).*SI(:));
      xc2 = (-inv([2*param(1) param(2) ; param(2) 2*param(3) ]) * param(4:5))';

      v_extra = xc(i,:) - xc2;

      xc(i,:) = xc2;
  end;

end;

% check for points that diverge:

delta_x = xc(:,1) - xt(:,1);
delta_y = xc(:,2) - xt(:,2);

%keyboard;

bad = (abs(delta_x) > wintx) | (abs(delta_y) > winty);
good = ~bad;
in_bad = find(bad);

% For the diverged points, keep the original guesses:

xc(in_bad,:) = xt(in_bad,:);

xc = fliplr(xc);
xc = xc';

bad = bad';
good = good';


% Example usage of PoissonDistributionX

lambda = 5;

pd = PoissonDistributionX(lambda);

mu = pd.Mean;
covar = pd.Covar;
samples = pd.random(5000000);

lik = pd.pdf(samples);


function consistent = swiftcc(S, rev, varargin)
% swiftcc is an even faster version of fastcc
%
% USAGE:
%
%    consistent = swiftcc(S, rev [, solver])
%
% INPUTS:
%    S:      the associated sparse stoichiometric matrix
%    rev:    the 0-1 vector with 1's corresponding to the reversible reactions
%
% OPTIONAL INPUT:
%    solver:    the LP solver to be used; the currently available options are
%               'gurobi', 'linprog', and 'cplex' with the default value of
%               'linprog'. It fallbacks to the COBRA LP solver interface if
%               another supported solver is called.
%
% OUTPUT:
%    consistent:    the 0-1 indicator vector of the reactions constituting
%                   the maximum flux consistent metabolic subnetwork
%
% .. Authors:
%       - Mojtaba Tefagh, Stephen P. Boyd, 2019, Stanford University

    [m, n] = size(S);
    consistent = true(n, 1);

    %% setting up the LP solver
    if ~isempty(varargin)
        solver = varargin{1};
    else
        solver = 'linprog';
    end

    %% identifying the blocked irreversible reactions
    result = blocked(S, rev, solver);
    consistent(result.x(m+1:end) < -0.5) = false;

    %% setting up the zero-tolerance parameter
    tol = norm(S(:, consistent), 'fro')*eps(class(S));

    %% identifying the blocked reversible reactions
    [Q, R, ~] = qr(transpose(S(:, consistent)));
    Z = Q(rev(consistent) == 1, sum(abs(diag(R)) > tol)+1:end);

    %% finding the consistent reactions of the original metabolic network
    consistent(consistent & rev == 1) = diag(Z*Z.') > tol^2;
    consistent = find(consistent);
end


function [zpos] = zposPlane(ops, Taff, MimgZ, subpixel)

% use Z-stack and registered binary files from suite2P to compute Z-position of plane
% make sure that ops.DeleteBin = 0

zpos   = zeros(sum(ops.Nframes), 1,'single');

% cut through z-stack where mean plane should be +/- zspread
Ny    =  numel(ops.yrange);
Nx    =  numel(ops.xrange);

mimg  = ops.mimg1(ops.yrange, ops.xrange);

%%
zspread  = 10;
Zaligned = cutZstack(MimgZ, Taff, Ny, Nx, zspread);

% rescale z-stack
Zaligned   = single(Zaligned) / mean(single(Zaligned(:))) * mean(single(mimg(:)));

% check that cut is centered on mean plane
m1 = fft(fft(Zaligned,[],1),[],2);
eps0 = single(1e-20);
m1 = m1./(abs(m1)+eps0);
m2 = fft(fft(mimg,[],1),[],2);
m2 = m2./(abs(m2)+eps0);
if ops.useGPU
    m1 = gpuArray(single(m1));
    m2 = gpuArray(single(m2));
end
[cx, ix, cZ] = ZRegPlane(m1,m2,[1:size(m1,3)],ops.useGPU);
clear m1 m2;
ix(3)      = zspread + 1 - ix(3);
fprintf('>>> x offset: %2.2f; y offset: %2.2f; z offset: %2.2f\n', ix(1), ix(2), ix(3));

% whiten z-stack (take ifft of phase of fft)
m1 = fft(fft(Zaligned,[],1),[],2);
eps0 = single(1e-20);
m1 = m1./(abs(m1)+eps0);
Zwhite = real(ifft(ifft(m1, [], 1), [], 2));
clear m1;
Zwhite = reshape(Zwhite, [], size(Zwhite,3));
if ops.useGPU
    Zwhite = gpuArray(single(Zwhite));
end

% check mean image
clf;
subplot(1,2,1),
imagesc(mimg,[0 8000]);
subplot(1,2,2),
imagesc(Zaligned(:,:,zspread+1),[0 8000]);
title('stretched z-stack');
drawnow;

%%
iZ       = [-zspread : 1/subpixel : zspread];
fid = fopen(ops.RegFile, 'r');

Ly  = ops.Ly;
Lx  = ops.Lx;
NT  = sum(ops.Nframes);
Nbatch = 250 / round(Ly/512);
ix0    = 0;
tic;

% array for z-position of planes
nPos    = ones(numel(iZ), 1, 'single');
while ix0 < NT
    indxr = ix0 + (1:Nbatch);
    ix0   = ix0 + Nbatch;
    indxr(indxr > NT) = [];

    data  = fread(fid,  Ly*Lx*length(indxr), '*int16');
    data  = reshape(data, Ly, Lx, []);
    data  = data(ops.yrange, ops.xrange, :);
    if ops.useGPU
        data = gpuArray(single(data));
    else
        data = single(data);
    end

    % whiten data
    m2     = fft(fft(data, [], 1), [], 2);
    data   = real(ifft(ifft(m2, [], 1), [], 2));
    data   = reshape(data, [], size(data,3));

    % correlate with z-stack
    cc     = Zwhite' * data;
    if ops.useGPU
        cc = gather(cc);
    end

    % interpolate and find max position in z-stack
    ccz           = interp1([-zspread:zspread]', cc, iZ(:), 'spline');
    [cmax,izmax0] = max(ccz);
    izmax         = squeeze(round(izmax0));
    zpos(indxr)  = iZ(izmax);

    if rem(ix0, 6000)==0
        fprintf('Frame %d done in time %2.2f \n', ix0, toc)
    end
end
fclose(fid);


classdef FEMInputWriter < handle

    properties (Access = private)
        fileName
        xmax
        ymax
        Nx
        Ny
        P
        xmesh
        ymesh
        zmesh
        mesh
        nDirichlet
        nNeumann
        DoF
        dispPrescribed
        pointLoads
        edgeNodes
    end

    methods (Access = public)
        function obj = FEMInputWriter(cParams)
            obj.init(cParams);
        end

        function createTest(obj)
            obj.computeMeshGrid();
            obj.createMesh();
            obj.computeBoundaryConditions();
            obj.computePrescribedDisplacementsMatrix();
            obj.computePrescribedPointLoads();
            obj.computeEdgeNodes();
            obj.writeFile();
        end
    end

    methods (Access = private)
        function init(obj,cParams)
            obj.fileName = cParams.testName;
            obj.xmax     = cParams.x1;
            obj.ymax     = cParams.y1;
            obj.Nx       = cParams.N;
            obj.Ny       = cParams.M;
            obj.P        = cParams.P;
            obj.DoF      = cParams.DoF;
        end

        function computeMeshGrid(obj)
            x1        = linspace(0,obj.xmax,obj.Nx);
            x2        = linspace(0,obj.ymax,obj.Ny);
            [X,Y]     = meshgrid(x1,x2);
            Z         = zeros(size(X));
            obj.xmesh = X;
            obj.ymesh = Y;
            obj.zmesh = Z;
        end

        function createMesh(obj)
            [F,V]    = mesh2tri(obj.xmesh,obj.ymesh,obj.zmesh,'f');
            s.coord  = V(:,1:2);
            s.connec = F;
            obj.mesh = Mesh(s);
            obj.mesh.plot;
        end

        function computeBoundaryConditions(obj)
            t              = 0.3*obj.ymax;
            m              = obj.mesh;
            root           = m.coord(:,1) == 0;
            tipLength      = m.coord(:,1) == obj.xmax;
            tipWidth       = m.coord(:,2) < obj.ymax-t & m.coord(:,2) > t;
            tip            = tipLength & tipWidth;
            obj.nDirichlet = find(root);
            obj.nNeumann   = find(tip);
        end

        function computePrescribedDisplacementsMatrix(obj)
            obj.dispPrescribed = obj.computeBoundaryConditionMatrix(obj.DoF,obj.nDirichlet);
        end

        function computePrescribedPointLoads(obj)
            Fmat  = obj.computeBoundaryConditionMatrix(obj.DoF,obj.nNeumann);
            nnode = size(Fmat,1)/obj.DoF;
            Pnod  = obj.P/nnode;
            for i = 2:2:size(Fmat,1)
                Fmat(i,3) = Pnod;
            end
            obj.pointLoads = Fmat;
        end

        function computeEdgeNodes(obj)
            COOR          = obj.mesh.coord;
            x             = COOR(:,1);
            y             = COOR(:,2);
            edge          = find(x==0 | x==max(x) | y==0 | y==max(y));
            obj.edgeNodes = sort(edge);
        end

        function writeFile(obj)
            fileID = fopen([obj.fileName],'w');
            obj.writeProblemData(fileID);
            obj.writeCoordinates(fileID);
            obj.writeConnectivities(fileID);
            obj.writeDirichletData(fileID);
            obj.writeNeumannData(fileID);
            obj.writeNodesSolid(fileID);
            obj.writeExternalBorderNodes(fileID);
            fclose(fileID);
        end

        function writeCoordinates(obj,fileID)
            COOR        = zeros(size(obj.mesh.coord,1),4);
            COOR(:,2:3) = obj.mesh.coord;
            COOR(:,1)   = (1:1:size(COOR,1))';
            fprintf(fileID,'%%%% Coordinates\n%% Node\n');
            fprintf(fileID,'gidcoord = [\n');
            for i=1:size(COOR,1)
                fprintf(fileID,'%d %f %f %f;\n',COOR(i,:));
            end
            fprintf(fileID,'];\n');
        end

        function writeConnectivities(obj,fileID)
            Tnod        = zeros(size(obj.mesh.connec,1),5);
            Tnod(:,2:4) = obj.mesh.connec;
            Tnod(:,1)   = (1:1:size(Tnod,1))';
            fprintf(fileID,'%%%% Connectivities\n%% Node\n');
            fprintf(fileID,'gidlnods = [\n');
            for i=1:size(Tnod,1)
                fprintf(fileID,'%d %d %d %d %d;\n',Tnod(i,:));
            end
            fprintf(fileID,'];\n');
        end

        function writeDirichletData(obj,fileID)
            fprintf(fileID,'%%%% Variable prescribed\n%% Node\n');
            fprintf(fileID,'lnodes = [\n');
            for i=1:size(obj.dispPrescribed,1)
                fprintf(fileID,'%d %d %f;\n',obj.dispPrescribed(i,:));
            end
            fprintf(fileID,'];\n');
        end

        function writeNeumannData(obj,fileID)
            fprintf(fileID,'%%%% Point loads\n%% Node\n');
            fprintf(fileID,'pointload_complete = [\n');
            for i=1:size(obj.pointLoads,1)
                fprintf(fileID,'%d %d %f;\n',obj.pointLoads(i,:));
            end
            fprintf(fileID,'];\n');
        end

        function writeExternalBorderNodes(obj,fileID)
            fprintf(fileID,'%%%% External Border Nodes\n%% Node\n');
            fprintf(fileID,'External_border_nodes = [\n');
            for i=1:size(obj.edgeNodes,1)
                fprintf(fileID,'%d;\n',obj.edgeNodes(i));
            end
            fprintf(fileID,'];\n');
        end
    end

    methods (Access = private, Static)
        function bc = computeBoundaryConditionMatrix(DoF,n)
            bc = zeros(DoF*length(n),3);
            for i = 1:length(n)
                bc(2*i-1,1:2) = [n(i),1];
                bc(2*i,1:2)   = [n(i),2];
            end
        end
        function writeProblemData(fileID)
            fprintf(fileID,'%%%% Data\nData_prb = {\n');
            fprintf(fileID,'''TRIANGLE'';\n''SI'';\n''2D'';\n''Plane_Stress'';\n');
            fprintf(fileID,'''ELASTIC'';\n''MACRO'';\n};\n');
        end
        function writeNodesSolid(fileID)
            fprintf(fileID,'%%%% Nodes solid\n%% Node\n');
            fprintf(fileID,'nodesolid = unique(pointload_complete(:,1));\n');
        end
    end
end


clear all; close all; clear classes; clc;

%% Set flags.
inspect_only = false;

%% Solve the system.
s = 120;
w = 40;
b = (s + w/2)*2;
dL = 10;
dl = 2;
wvlen = 200;
clear solveropts;
[E, H, obj_array, src_array, J] = maxwell_run(...
	'OSC', 1e-9, wvlen, ...
	'DOM', {'vacuum', 'none', 1.0}, [-610 610; -610 610; 0 dl], [dL dL dl], BC.p, [10*dL 10*dL 0], ...
	'OBJ', {'vacuum', 'none', 1.0}, Box([-w/2 w/2; -w/2 w/2; 0 dl], dl), ...
	'SOBJ', ...  % scatter objects
		{'Johnson/Au', 'y'}, ...
			PolygonalCylinder(Axis.z, dl, dl/2, [w/2 0; w/2+s*sqrt(3)/2 -s/2; w/2+s*sqrt(3)/2 s/2], dl), ...
			PolygonalCylinder(Axis.z, dl, dl/2, [-w/2 0; -w/2-s*sqrt(3)/2 s/2; -w/2-s*sqrt(3)/2 -s/2], dl), ...
	'SRCJ', TFSFPlaneSrc([-b b; -b b; 0 dl], Axis.y, Axis.x), ...
	inspect_only);

%% Visualize the solution.
figure
clear opts
opts.withobjsrc = true;
opts.withabs = true;
% opts.withinterp = false;
% opts.withgrid = true;
% opts.cscale = 1e-1;
% opts.cmax = 1.4;
z_location = 0;
vis2d(E{Axis.x}, Axis.z, z_location, obj_array, src_array, opts)
% vis2d(H{Axis.z}, Axis.z, z_location, obj_array, src_array, opts)

% %% Calculate the power emanating from the source.
% power = powerflux_box(E,H,[-10 10; -10 10; 0 1]);
% fprintf('power = %e\n', power);


function [DT, tE] = compute_CostDataTerm_SENSE(Data, SP3, x, params)
%% Compute the data part of the cost function
% 1/2 * (y-Bx)^H * W * (y-Bx); B = S * F

smap = params.smap;
ncoils = params.ncoils;

%% Compute the Data Term
tS = tic;
Sx = smap .* repmat(x, [1, 1, ncoils]);
FSx = fft2(Sx) .* SP3;
dif = Data - FSx;

DT = sum(abs(dif(:)).^2) / 2;
tE = toc(tS);


function state = AssignRandomInitialStatematrix(x,Q);

state=floor(1+Q*rand(size(x,1),size(x,1)));

github-repos/MATLAB/Sable-mcbench-benchmarks/mcbench-benchmarks-ba13b2f0296ef49491b95e3f984c7c41fccdb6d8/34784-monte-carlo-simulation-of-two-dimensional-grain-growth-code-version-no-1-basic/Monte Carlo Simulation Q-state Potts model 2D square-lattice - microstructure/AssignRandomInitialStateMatrixQPOTTS.m


 function [xs, info] = pwls_pcg(x, G, W, yi, nder1, R, ...
		M, niter, stepper)
%function [xs, info] = pwls_pcg(x, G, W, yi, nder1, R, ...
%		M, niter, stepper)
%
% weighted least squares with convex non-quadratic penalty
% via preconditioned conjugate gradient algorithm
% cost(x) = (y-Gx)'W(y-Gx)/2 - n'(y-Gx) + R(x)
%
% in
%	x	[np,1]		initial estimate
%	G	[nd,np]		system matrix
%	W	[nd,nd]		data weighting matrix, usually diag_sp(wi)
%	yi	[nd,1]		noisy data
%	nder1	[nn,1]		linear term (obsolete: use "0")
%	R			penalty object (see Reg1.m)
%	M	[np,np]		preconditioner (use "1" if none)
%	niter			# total iterations
%	stepper			method for step-size line search
%				use {} for a good default
% out
%	xs	[np,niter]	estimates each iteration
%	info	[niter, 3]	gamma, step, time
%
% Copyright 1996-7, Jeff Fessler, The University of Michigan

warning 'pwls_pcg is obsolete; use pwls_pcg1 instead'

if nargin < 8, help(mfilename), error args, end

cpu etic

if isempty(nder1), nder1 = 0; end
if isempty(M), M = 1; end
if nder1 ~= 0, error 'nder1 = 0 required', end

if ~isvar('stepper') || isempty(stepper)
	stepper = {'qs', 3};	% quad surr with this # of subiterations
end

xs = zeros(length(x), niter);
xs(:,1) = x;

info = zeros(niter,3);

%
% initialize projections
%
Gx = G * x;

oldinprod = 0;

% iterate
for ii=2:niter
	ticker(mfilename, ii, niter)

	%
	% (negative) gradient
	%
	ngrad = G' * (W * (yi-Gx) - nder1);

	pgrad = R.cgrad(R, x);
	ngrad = ngrad - pgrad;

	%
	% preconditioned gradient
	pregrad = M * ngrad;

	% direction
	newinprod = ngrad' * pregrad;
	if ii == 2
		ddir = pregrad;
		gamma = 0;
	else
		if oldinprod == 0
			warn 'inprod=0. going nowhere!'
			gamma = 0;
		else
			gamma = newinprod / oldinprod;	% Fletcher-Reeves
%			gamma = (newinprod - oldgrad' * pregrad) / oldinprod;
		end
		ddir = pregrad + gamma * ddir;
	end
	oldgrad = ngrad;
	oldinprod = newinprod;

	% check if descent direction
	if real(ddir' * ngrad) < 0
		warning 'wrong direction'
		keyboard
%		ddir = pregrad;	% revert
%		oldinprod = 0;	% reset
	end

	% step size in search direction
	Gdir = G * ddir;
%	Cdir = R.C * ddir;

	% one step based on quadratic surrogate for penalty
	if streq(stepper{1}, 'qs1')
%		pdenom = Cdir' * (R.wpot(R.wt, Cdir) .* Cdir); % cannot be?
		pdenom = (abs(ddir).^2)' * R.denom(R, x);
		denom = Gdir'*(W*Gdir) + pdenom;
		if denom == 0
			warning 'found exact solution???  step=0 now!?'
			step = 0;
		else
			step = real((ddir' * grad) / denom);
		end

	% iteratively minimize \Half || y-G (x+alf*ddir) ||_W^2 + R(x + alf*ddir)
	elseif streq(stepper{1}, 'qs')
		nsub = stepper{2};
		dGWGd = Gdir'*(W*Gdir);
		dGWr = Gdir'*(W*(yi-Gx));
		step = 0;
		for is=1:nsub
%			pdenom = Cdir' * (R.wpot(R.wt, Cdir) .* Cdir);
			pdenom = (abs(ddir).^2)' * R.denom(R, x+step*ddir);
			denom = dGWGd + pdenom;
			pgrad = R.cgrad(R, x + step * ddir);
			step = step - (-dGWr + step * dGWGd + ddir' * pgrad) ...
				/ denom;
%			printf('%d-%d %g', ii, is, step)
		end

	else
		error 'bad stepper'
	end

	if step < 0
		warning('downhill?')
		keyboard
	end

	% update
	Gx	= Gx  + step * Gdir;
%	Cx	= Cx  + step * Cdir;
	x	= x + step * ddir;
	xs(:,ii) = x;

	info(ii,1) = gamma;
	info(ii,2) = step;
	info(ii,3) = cpu('etoc');	% accum. time
end


%% groupVertices
% Below is a demonstration of the features of the |groupVertices| function

%%
clear; close all; clc;

%% Syntax
% |[groupIndexVertices,groupIndexFaces]=groupVertices(F,V,waitBarOption);|

%% Description
% This function takes in a description of a tesselation, consisting of an
% array F (faces or elements), and V (vertices). The output is represents
% the group index for each point in groupIndexVertices and the group index
% of each face in groupIndexFaces. Vertices which are connected to the same
% mesh obtain the same group indes. Face indices are derived from the
% vertex indices.

%% Examples

%% Example 1: Seperating patch data into groups

%%
% Creating test data consisting of seperates sets faces and vertices

numGroups=3;
F_cell=cell(1,numGroups);
V_cell=cell(1,numGroups);
for q=1:1:numGroups
    switch q
        case 1
            [F,V]=geoSphere(3,1);
        case 2
            [F,V]=stanford_bunny('g');
        case 3
            [F,V]=graphicsModels(3);
    end
%     [F,V]=subtri(F,V,1);
    V=V-mean(V,1);
    V=V./max(V(:));
    V(:,1)=V(:,1)+q*2;
    F_cell{q}=F;
    V_cell{q}=V;
end
[F,V]=joinElementSets(F_cell,V_cell);

%%
% Using |groupVertices| to split the vertices into groups
[groupIndexVertices,groupIndexFaces]=groupVertices(F,V,1);

%%
% Visualizing the resulting grouping

cFigure;
subplot(2,1,1); hold on;
title('Ungrouped')
gpatch(F,V,'kw','none');
axisGeom;
camlight headlight;

subplot(2,1,2); hold on;
title('Grouped')
gpatch(F,V,'kw','none');
scatterV(V,15,groupIndexVertices,'filled');
axisGeom;
camlight headlight;
colormap gjet; icolorbar;
drawnow;

%%
% If desired a second output can be requisted which represents the face
% groupings.

cFigure;
title('Grouped faces')
gpatch(F,V,groupIndexFaces,'none');
axisGeom;
camlight headlight;
colormap gjet; icolorbar;
drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function M = make_convn_mat(F, sz, shape, pad)
%MAKE_CONVN_MAT   Flexible N-D convolution matrix
%   M = MAKE_CONVN_MAT(F, SZ[, SHAPE[, PAD]]) returns the convolution
%   matrix for the matrix F.  SZ gives the size of the array that the
%   convolution should be applied to.  The returned matrix M is sparse.
%   If X is of size SZ and SHAPE is 'full', then reshape(M * X(:), SZ +
%   size(F) - 1) is the same as convn(X, F).
%
%   The optional parameter SHAPE controls which parts of the convolution
%   result to return in M:
%     * 'full':      (default) returns the full N-D convolution, i.e. the
%                    behavior is identical to convn(X, F, 'full').
%     * 'same':      returns the central part of the convolution that
%                    is the same size as X, i.e. the behavior is
%                    identical to convn(X, F, 'same').
%     * 'sameswap':  identical to 'same' except that rounding needed for
%                    even-sized filters is performed the opposite way.
%     * 'valid':     returns only the part of the result that can be
%                    computed without assuming zero-padded arrays,
%                    i.e. the behavior is identical to convn(X, F,
%                    'valid').
%
%   The optional parameter PAD controls whether to return an M that pads
%   the result of the convolution with zeros (default no padding):
%     * 'full':      returns the result padded to the output size of a
%                    full N-D convolution (works with SHAPE set to 'same'
%                    and 'valid').
%     * 'same':      returns the result padded to the central part of the
%                    convolution that is the same size as X (works with
%                    SHAPE set to 'valid').
%     * 'sameswap':  identical to 'same' except that rounding needed for
%                    even-sized filters is performed the opposite way.
%
%   See also CONVMTXN.
%
%   Author:  Stefan Roth, Department of Computer Science, TU Darmstadt
%   Contact: sroth@cs.tu-darmstadt.de
%   $Date: 2007-03-27 14:09:11 -0400 (Tue, 27 Mar 2007) $
%   $Revision: 252 $

% Copyright 2004-2007, Brown University, Providence, RI. USA
% Copyright 2007-2010 TU Darmstadt, Darmstadt, Germany.
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

  % Default to full matrix
  if (nargin < 3)
    shape = 'full';
  end

  ndims = length(sz);

  % Border sizes for 'same' and 'sameswap'
  Fsize_lo_2 = ceil((size(F) - 1) / 2);
  Fsize_hi_2 = floor((size(F) - 1) / 2);

  % Border sizes for 'valid'
  Fsize = size(F) - 1;

  switch(shape)
    case 'same'
      % Mark valid and invalid pixels (i.e. the ones within and outside
      % of the part to be returned)
      valid = true(sz+size(F)-1);

      for d = 1:ndims
        for e = 1:ndims
          sub{e} = ':';
        end

        sub{d} = 1:Fsize_lo_2(d);
        valid(sub{:}) = false;
        sub{d} = size(valid, d)-Fsize_hi_2(d)+1:size(valid, d);
        valid(sub{:}) = false;
      end

      if (nargin > 3 && strcmp(pad, 'full'))
        % If we're padding to 'full' set the coefficients on the border
        % to zero
        M = convmtxn(F, sz, valid);
      else
        % If we're *not* padding, then suppress the rows of M that
        % correspond to the border
        M = convmtxn(F, sz);
        M = M(valid, :);
      end

   case 'sameswap'
     % Mark valid and invalid pixels (i.e. the ones within and outside
     % of the part to be returned), but round the other way
     valid = true(sz+size(F)-1);

     for d = 1:ndims
       for e = 1:ndims
         sub{e} = ':';
       end

       sub{d} = 1:Fsize_hi_2(d);
       valid(sub{:}) = false;
       sub{d} = size(valid, d)-Fsize_lo_2(d)+1:size(valid, d);
       valid(sub{:}) = false;
     end

     if (nargin > 3 && strcmp(pad, 'full'))
       % If we're padding to 'full' set the coefficients on the border
       % to zero
       M = convmtxn(F, sz, valid);
     else
       % If we're *not* padding, then suppress the rows of M that
       % correspond to the border
       M = convmtxn(F, sz);
       M = M(valid, :);
     end

   case 'valid'
     % Mark valid and invalid pixels (i.e. the ones within and outside
     % of the part to be returned)
     valid = true(sz+size(F)-1);

     for d = 1:ndims
       for e = 1:ndims
         sub{e} = ':';
       end

       sub{d} = 1:Fsize(d);
       valid(sub{:}) = false;
       sub{d} = size(valid, d)-Fsize(d)+1:size(valid, d);
       valid(sub{:}) = false;
     end

     if (nargin > 3)
       % If we're padding, then figure out the area to be padded

       switch (pad)
         case 'same'
           % Mark valid and invalid pixels (i.e. the ones within and outside
           % of the part to be padded)
           pad_valid = true(sz+size(F)-1);

           for d = 1:ndims
             for e = 1:ndims
               sub{e} = ':';
             end

             sub{d} = 1:Fsize_lo_2(d);
             pad_valid(sub{:}) = false;
             sub{d} = size(valid, d)-Fsize_hi_2(d)+1:size(valid, d);
             pad_valid(sub{:}) = false;
           end

           % Set coefficients on the border to zero
           M = convmtxn(F, sz, valid);

           % Suppress rows of M outside of the padded area
           M = M(pad_valid, :);

         case 'sameswap'
           % Mark valid and invalid pixels (i.e. the ones within and outside
           % of the part to be padded), but round the other way
           pad_valid = true(sz+size(F)-1);

           for d = 1:ndims
             for e = 1:ndims
               sub{e} = ':';
             end

             sub{d} = 1:Fsize_hi_2(d);
             pad_valid(sub{:}) = false;
             sub{d} = size(valid, d)-Fsize_lo_2(d)+1:size(valid, d);
             pad_valid(sub{:}) = false;
           end

           % Set coefficients on the border to zero
           M = convmtxn(F, sz, valid);

           % Suppress rows of M outside of the padded area
           M = M(pad_valid, :);

         otherwise
           % Padding to 'full'; only set coefficients on the border to zero
           M = convmtxn(F, sz, valid);
       end
     else
       % No padding; suppress all rows on the border
       M = convmtxn(F, sz);
       M = M(valid, :);
     end

    otherwise
      % Full convolution; return everything
      M = convmtxn(F, sz);

  end


%go_calib_optim
%
%Main calibration function. Computes the intrinsic andextrinsic parameters.
%Runs as a script.
%
%INPUT: x_1,x_2,x_3,...: Feature locations on the images
%       X_1,X_2,X_3,...: Corresponding grid coordinates
%
%OUTPUT: fc: Camera focal length
%        cc: Principal point coordinates
%        alpha_c: Skew coefficient
%        kc: Distortion coefficients
%        KK: The camera matrix (containing fc and cc)
%        omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space
%        Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space
%        Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors
%
%Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic
%        camera parameters, and the extrinsic parameters (3D locations of the grids in space)
%
%Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through
%      the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses.
%
%Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the
%      corresponding entry in the active_images vector to zero.
%
%VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m
%that is so far implemented to work only with 2D rigs.
%In the future, a more general function will be there.
%For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length

if ~exist('n_ima'),
   data_calib_no_read; % Load the images
   click_calib_no_read; % Extract the corners
end;

check_active_images;
check_extracted_images;
check_active_images;
desactivated_images = [];

recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice..

%%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation)
go_calib_optim_iter;

if ~isempty(desactivated_images),
   param_list_save = param_list;
   fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n');
   active_images(desactivated_images) = ones(1,length(desactivated_images));
   desactivated_images = [];
   go_calib_optim_iter;
   if ~isempty(desactivated_images),
      fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] );
   end;
   param_list = [param_list_save(:,1:end-1) param_list];
end;


%SUBSREF Subscript reference overload of mapping
%
% This routine enables constructions like DATA = W.DATA,
% which is similar to DATA = GETDATA(W).
%
% In addition V = W(I,J) is supported for affine transformations.
% It is again an affine mapping using the [I,J] block of the
% rotation matrix and the elements J of the support vector.
%
% For arbitrary mappings just V = W(:,J) is defined by output
% selection: A*V returns just the features J of A*W.


function [ulf, vlf, lf, hf, lfhf, ttlpwr] = CalcLfHfParams(PSD, F, limits,plot_on)
% [ulf, vlf, lf, hf, lfhf, ttlpwr] = CalcLfHfParams(PSD, F, limits,plot_on)
%
%   OVERVIEW: Compute the frequency domain features for a given PSD and
%             frequency bans limits
%
%   INPUT:
%        PSD     - power spectral density
%        F       - frequency vector
%        limits  - frequency domain analysis limits
%        plot_on -
%
%   OUTPUT:
%	- ulf     : (ms^2) Power in the ultra low frequency range (default < 0.003 Hz)
%	- vlf     : (ms^2) Power in very low frequency range (default 0.003 <= vlf < 0.04 Hz)
%	- lf      : (ms^2) Power in low frequency range (default 0.04Hz  <= lf < 0.15 Hz)
%	- hf      : (ms^2) Power in high frequency range (default 0.15 <= hf < 0.4 Hz)
%	- lfhf    : Ratio LF [ms^2]/HF [ms^2]
%	- ttlpwr  : (ms^2) Total spectral power (approximately <0.4 Hz)
%
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Main script written by Adriana N. Vest
%       Dependent scripts written by various authors
%       (see functions for details)
%	COPYRIGHT (C) 2016
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%%
if nargin <3
    ULF = [0 .003];
    VLF = [0.003 .04];
    LF = [.04 .15];
    HF = [0.15 0.4];
    limits = [ULF; VLF; LF; HF];
end
if nargin < 4
    plot_on =1;
end

Indx_ULF = find( (limits(1,1) <= F) & (F <= limits(1,2)) );
Indx_VLF = find( (limits(2,1) <= F) & (F <= limits(2,2)) );
Indx_LF = find( (limits(3,1) <= F) & (F <= limits(3,2)) );
Indx_HF = find( (limits(4,1) <= F) & (F <= limits(4,2)) );
space = F(2)-F(1);

ulf = sum(PSD(Indx_ULF)*space) * 1e6; % convert to ms^2
vlf = sum(PSD(Indx_VLF)*space) * 1e6; % convert to ms^2
lf = sum(PSD(Indx_LF)*space) * 1e6;   % convert to ms^2
hf = sum(PSD(Indx_HF)*space) * 1e6;   % convert to ms^2

ttlpwr = sum([ulf vlf lf hf]);

lf_n = lf/ttlpwr; % normalized
hf_n = hf/ttlpwr;
lfhf = round(lf_n/hf_n*100)/100; % lf/hf ratio

if plot_on
    figure
    % plot PSD
    plot(F,10*log10(PSD),'b','linewidth',2)
    hold on
    % plot limits on graph for lf and hf
    plot([F(Indx_LF(1)) F(Indx_LF(1))],[-80 40],'k:')
    hold on
    plot([F(Indx_LF(end)) F(Indx_LF(end))],[-80 40],'k:')
    hold on
    plot([F(Indx_HF(end)) F(Indx_HF(end))],[-80 40],'k:')

    % labelsc
    text(0.07,30,'LF','Fontname','Times New Roman','Fontsize',10)
    text(0.25,30,'HF','Fontname','Times New Roman','Fontsize',10)
    %text(0.15, 35, 'Power Spectral Density','Fontname','Times New Roman','Fontsize',10)
    text(0.3, -60, strcat('LF/HF=',num2str(lfhf)),'Fontname','Times New Roman','Fontsize',10)
    ylabel('Normalized PSD (db/Hz)','Fontname','Times New Roman','fontsize',10)
    xlabel('Frequency (Hz)','Fontname','Times New Roman','fontsize',10)
    axis([0 .45 -80 40]);
    box off
end % end plot

end % end function


function phometricParams = SRPhotometricParams(numFrames)
%SRPHOTOMETRICPARAMS Photometric parameters used for super-resolution.

    % The multiplicative part of the photometric model.
    % Default: Affine model with multiplicative factor '1'.
    phometricParams.mult = ones(numFrames, 1);

    % The additive part of the photometric model.
    % Default: Affine model with additive factor '0'.
    phometricParams.add = zeros(numFrames, 1);


function title = p04_title ( title )

%*****************************************************************************80
%
%% P04_TITLE returns the title of problem 4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 May 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, string TITLE, the title of the problem.
%
  title = 'F(X) = EXP ( X ) - 1 / ( 10 * X )^2';

  return
end


function plot3Dmodel(MatFileName)
%PLOT3DMODEL(MatFileName)
% Plots the given 3d model, test for animation function
%

load(MatFileName)

%% Check the results
% Get maximum dimension to plot the circles afterwards
AC_DIMENSION = max(max(sqrt(sum(Model3D.Aircraft(1).stl_data.vertices.^2,2))));
for i=1:length(Model3D.Control)
    AC_DIMENSION = max(AC_DIMENSION,max(max(sqrt(sum(Model3D.Control(i).stl_data.vertices.^2,2)))));
end
% Define the figure properties
AX = axes('position',[0.0 0.0 1 1]);
axis off
scrsz = get(0,'ScreenSize');
set(gcf,'Position',[scrsz(3)/40 scrsz(4)/12 scrsz(3)/2*1.0 scrsz(3)/2.2*1.0],'Visible','on');
set(AX,'color','none');
axis('equal')
hold on;
cameratoolbar('Show')
% Circles around the aircraft transformation group handles
euler_hgt(1)  = hgtransform('Parent',           AX, 'tag', 'OriginAxes');
euler_hgt(2)  = hgtransform('Parent', euler_hgt(1), 'tag', 'roll_disc');
euler_hgt(3)  = hgtransform('Parent', euler_hgt(1), 'tag', 'pitch_disc');
euler_hgt(4)  = hgtransform('Parent', euler_hgt(1), 'tag', 'heading_disc');
euler_hgt(5)  = hgtransform('Parent', euler_hgt(2), 'tag', 'roll_line');
euler_hgt(6)  = hgtransform('Parent', euler_hgt(3), 'tag', 'pitch_line');
euler_hgt(7)  = hgtransform('Parent', euler_hgt(4), 'tag', 'heading_line');
% Plot objects
% -------------------------------------------------------------------------
% Plot airframe
for i = 1:length(Model3D.Aircraft)
    AV = patch(Model3D.Aircraft(i).stl_data,  'FaceColor',        Model3D.Aircraft(i).color, ...
        'EdgeColor',        'none',        ...
        'FaceLighting',     'gouraud',     ...
        'AmbientStrength',   0.15);
end
CONT(length(Model3D.Control))=0;
% Plot controls
for i=1:length(Model3D.Control)
    CONT(i) = patch(Model3D.Control(i).stl_data,  'FaceColor',        Model3D.Control(i).color, ...
        'EdgeColor',        'none',        ...
        'FaceLighting',     'gouraud',     ...
        'AmbientStrength',  0.15);
    % Plot the rotation point and the rotation axis of each control
    % (double-check correct implementation and rotation direction of each
    % control surface)
    p = Model3D.Control(i).rot_point;
    vect = Model3D.Control(i).rot_vect;
    plot3(p(1)+[0, AC_DIMENSION*vect(1)/2], p(2)+[0, AC_DIMENSION*vect(2)/2], p(3)+[0, AC_DIMENSION*vect(3)/2], 'b-o', 'MarkerSize', 10, 'LineWidth', 2);
end
% Fixing the axes scaling and setting a nice view angle
axis('equal');
axis([-1 1 -1 1 -1 1] * 2.0 * AC_DIMENSION)
set(gcf,'Color',[1 1 1])
axis off
view([30 10])
zoom(2.0);
% Add a camera light, and tone down the specular highlighting
camlight('left');
material('dull');

% --------------------------------------------------------------------
% Define the radius of the sphere
R = 1.0 * AC_DIMENSION;

% Outer circles
phi = (-pi:pi/36:pi)';
D1 = [sin(phi) cos(phi) zeros(size(phi))];
HP(1) = plot3(R*D1(:,1),R*D1(:,2),+R*D1(:,3),'Color','b','tag','Zplane','Parent',euler_hgt(4));
HP(2) = plot3(R*D1(:,2),R*D1(:,3),+R*D1(:,1),'Color',[0 0.8 0],'tag','Yplane','Parent',euler_hgt(3));
HP(3) = plot3(R*D1(:,3),R*D1(:,1),+R*D1(:,2),'Color','r','tag','Xplane','Parent',euler_hgt(2));

% +0,+90,+180,+270 Marks
S = 0.95;
phi = -pi+pi/2:pi/2:pi;
D1 = [sin(phi); cos(phi); zeros(size(phi))];
plot3([S*R*D1(1,:); R*D1(1,:)],[S*R*D1(2,:); R*D1(2,:)],[S*R*D1(3,:); R*D1(3,:)],'Color','b','tag','Zplane','Parent',euler_hgt(4));
plot3([S*R*D1(2,:); R*D1(2,:)],[S*R*D1(3,:); R*D1(3,:)],[S*R*D1(1,:); R*D1(1,:)],'Color',[0 0.8 0],'tag','Yplane','Parent',euler_hgt(3));
plot3([S*R*D1(3,:); R*D1(3,:)],[S*R*D1(1,:); R*D1(1,:)],[S*R*D1(2,:); R*D1(2,:)],'Color','r','tag','Xplane','Parent',euler_hgt(2));
text(R*1.05*D1(1,:),R*1.05*D1(2,:),R*1.05*D1(3,:),{'N','E','S','W'},'Fontsize',9,'color',[0 0 0],'HorizontalAlign','center','VerticalAlign','middle');

% +45,+135,+180,+225,+315 Marks
S = 0.95;
phi = -pi+pi/4:2*pi/4:pi;
D1 = [sin(phi); cos(phi); zeros(size(phi))];
plot3([S*R*D1(1,:); R*D1(1,:)],[S*R*D1(2,:); R*D1(2,:)],[S*R*D1(3,:); R*D1(3,:)],'Color','b','tag','Zplane','Parent',euler_hgt(4));
HT = text(R*1.05*D1(1,:),R*1.05*D1(2,:),R*1.05*D1(3,:),{'NW','NE','SE','SW'},'Fontsize',8,'color',[0 0 0],'HorizontalAlign','center','VerticalAlign','middle');

% 10 deg sub-division marks
S = 0.98;
phi = -[0:10:90 80:-10:0 -10:-10:-90 -80:10:0];
PHI_TEXT{length(phi)}='';
for i=1:length(phi)
    PHI_TEXT{i} = num2str(phi(i));
end
theta_t = -[0:10:90 80:-10:0 -10:-10:-90 -80:10:0];
THETA_TEXT{length(theta_t)}='';
for i=1:length(theta_t)
    THETA_TEXT{i} = num2str(theta_t(i));
end
phi = -180:10:180;
phi = phi*pi/180;
D1 = [sin(phi); cos(phi); zeros(size(phi))];
plot3([S*R*D1(1,:); R*D1(1,:)],[S*R*D1(2,:); R*D1(2,:)],[S*R*D1(3,:); R*D1(3,:)],'Color','b','tag','Zplane','Parent',euler_hgt(4));
plot3([S*R*D1(2,:); R*D1(2,:)],[S*R*D1(3,:); R*D1(3,:)],[S*R*D1(1,:); R*D1(1,:)],'Color',[0 0.8 0],'tag','Yplane','Parent',euler_hgt(3));
plot3([S*R*D1(3,:); R*D1(3,:)],[S*R*D1(1,:); R*D1(1,:)],[S*R*D1(2,:); R*D1(2,:)],'Color','r','tag','Xplane','Parent',euler_hgt(2));

% Plot guide lines
HL(1) = plot3([-R R],[0 0],[0 0],'b-','tag','heading_line','parent',euler_hgt(7));
HL(2) = plot3([-R R],[0 0],[0 0],'g-','tag','pitch_line','parent',euler_hgt(6),'color',[0 0.8 0]);
HL(3) = plot3([0 0],[-R R],[0 0],'r-','tag','roll_line','parent',euler_hgt(5));
end


% Test file for trigtech/qr.m

function pass = test_qr(pref)

% Get preferences.
if ( nargin < 1 )
    pref = trigtech.techPref();
end

% Generate a few random points to use as test values.
seedRNG(6178);
x = 2 * rand(100, 1) - 1;

testclass = trigtech();

%%
% Do a few spot-checks.

f = testclass.make(@(x) exp(sin(pi*x)), [], pref);
pass(1:2) = test_one_qr(f, x);
pass(3:4) = test_one_qr_with_perm(f, x);

f = testclass.make(@(x) [exp(sin(pi*x)) 3./(4-cos(pi*x))], [], pref);
pass(5:6) = test_one_qr(f, x);
pass(7:8) = test_one_qr_with_perm(f, x);

f = testclass.make(@(x) [ones(size(x)) sin(pi*x) cos(pi*x) sin(2*pi*x) ...
                         cos(2*pi*x) sin(3*pi*x)], [], pref);
pass(9:10) = test_one_qr(f, x);
pass(11:12) = test_one_qr_with_perm(f, x);

f = testclass.make(@(x) [3./(4-exp(1i*pi*x)) exp(sin(pi*x)) cos(3*pi*x)], ...
[], pref);
pass(13:14) = test_one_qr(f, x);
pass(15:16) = test_one_qr_with_perm(f, x);

%%
% Check that the 'vector' flag works properly.
N = size(f, 2);
[Q1, R1, E1] = qr(f, []);
[Q2, R2, E2] = qr(f, 'vector');
err = E1(E2, :) - eye(N);
pass(17) = all(err(:) == 0);

%%
% Check a rank-deficient problem:
% [TODO]: Is this correct?
%
% Rank deficient QR factorizations fail due to the bug
% in issue #1441. These tests are disabled until the bug
% is addressed.
f = testclass.make(@(x) [cos(pi*x) cos(pi*x) cos(pi*x)], [], pref);
[Q, R] = qr(f, []);
Q = simplify(Q,100*eps);
pass(18) = all(size(Q) == 3) && all(size(R) == 3);
% pass(18) = 1;
I = eye(3);
pass(19) = norm(innerProduct(Q, Q) - I, inf) < ...
10*max(vscale(f)*eps);
pass(19) = 1;
% These tests should be reverted once issue #1441 is
% fixed.

%%
% Check that the vscale comes out with the correct size for QR of an
% array-valued chebtech.
f = testclass.make(@(x) [sin(pi*x) cos(pi*x) cos(2*pi*x)], [], pref);
[Q, R] = qr(f, []);
pass(20) = isequal(size(vscale(Q)), [1 3]);

end

% Tests the QR decomposition for a CHEBTECH object F using a grid of points X
% in [-1  1] for testing samples.
function result = test_one_qr(f, x)
    N = size(f, 2);
    [Q, R] = qr(f);

    % Check orthogonality.
    ip = innerProduct(Q, Q);
    result(1) = max(max(abs(ip - eye(N)))) < 10*max(vscale(f)*eps);

    % Check that the factorization is accurate.
    err = Q*R - f;
    result(2) = norm(feval(err, x), inf) < 100*max(vscale(f)*eps);
end

% Same as the previous function but this time uses the QR factorization with
% permutations.
function result = test_one_qr_with_perm(f, x)
    N = size(f, 2);
    [Q, R, E] = qr(f);

    % Check orthogonality.
    ip = innerProduct(Q, Q);
    result(1) = max(max(abs(ip - eye(N)))) < 10*max(vscale(f)*eps);

    % Check that the factorization is accurate.
    err = Q*R - f*E;
    result(2) = norm(feval(err, x), inf) < 100*max(vscale(f)*eps);
end


function p_homo = homogenize(p,y);
%HOMOGENIZE Homogenize polynomial
%
% f = homogenize(p,t)

deg   = degree(p);
deg_y = degree(y);
if rem(deg,deg_y)~=0
    error('The degree of the homogenizer is not an even fraction of deg(p).');
end

if 0
    error('The homogenizer must be homogenious.');
end

p_variables = getvariables(p);
p_homo = getbasematrix(p,0)*y^(deg/deg_y);
for i = 1:length(p_variables);
    monom = recover(p_variables(i));
    if degree(monom)<deg
        power = (deg-(degree(monom)))/deg_y;
        p_homo = p_homo + getbasematrix(p,p_variables(i))*monom*y^power;
    else
        p_homo = p_homo + getbasematrix(p,p_variables(i))*monom;
    end;
end
% Reset info about conic terms
p_homo.conicinfo = [0 0];


function f = rhsfn(u)

  mu = -1-2*i;
  f = mu*u;


function [B0 phi0 G I_o omega f0 upsilon0]=stvol3prior(ar,arvar,lambda1,lambda3,lambda4,n,m,p,T,k,q,gamma,priorexo)

% obtain B0
% start first by obtaining beta0
beta0=zeros(q,1);
for ii=1:n
beta0((ii-1)*k+ii,1)=ar(ii,1);
end

% if a prior for the exogenous variables is selected put it in here:
for ii=1:n
    beta0(k*ii)=priorexo(ii,1);
end

% reshape the vector to obtain the matrix B0
B0=reshape(beta0,k,n);

% next compute phi0
% set first phi0 as a k*k matrix of zeros
phi0=zeros(k,k);
% set the variance for coefficients on lagged values
for ii=1:n
   for jj=1:p
   phi0((jj-1)*n+ii,(jj-1)*n+ii)=(1/arvar(ii,1))*(lambda1/jj^lambda3)^2;
   end
end
% set the variance for exogenous variables
for ii=1:m
phi0(k-m+ii,k-m+ii)=(lambda1*lambda4(ii))^2;
end

% compute the G matrix
G=speye(T)-sparse(diag(gamma*ones(T-1,1),-1));
% set the value for omega
omega=10000;
% compute I_omega
I_o=sparse(diag([1/omega;ones(T-1,1)]));

% compute the series of f0 vector and upsilon0 matrices
f0=cell(n,1);
upsilon0=cell(n,1);
for ii=2:n
f0{ii,1}=zeros(ii-1,1);
upsilon0{ii,1}=10000*eye(ii-1);
end


function c = mtimes(a,b)
% implements e * c and c * e
%
% Description
% If |e| is a matrix of embeddings and |c| is a matrix of coefficients
% then |e * c| is again a matrix of embeddings defined by
%
% $$ [\mathrm{e * c}]_{j\ell} = \sum_{k} \mathrm{e}_{jk} \mathrm{c}_{k \ell}$$
%
% Syntax
%   out = e * c
%   out = c * e
%
% Input
%  e - @embedding
%  c - double
%
% Output
%  out- @embedding
%

if isa(a,'embedding')
  for i = 1:length(a.u), a.u{i} = a.u{i} * b; end
  c = a;
else
  for i = 1:length(b.u), b.u{i} = a * b.u{i}; end
  c = b;
end

end


function [Vlims, dVlims] = opf_vlim_fcn(x, mpc, idx, mpopt)
%OPF_VLIM_FCN  Evaluates voltage magnitudes and their gradients.
%   [Vlims, dVlims] = OPF_VLIM_FCN(X, MPC, IDX, MPOPT)
%
%   Computes the voltage magnitudes using real and imaginary part of complex voltage for
%   AC optimal power flow. Computes constraint vectors and their gradients.
%
%   Inputs:
%     X : optimization vector
%     MPC : MATPOWER case struct
%     IDX : index of buses whose voltage magnitudes should be fixed
%     MPOPT : MATPOWER options struct
%
%   Outputs:
%     VLIMS  : vector of voltage magnitudes
%     DVLIMS : (optional) magnitude gradients
%
%   Examples:
%       Vlims = opf_vlim_fcn(x, mpc, mpopt);
%       [Vlims, dVlims] = opf_vlim_fcn(x, mpc, idx, mpopt);
%
%   See also OPF_VLIM_HESS

%   MATPOWER
%   Copyright (c) 2018, Power Systems Engineering Research Center (PSERC)
%   by Baljinnyam Sereeter, Delft University of Technology
%   and Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% define named indices into data matrices
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;

%% unpack data
[Vr, Vi] = deal(x{:});

%% problem dimensions
nb = length(Vi);            %% number of buses
n = length(idx);            %% number of buses with voltage limits

%% compute voltage magnitude
Vm2 = Vr(idx).^2 + Vi(idx).^2;
Vlims = [ mpc.bus(idx, VMIN).^2 - Vm2;
          Vm2 - mpc.bus(idx, VMAX).^2 ];

if nargout > 1
    %% compute partials of voltage magnitude w.r.t Vr and Vi
    dVm_dVr = sparse(1:n, idx, 2 * Vr(idx), n, nb);
    dVm_dVi = sparse(1:n, idx, 2 * Vi(idx), n, nb);
    dVlims = [ -dVm_dVr -dVm_dVi;   %% Vlims w.r.t Vr, Vi
                dVm_dVr  dVm_dVi  ];
end


function [min_val max_val] = GradDescLimits(modelname)
%
% function [min_val max_val] = GradDescLimits(modelname)
%
% Returns maximum and minimum settings for the parameters of different
% models to use during direct fitting.
%
% author: Gary Hui Zhang (gary.zhang@ucl.ac.uk)
%

LARGE = 100000;

D_MIN = 0.001;
D_MAX = 3;
D_PERP_MIN = 0.001;

ANGLEMAX=100;

R_MIN = 0.1;
R_MAX = 5;

% the values need to account for scaling defined
% in GetScalingFactors.m
K_MIN = 0;
K_MAX = 6.4;

B_MIN = 0;
B_MAX = 3.2;

parameterStrings = GetParameterStrings(modelname);
min_val = zeros(1, length(parameterStrings));
max_val = zeros(1, length(parameterStrings));

for i=1:length(parameterStrings)
    if (strcmp(parameterStrings(i), 'ficvf') ||...
        strcmp(parameterStrings(i), 'fiso') ||...
        strcmp(parameterStrings(i), 'irfrac'))
        min_val(i) = 0;
        max_val(i) = LARGE;
    elseif (strcmp(parameterStrings(i), 'di') ||...
            strcmp(parameterStrings(i), 'diso'))
        min_val(i) = sqrt(D_MIN);
        max_val(i) = sqrt(D_MAX);
    elseif (strcmp(parameterStrings(i), 'dh'))
        min_val(i) = sqrt(D_PERP_MIN);
        max_val(i) = LARGE;
    elseif (strcmp(parameterStrings(i), 'rad'))
        min_val(i) = sqrt(R_MIN);
        max_val(i) = sqrt(R_MAX);
    elseif (strcmp(parameterStrings(i), 'kappa'))
        min_val(i) = sqrt(K_MIN);
        max_val(i) = sqrt(K_MAX);
    elseif (strcmp(parameterStrings(i), 'beta'))
        min_val(i) = sqrt(B_MIN);
        max_val(i) = sqrt(B_MAX);
    elseif (strcmp(parameterStrings(i), 'theta') ||...
            strcmp(parameterStrings(i), 'phi') ||...
            strcmp(parameterStrings(i), 'psi'))
        min_val(i) = -ANGLEMAX;
        max_val(i) = ANGLEMAX;
    elseif (strcmp(parameterStrings(i), 'b0'))
        min_val(i) = 0.001;
        max_val(i) = LARGE;
    elseif (strcmp(parameterStrings(i), 't1'))
        min_val(i) = 0.1;
        max_val(i) = 1;
    end
end


function [mmHg] = kPa2mmHg(kPa)
% Convert pressure from kilopascals to millimeters of mercury.
% Chad Greene 2012
mmHg = kPa*7.50062;


function [train_eval,test_eval] = ml_kcv_clustering_eval(X,labels,g,train,test,k,train_eval,test_eval,varargin)
%ML_CLUSTERING_EVAL
%
%  input ------------------------------------------------------------------
%
%       o   X        : (N x D), dataset.
%
%       o   labels   : (N x 1),  class labels : if classification (discrete)
%
%       o   g        : function handle, y = g(x)
%
%       o   train    : (M x 1),  set of indicies of X to use as train data.
%
%       o   test     : (P x 1),  set of indicies of X to use for testing.
%
%       o   k        : (1 x 1),  current iteration of k-fold
%
%       o   varargin : if you want to have a different set of labels to
%                       test put it here
%
%   output ----------------------------------------------------------------
%
%       o   train_eval : struct
%
%       o   test_eval  : struct

 % Evaluate classifier on train data

    M                        = ml_confusion_matrix(X(train(:),:),labels(train(:)),g);
    [A, P, R, F, FPR, TNR]   = ml_confusion_matrix_evaluation(M);

    if isempty(k)
        k = 1;
    end
    train_eval.accuracy(k)    = A;
    %     train_eval.precision(:,k) = P;
    %     train_eval.recall(:,k)    = R;
    train_eval.fmeasure(k)    = F;
    train_eval.fpr(k)         = FPR;
    train_eval.tnr(k)         = TNR;

    if (test~=0)
        % Evaluate classifier on test data
        if(length(varargin) == 0)
            M           = ml_confusion_matrix(X(test(:),:),labels(test(:)),g);
        else
            labelsTest = varargin{1};
            M           = ml_confusion_matrix(X(test(:),:),labelsTest(test(:)),g);
        end

        [A, P, R, F, FPR, TNR]   = ml_confusion_matrix_evaluation(M);

        test_eval.accuracy(k)    = A;
        %     train_eval.precision(:,k) = P;
        %     train_eval.recall(:,k)    = R;
        test_eval.fmeasure(k)    = F;
        test_eval.fpr(k)         = FPR;
        test_eval.tnr(k)         = TNR;
    end
end


function [ obj ] = bold_grad_cd( obj, n )
% Calculate jacobian matrix of predicted fMRI BOLD signal with respect to
% DCM parameters using central difference method.
%
% This is a protected method of the tapas_Huge class. It cannot be called
% from outside the class.
%

% Author: Yu Yao (yao@biomed.ee.ethz.ch)
% Copyright (C) 2019 Translational Neuromodeling Unit
%                    Institute for Biomedical Engineering,
%                    University of Zurich and ETH Zurich.
%
% This file is part of TAPAS, which is released under the terms of the GNU
% General Public Licence (GPL), version 3. For further details, see
% <https://www.gnu.org/licenses/>.
%
% This software is provided "as is", without warranty of any kind, express
% or implied, including, but not limited to the warranties of
% merchantability, fitness for a particular purpose and non-infringement.
%
% This software is intended for research only. Do not use for clinical
% purpose. Please note that this toolbox is under active development.
% Considerable changes may occur in future releases. For support please
% refer to:
% https://github.com/translationalneuromodeling/tapas/issues
%

% prediction error for current mean DCM parameter
mu_n = obj.posterior.mu_n(n,:);
data = obj.data(n);
obj.aux.epsilon{n} = obj.bold_gen(mu_n, data, obj.inputs(n), ...
    obj.options.hemo, obj.R, obj.L, obj.idx );

% gradient
obj.aux.G{n} = zeros(numel(obj.data(n).bold), obj.idx.P_c + obj.idx.P_h);
data.bold(:) = 0;
% central difference
for p = 1:obj.idx.P_c + obj.idx.P_h
    for s = -1:2:1
        % perturb parameters
        mu_n = obj.posterior.mu_n(n,:);
        mu_n(p) = mu_n(p) + s*obj.options.delta;
        % generate bold signal
        pred = obj.bold_gen(mu_n, data, obj.inputs(n), obj.options.hemo,...
            obj.R, obj.L, obj.idx );
        % calculate difference
        obj.aux.G{n}(:,p) = obj.aux.G{n}(:,p) - s*pred(:);
    end
end
% normalize by step size
obj.aux.G{n} = obj.aux.G{n}/obj.options.delta/2;

end


function [M_R] = vox2ras_rsolveAA(Vc_C, inPlaneRotation, varargin)
%%
%% NAME
%%
%%     vox2ras_rsolveAA.m (vox2ras_r{otation}solveA{uto}A{lign})
%%
%% AUTHOR
%%
%%	Rudolph Pienaar
%%
%% SYNOPSIS
%%
%%     [M_R] = vox2ras_rsolveAA(Vc_C, inPlaneRotation, ch_orientation)
%%
%% ARGUMENTS
%%
%%      Vc_C		in      column vector defining a direction cosine for
%%					a volume
%%	inPlaneRotation	in	scalar float defining the in plane rotation as
%%					read from the data meas.asc file
%%	ch_orientation	in	optional character string definining the plane of
%%					the main slab orientation
%%	M_R		out	vox2ras rotational candidate
%%
%% DESCRIPTION
%%
%%	"vox2ras_rsolveAA" attempts to find a candidate vox2ras rotation matrix using
%%	techniques described in autoaligncorrect.cpp.
%%
%%	The optional character string, ch_orientation, defaults to 's' (sagittal)
%%	and defines the orientation of the main slab. This string is one of:
%%
%%		's'	sagittal
%%		'c'	coronal
%%		't'	transverse
%%
%% PRECONDITIONS
%%
%%	o The vector C is read from a Siemens meas.asc file such that
%%		ci	= sSliceArray.asSlice[0].sNormal.dSag
%%		cj	= sSliceArray.asSlice[0].sNormal.dCor
%%		ck	= sSliceArray.asSlice[0].sNormal.dTra
%%
%% POSTCONDITIONS
%%
%%	o All returned matrices are 4x4.
%%	o Only the rotations of the vox2ras matrix are determined by this function.
%%		The center of k-space is not determined.
%%
%% SEE ALSO
%%
%%	vox2ras_rsolve	- determine the rotational component of a vox2ras matrix
%%				using Siemens reference orientations indirectly
%%	vox2ras_ksolve	- determine the k-space col in RAS of a vox2ras matrix
%%	vox2ras_dfmeas	- main function: determines the vox2ras matrix from a
%%			  Siemens meas.asc file.
%%
%% HISTORY
%%
%% 02 June 2004
%% o Initial design and coding.
%%

%
% vox2ras_rsolveAA.m
%
% Original Author: Rudolph Pienaar
%
% Copyright © 2011 The General Hospital Corporation (Boston, MA) "MGH"
%
% Terms and conditions for use, reproduction, distribution and contribution
% are found in the 'FreeSurfer Software License Agreement' contained
% in the file 'LICENSE' found in the FreeSurfer distribution, and here:
%
% https://surfer.nmr.mgh.harvard.edu/fswiki/FreeSurferSoftwareLicense
%
% Reporting: freesurfer@nmr.mgh.harvard.edu
%

M_R		= zeros(4, 4);
Vc_Cn		= Vc_C./(norm(Vc_C));
%Vc_Cn		= Vc_C;
ch_orientation	= 's';
if length(varargin)
	ch_orientation	= varargin{1}(1);
end

%% phase reference vector -
%%	adapted from Andre van der Kouwe's "autoaligncorrect.cpp"
Vc_P	= zeros(3, 1);
switch ch_orientation
    case 't'
    	Vc_P(1)	= 0;
	Vc_P(2)	=  Vc_Cn(3)*sqrt(1/(Vc_Cn(2)*Vc_Cn(2)+Vc_Cn(3)*Vc_Cn(3)));
	Vc_P(3)	= -Vc_Cn(2)*sqrt(1/(Vc_Cn(2)*Vc_Cn(2)+Vc_Cn(3)*Vc_Cn(3)));
    case 'c'
	Vc_P(1)	=  Vc_Cn(2)*sqrt(1/(Vc_Cn(1)*Vc_Cn(1)+Vc_Cn(2)*Vc_Cn(2)));
	Vc_P(2)	= -Vc_Cn(1)*sqrt(1/(Vc_Cn(1)*Vc_Cn(1)+Vc_Cn(2)*Vc_Cn(2)));
    	Vc_P(3)	= 0;
    case 's'
	Vc_P(1)	= -Vc_Cn(2)*sqrt(1/(Vc_Cn(1)*Vc_Cn(1)+Vc_Cn(2)*Vc_Cn(2)));
	Vc_P(2)	=  Vc_Cn(1)*sqrt(1/(Vc_Cn(1)*Vc_Cn(1)+Vc_Cn(2)*Vc_Cn(2)));
    	Vc_P(3)	= 0;
    otherwise
        fprintf(1, 'Unknown orientation parameter passed. Returning with dummy M_R');
	return;
end

%% The readout reference vector is the cross product of Vc_Cn and Vc_P
Vc_R	= cross(Vc_Cn, Vc_P);

M_R(1:3, 1)	= Vc_P;
M_R(1:3, 2)	= Vc_R;
M_R(1:3, 3)	= Vc_C;

%% The above calculated rotation matrices define an (x,y) plane
%%	given by the first two column vectors. These reference
%%	vectors need to be rotated by inPlaneRotation to
%%	reach the final rotational vox2ras.

theta_f		= inPlaneRotation;

M3_Mu	= [	 cos(theta_f)	 sin(theta_f)	0
		-sin(theta_f)	 cos(theta_f)	0
		 	0		0	1];
M3_R	= M_R(1:3,1:3)	* M3_Mu;
M_R(1:3,1:3)	= M3_R;

%% The MGH vox2ras matrix inverts the Readout column
M_R		= M_R * [ 1  0  0  0
			  0 -1  0  0
			  0  0  1  0
			  0  0  0  1];

%% All done!


function res = indfrequency(this, f)
% Method for getting the index closest to given frequency
% FORMAT  res = indfrequency(this, f)
% this       - MEEG object
% f          - vector of frequencies (in Hz)
%
% res        - vector of sample indices matching indices
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Stefan Kiebel
% $Id: indfrequency.m 3254 2009-07-07 15:18:54Z vladimir $

if ~strncmpi(transformtype(this),'TF',2)
    error('Only TF datasets are supported');
end

res = NaN(1,length(f));
fdiff = mean(diff(frequencies(this)));
if nsamples(this) > 0
    F = frequencies(this);
    for i = 1:length(f)
        [m, res(i)] = min(abs(F-f(i)));
        if m > fdiff
            warning('Could not find an index matching the requested frequency %d Hz', f(i));
            res(i) = NaN;
        end
    end
end


function [orient,cano_rot]=cosmo_fmri_orientation(ds)
% get orientation of a dataset
%
% [orient,cano_rot]=cosmo_fmri_orientation(ds)
%
% Inputs:
%    ds             fmri dataset struct;
%
% Output:
%    orient         three letter string indicating the orientation of this
%                   dataset
%    cano_rot       canonical rotation matrix (relative to LPI)
%
% Example:
%     ds=cosmo_synthetic_dataset();
%     [orient,cano_rot]=cosmo_fmri_orientation(ds);
%     disp(orient)
%     %|| LPI
%     disp(cano_rot)
%     %|| 1 0 0 0
%     %|| 0 1 0 0
%     %|| 0 0 1 0
%     %|| 0 0 0 1
%
% Notes:
%   - there are 3!*3^2 valid orientations, these are:
%         'SAR'  'SAL'  'SPR'  'SPL'  'IAR'  'IAL'  'IPR'  'IPL'
%         'SRA'  'SLA'  'SRP'  'SLP'  'IRA'  'ILA'  'IRP'  'ILP'
%         'ASR'  'ASL'  'PSR'  'PSL'  'AIR'  'AIL'  'PIR'  'PIL'
%         'ARS'  'ALS'  'PRS'  'PLS'  'ARI'  'ALI'  'PRI'  'PLI'
%         'RAS'  'LAS'  'RPS'  'LPS'  'RAI'  'LAI'  'RPI'  'LPI'
%         'RSA'  'LSA'  'RSP'  'LSP'  'RIA'  'LIA'  'RIP'  'LIP'
%     For example, 'LPI' (used in Talairach/MNI) means that
%       * the first dimension goes from left to right
%       * the second dimension goes from posterior to anterior
%       * the third dimension goes from inferior to superior
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

    mx=get_affine_matrix(ds);

    rot_orig=mx(1:3,1:3);

    % normalize the rows
    rot_row_norms=sqrt(sum(rot_orig.^2,2));
    rot=bsxfun(@rdivide,rot_orig,rot_row_norms);

    cano_rot=get_canonical_matrix(rot);

    orient=get_orientation(cano_rot);

function cano_rot=get_canonical_matrix(rot)
    % find the three major axes of the rotation matrix
    visited=false(3);
    cano_rot=zeros(4);
    cano_rot(4,4)=1;

    for dim=1:3
        max_v=0;
        for row=1:3
            for col=1:3
                v=abs(rot(row,col));
                if ~visited(row,col) && v>max_v
                    max_row=row;
                    max_col=col;
                    max_v=v;
                end
            end
        end
        if max_v==0
            error('Illegal matrix');
        end
        visited(max_row,:)=true;
        visited(:,max_col)=true;

        cano_rot(max_row,max_col)=sign(rot(max_row,max_col));
    end

    assert(all(visited(:)));
    assert(all(sum(cano_rot~=0,1)==1));
    assert(all(sum(cano_rot~=0,2)==1));

function orient=get_orientation(cano_rot)
    labs=['LR';'PA';'IS'];

    orient='   ';
    for k=1:3
        col=find(cano_rot(1:3,k)~=0);
        assert(numel(col)==1);
        v=cano_rot(col,k);
        orient(k)=labs(col,1+(v<0));
    end

function mx=get_affine_matrix(ds)
    if cosmo_isfield(ds,'a.vol.mat')
        mx=ds.a.vol.mat;
    elseif isnumeric(ds) && size(ds,1)>=3 && size(ds,2)>=3
        mx=ds;
    else
        error('cannot find affine transformation matrix');
    end


clear
close all
clc;

%time
dt=0.01;

%coeffs
Cg=1.3;
Cd=1;
minSpeed=20;

%green
greenheight=double(imread('testgreen.tiff'));
[gradX,gradY]=gradient(greenheight(:,:,1));
forceFun=@(R,RL) (RL-R)/dt*Cd + [interp2(-gradX,R(1,:),R(2,:));interp2(-gradY,R(1,:),R(2,:))]*Cg;
%forceFun=@(R,RL) [interp2(-gradX,R(1,:),R(2,:));interp2(-gradY,R(1,:),R(2,:))];
l=size(greenheight,1);

%graphics
%sph=sphere(30)
alt=70;
az=0;

% balls
startLoc=[64;400];

angles=linspace(0,2*pi,360/4+1);
angles=angles(1:end-1);
speeds=linspace(minSpeed,1000,20);

numParticles=length(speeds)*length(angles);

speeds=reshape(speeds,1,1,length(speeds));
angleVectors=[cos(angles);sin(angles)];
startConditions=bsxfun(@times,speeds,angleVectors);
startConditions=reshape(startConditions,2,length(speeds)*length(angles));

r=repmat(startLoc,1,numParticles);
rl=r-startConditions*dt;

i=0;
while sum(sum(r~=rl))
    i=i+1;
    rn=2*r-rl+(forceFun(r,rl))*dt^2;
    rl=r;
    r=rn;

    %static friction
    dr=rl-r;
    s=sqrt(dr(1,:).^2+dr(2,:).^2)/dt;

    %walls
    haltBallsEdge=(r>l)|(r<1);
    haltBalls=(haltBallsEdge(1,:)|haltBallsEdge(2,:))|(s<minSpeed);

    r(:,haltBalls)=rl(:,haltBalls);

    if mod(i,10)==0
        figure(1)
        surf(greenheight,'edgecolor','none')
        hold on
%         scatter3(r(1,1),r(2,1),greenheight(floor(r(2,1)),floor(r(1,1))),'wo','filled')
%         hold off
%         az=az+1;
%         view([az alt])
        imshow(greenheight,[min(min(greenheight)),max(max(greenheight))],'colormap',colormap('parula'))
        hold on
        scatter(r(1,:),r(2,:),'w.')
        hold off
        title(num2str(s(1)))
    end
end


function r = invgamrand1(a, b, varargin)
%INVGAMRAND1 Random matrices from inverse gamma distribution (mex)
%
%   R = INVGAMRAND1(A,B) returns a matrix of random numbers chosen
%   from the inverse gamma distribution with parameters A and B.
%   The size of R is the common size of A and B if both are matrices.
%   Both parameters have to be a scalar.
%
%   Note: Parameterization as in (Neal, 1996).
%      A is mean of the distribution
%      B is degrees of freedom
%
%	See also INVGAMRAND, GAMRAND1
%
% Copyright (c) 1999 Aki Vehtari

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

error('No mex-file for this archtitecture. See Matlab help and convert.m in ./linuxCsource or ./winCsource for help.')


function [angle_rot, bCheck] = WardSimpleRot(img1, img2)
%
%
%       [angle_rot, bCheck] = WardSimpleRot(img1, img2)
%
%       This function computes the Ward's MTB.
%
%       Input:
%           -img1: the target image
%           -img2: the image that needs to be aligned to img1
%
%       Output:
%           -rot: rotation angle (degree) for aligning img2 into img1.
%
%     Copyright (C) 2013-15  Francesco Banterle. A big thank to Greg J. Ward
%     for help during the implementation.
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

[r, c, ~] = size(img1);

blocksY = 3;
blocksX = 4;
sizeY = round(r / blocksY);
sizeX = round(c / blocksX);

angle = [];

%Analyzing blocks
for i=1:blocksY
    rect  = [sizeY * (i - 1) + 1, sizeY * i, 1, sizeX];
    [tmpAngle, check] = WardSimpleRotAux(img1, img2, rect);
    if(check == 1)
        angle = [angle, tmpAngle];
    end
end

%Final Merging
if(isempty(angle))
    angle_rot = 0.0;
    bCheck = 0;
else
    rotThreshold = (0.07 * 180.0) / pi;
    npos = 0;
    nneg = 0;
    for i=1:length(angle)

        if(angle(i) >  rotThreshold)
            npos = npos + 1;
        end

        if(angle(i) < -rotThreshold)
            nneg = nneg + 1;
        end
    end

    if(bitand(nneg, npos))
        angle_rot = 0.0;
        bCheck = 0;
    else
        angle_rot = mean(angle);
        bCheck = 1;
    end
end

end


function chebyshev_polynomial_test03 ( )

%*****************************************************************************80
%
%% CHEBYSHEV_POLYNOMIAL_TEST03 tests T_POLYNOMIAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBYSHEV_POLYNOMIAL_TEST03:\n' );
  fprintf ( 1, '  T_POLYNOMIAL_VALUES stores values of\n' );
  fprintf ( 1, '  the Chebyshev polynomials.\n' );
  fprintf ( 1, '  T_POLYNOMIAL evaluates the polynomial.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '                        Tabulated                 Computed\n' );
  fprintf ( 1, '     N        X           T(n,x)                    T(n,x)                     Error\n' );

  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, x, fx1 ] = t_polynomial_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2_vec = t_polynomial ( 1, n, x );
    fx2 = fx2_vec(1,n+1);
    e = fx1 - fx2;

    fprintf ( 1, '  %4d  %12f  %24.16e  %24.16e  %8.2g\n', n, x, fx1, fx2, e );

  end

  return
end


function avg = MovAvgFilter2(x)
%
%
persistent n xbuf
persistent firstRun

if isempty(firstRun)
  n = 10;
  xbuf = x*ones(n, 1);

  firstRun = 1;
end

for m=1:n-1
  xbuf(m) = xbuf(m+1);
end
xbuf(n) = x;

avg = sum(xbuf) / n;


function [map,timeBins] = SyncMap(synchronized,indices,varargin)

%SyncMap - Create a map from successive event-synchronized data.
%
% Using N repetitions of similar data centered around synchronizing
% events (e.g. N evoked potentials), create a map v = f(t,i) where
% t is the time relative to the synchronizing events and i is the
% occurrence (from 1 to N).
%
%  USAGE
%
%    [map,timeBins] = SyncMap(synchronized,indices,<options>)
%
%    synchronized   event-synchronized samples
%    indices        list of synchronizing event indices for each sample
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'durations'   durations before and after synchronizing events for each
%                   trial (in s) (default = [-0.5 0.5])
%     'nBins'       total number of time bins (default 100)
%     'smooth'      smoothing size (0 = no smoothing) (default = 0.01*nBins)
%    =========================================================================
%
%  SEE
%
%    See also Sync, SyncHist, PlotSync, PETHTransition.

% Copyright (C) 2004-2011 by Michaël Zugaro
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.

% Default values
durations = [-0.5 0.5];
nBins = 100;
smooth = [];

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

% Check parameter sizes
if size(synchronized,2) > 2,
	error('Parameter ''synchronized'' is not a Nx2 matrix (type ''help SyncMap'' for details).');
end
if size(indices,2) ~= 1,
	error('Parameter ''indices'' is not a vector (type ''help SyncMap'' for details).');
end
if size(indices,1) ~= size(synchronized,1),
	error('Parameters ''synchronized'' and ''indices'' have different lengths (type ''help SyncMap'' for details).');
end

% Parse parameter list
for i = 1:2:length(varargin),
	if ~ischar(varargin{i}),
		error(['Parameter ' num2str(i+2) ' is not a property (type ''help SyncMap'' for details).']);
	end
	switch(lower(varargin{i})),
		case 'durations',
			durations = varargin{i+1};
			if ~isdvector(durations,'#2','<'),
				error('Incorrect value for property ''durations'' (type ''help SyncMap'' for details).');
			end
		case 'nbins',
			nBins = varargin{i+1};
			if ~isiscalar(nBins,'>0'),
				error('Incorrect value for property ''nBins'' (type ''help SyncMap'' for details).');
			end
		case 'smooth',
			smooth = varargin{i+1};
			if ~isdvector(smooth,'>=0'),
				error('Incorrect value for property ''smooth'' (type ''help SyncMap'' for details).');
			end

		otherwise,
			error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help SyncMap'' for details).']);
	end
end

if isempty(smooth),
	smooth = ceil(0.01*nBins);
end

start = durations(1);
stop = durations(2);
timeBinSize = (stop - start)/nBins;
timeBins = (start:timeBinSize:stop-timeBinSize)+timeBinSize/2;
binnedTime = Bin(synchronized(:,1),durations,nBins);
if size(synchronized,2) == 1,
	% Occurrences
	s = Accumulate([indices binnedTime],1);
	map = Smooth(s,smooth);
else
	% Values
	s = Accumulate([indices binnedTime],synchronized(:,2));
	n = Accumulate([indices binnedTime],1);
	n(n==0) = 1;
	map = Smooth(s,[smooth 0])./Smooth(n,[smooth 0]);
end


function [vert,conn,tria,tnum] = tridiv2(varargin)
%TRIDIV2 "quadtree" refinement for 2-simplex triangulations.
%   [VERT,EDGE,TRIA,TNUM] = TRIDIV2(VERT,EDGE,TRIA,TNUM) re-
%   turns a globally refined triangulation, in which all ed-
%   ges are bisected about their midpoints. Such refinement
%   splits each triangle into four new sub-triangles accord-
%   ing to a shape-preserving scheme.
%
%   VERT is a V-by-2 array of XY coordinates in the triangu-
%   lation, EDGE is an array of constrained edges, TRIA is a
%   T-by-3 array of triangles, and TNUM is a T-by-1 array of
%   part indices. Each row of TRIA and EDGE define an eleme-
%   nt. VERT(TRIA(II,1),:), VERT(TRIA(II,2),:) and VERT(TRIA
%   (II,3),:) are the coordinates of the II-TH triangle. The
%   edges in EDGE are defined in a similar manner. NUM is an
%   array of part indexing, such that TNUM(II) is the index
%   of the part in which the II-TH triangle resides.
%
%   [VERT,EDGE,TRIA,TNUM] = TRIDIV2(... ,TDIV) returns a se-
%   lectively refined mesh, where TDIV is a T-by-1 logical
%   array, with TDIV(KK) = TRUE if TRIA(KK,:) is to be refi-
%   ned. Such triangles are refined using the four-way split
%   described above. Additionally, a "halo" of adjacent tri-
%   angles are also refined to preseve compatibility of the
%   mesh. Such triangles are refined using a two-way bisect-
%   ion type scheme.

%   See also REFINE2, SMOOTH2

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 29/01/2017

%---------------------------------------------- extract args
    vert = []; conn = []; tria = []; tnum = [] ;
    tdiv = [];

    if (nargin>=+1), vert = varargin{1}; end
    if (nargin>=+2), conn = varargin{2}; end
    if (nargin>=+3), tria = varargin{3}; end
    if (nargin>=+4), tnum = varargin{4}; end
    if (nargin>=+5), tdiv = varargin{5}; end

%---------------------------------------------- default arg.
    if (isempty(tnum))
        tnum = ones(size(tria,1),1) ;
    end
    if (isempty(tdiv))
        tdiv = true(size(tria,1),1) ;
    end

%---------------------------------------------- basic checks
    if ( ~isnumeric(vert) || ...
         ~isnumeric(conn) || ...
         ~isnumeric(tria) || ...
         ~isnumeric(tnum) || ...
         ~islogical(tdiv) )
        error('tridiv2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    tnum = tnum(:) ; tdiv = tdiv(:) ;
    if (ndims(vert) ~= +2 || ...
        ndims(conn) ~= +2 || ...
        ndims(tria) ~= +2 || ...
        ndims(tnum) ~= +2 || ...
        ndims(tdiv) ~= +2 )
        error('tridiv2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(vert,2)~= +2 || ...
        size(conn,2)~= +2 || ...
        size(tria,2)~= +3 || ...
        size(tnum,2)~= +1 || ...
        size(tria,1)~= size(tnum,1) )
        error('tridiv2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    nvrt = size(vert,1) ;

%---------------------------------------------- basic checks
    if (min(min(conn(:,1:2))) < +1 || ...
            max(max(conn(:,1:2))) > nvrt )
        error('tridiv2:invalidInputs', ...
            'Invalid EDGE input array.') ;
    end

    if (min(min(tria(:,1:3))) < +1 || ...
            max(max(tria(:,1:3))) > nvrt )
        error('tridiv2:invalidInputs', ...
            'Invalid TRIA input array.') ;
    end

%------------------------------ assemble extended adj. info.
   [edge,tria] = tricon2(tria,conn) ;

    ediv = false(size(edge,1),1) ;
    ediv(tria(tdiv,4:6))  = true ;

    snum = length(find(ediv));

    while (true)

    %-------------------------- tria's with >= 2 edge splits
        div3 = sum(double( ...
            ediv(tria(:,4:6))),2)>=2;

    %-------------------------- expand onto adj. edge splits
        ediv(tria(div3,4:6)) = true ;

        snew = length(find(ediv)) ;

        if (snew == snum), break; end

        snum = snew ;

    end

%------------------------------ tria's with == 1 edge splits
    div1 = sum( ...
    double( ediv(tria(:,4:6))),2)==1;

%------------------------------ indexing for mid-point vert.
    ivec = zeros(size(edge,1),1);
    ivec(ediv) = ...
        (1:snum)' + size(vert,1);

%------------------------------------------ update vert. set
    emid = vert(edge(ediv,1),:) ...
         + vert(edge(ediv,2),:) ;
    vert =[vert ; emid * 0.5] ;

%------------------------------------------ update edge. set
    [cvec,eloc] = ...
        setset2(conn,edge(ediv, 1:2)) ;

    epos = ivec(ediv);
    epos = epos(eloc(eloc>0)) ;

    conn = [conn(~cvec,1:2) ; ...
        conn(cvec,1) , epos ; ...
        conn(cvec,2) , epos ] ;

%------------------------------------ push 1-to-4 refinement
    tr41 = ones(length(find(div3)),3) ;
    tn41 = ones(length(find(div3)),1) ;
    tn41(:,1) = tnum(div3,1);
    tr41(:,1) = tria(div3,1);
    tr41(:,2) = ivec(tria(div3,4));
    tr41(:,3) = ivec(tria(div3,6));

    tr42 = ones(length(find(div3)),3) ;
    tn42 = ones(length(find(div3)),1) ;
    tn42(:,1) = tnum(div3,1);
    tr42(:,1) = ivec(tria(div3,4));
    tr42(:,2) = tria(div3,2);
    tr42(:,3) = ivec(tria(div3,5));

    tr43 = ones(length(find(div3)),3) ;
    tn43 = ones(length(find(div3)),1) ;
    tn43(:,1) = tnum(div3,1);
    tr43(:,1) = ivec(tria(div3,6));
    tr43(:,2) = ivec(tria(div3,5));
    tr43(:,3) = tria(div3,3);

    tr44 = ones(length(find(div3)),3) ;
    tn44 = ones(length(find(div3)),1) ;
    tn44(:,1) = tnum(div3,1);
    tr44(:,1) = ivec(tria(div3,6));
    tr44(:,2) = ivec(tria(div3,4));
    tr44(:,3) = ivec(tria(div3,5));

%----------------------- push 1-to-2 refinement about edge 1
    tvec = false(size(tria,1), 1) ;
    tvec(ediv(tria(:,4))&div1) = true ;

    tr21 = ones(length(find(tvec)),3) ;
    tn21 = ones(length(find(tvec)),1) ;
    tn21(:,1) = tnum(tvec,1);
    tr21(:,1) = ivec(tria(tvec,4));
    tr21(:,2) = tria(tvec,3);
    tr21(:,3) = tria(tvec,1);

    tr22 = ones(length(find(tvec)),3) ;
    tn22 = ones(length(find(tvec)),1) ;
    tn22(:,1) = tnum(tvec,1);
    tr22(:,1) = ivec(tria(tvec,4));
    tr22(:,2) = tria(tvec,2);
    tr22(:,3) = tria(tvec,3);

%----------------------- push 1-to-2 refinement about edge 2
    tvec = false(size(tria,1), 1) ;
    tvec(ediv(tria(:,5))&div1) = true ;

    tr23 = ones(length(find(tvec)),3) ;
    tn23 = ones(length(find(tvec)),1) ;
    tn23(:,1) = tnum(tvec,1);
    tr23(:,1) = ivec(tria(tvec,5));
    tr23(:,2) = tria(tvec,1);
    tr23(:,3) = tria(tvec,2);

    tr24 = ones(length(find(tvec)),3) ;
    tn24 = ones(length(find(tvec)),1) ;
    tn24(:,1) = tnum(tvec,1);
    tr24(:,1) = ivec(tria(tvec,5));
    tr24(:,2) = tria(tvec,3);
    tr24(:,3) = tria(tvec,1);

%----------------------- push 1-to-2 refinement about edge 3
    tvec = false(size(tria,1), 1) ;
    tvec(ediv(tria(:,6))&div1) = true ;

    tr25 = ones(length(find(tvec)),3) ;
    tn25 = ones(length(find(tvec)),1) ;
    tn25(:,1) = tnum(tvec,1);
    tr25(:,1) = ivec(tria(tvec,6));
    tr25(:,2) = tria(tvec,2);
    tr25(:,3) = tria(tvec,3);

    tr26 = ones(length(find(tvec)),3) ;
    tn26 = ones(length(find(tvec)),1) ;
    tn26(:,1) = tnum(tvec,1);
    tr26(:,1) = ivec(tria(tvec,6));
    tr26(:,2) = tria(tvec,1);
    tr26(:,3) = tria(tvec,2);

%------------------------------------------ update tria. set
    tria = [tria(~div1&~div3,1:3) ; ...
        tr41 ; tr42 ; tr43 ; tr44 ; ...
        tr21 ; tr22 ;
        tr23 ; tr24 ;
        tr25 ; tr26 ] ;

    tnum = [tnum(~div1&~div3,1:1) ; ...
        tn41 ; tn42 ; tn43 ; tn44 ; ...
        tn21 ; tn22 ;
        tn23 ; tn24 ;
        tn25 ; tn26 ] ;

end


function y = Insert(x,lines,where)

%Insert - Insert lines in a matrix.
%
%  USAGE
%
%    result = Insert(matrix,lines,indices)
%
%    array          matrix where the lines should be inserted
%    lines          list of values to insert
%    indices        list of (possibly repeated) matrix line numbers
%                   after which new lines should be inserted (use 0
%                   to insert before first line)
%
%  EXAMPLES
%
%    >> Insert([1;2;3;4;5],[-1;-2;-3],[0;3;5])
%
%    ans =
%
%      -1
%       1
%       2
%       3
%      -2
%       4
%       5
%      -3

% Copyright (C) 2004-2011 by Michaël Zugaro
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.

where = where(:);
mx = size(x,1);
nx = size(x,2);
ml = length(where);
if size(lines,1) == 1,
	lines = repmat(lines,ml,1);
end

% Lines in x must be moved to new indices in y
w = Accumulate(where+1,1,[mx+1,1]);
new = cumsum(w)+(1:mx+1)';
new(end) = [];

% Copy x into y
y = zeros(mx+ml,nx);
y(new,:) = x;

inserted = where+(1:ml)';
y(inserted,:) = lines;


function [F1,F2]=framepair(ftype,g1,g2,varargin)
%FRAMEPAIR  Construct a new frame
%   Usage: [F1,F2]=framepair(ftype,g1,g2,...);
%
%   `[F1,F2]=framepair(ftype,g1,g2,...)` constructs two new frame objects
%   *F1* and *F2* of the same type *ftype* using the windows *g1* and *g2*.
%   The windows are specific to choosen frame type. See the help on *frame*
%   for the windows and arguments.
%
%   This function makes it easy to create a pair of canonical dual frames:
%   simply specify `'dual'` as window if one frame should be the dual of the
%   other.
%
%   This is most easily explained through some examples. The following
%   example creates a Gabor frame for real-valued signals with a Gaussian
%   analysis window and its canonical dual frame as the synthesis frame:::
%
%      f=greasy;
%      [Fa,Fs]=framepair('dgtreal','gauss','dual',20,294);
%      c=frana(Fa,f);
%      r=frsyn(Fs,c);
%      norm(f-r)
%
%   The following example creates a Wilson basis with a Gaussian
%   synthesis window, and its canonical dual frame as the analysis
%   frame::
%
%     [Fa,Fs]=framepair('dwilt','dual','gauss',20);
%
%   See also: frame, framedual, frametight

complainif_notenoughargs(nargin,3,'FRAMEPAIR');

ftype=lower(ftype);

if ~strcmp(g1,'dual')
    F1=frame(ftype,g1,varargin{:});
end;

if ~strcmp(g2,'dual')
    F2=frame(ftype,g2,varargin{:});
end;

if strcmp(g1,'dual')
    F1=framedual(F2);
end;

if strcmp(g2,'dual')
    F2=framedual(F1);
end;


function varargout = drawCylinder(varargin)
%DRAWCYLINDER Draw a cylinder.
%
%   drawCylinder(CYL)
%   Draws the cylinder CYL on the current axis.
%   CYL is a 1-by-7 row vector in the form [x1 y1 z1 x2 y2 z2 r] where:
%   * [x1 y1 z1] are the coordinates of starting point,
%   * [x2 y2 z2] are the coordinates of ending point,
%   * R is the radius of the cylinder
%
%   drawCylinder(CYL, N)
%   Uses N points for discretizating the circles of the cylinder. Default
%   value is 32.
%
%   drawCylinder(..., OPT)
%   with OPT = 'open' (default) or 'closed', specify if the bases of the
%   cylinder should be drawn.
%
%   drawCylinder(..., 'FaceColor', COLOR)
%   Specifies the color of the cylinder. Any couple of parameters name and
%   value can be given as argument, and will be transfered to the 'surf'
%   matlab function
%
%   drawCylinder(..., 'FaceAlpha', ALPHA)
%   Specifies the transparency of the cylinder and of the optionnal caps.
%
%   drawCylinder(AX, ...)
%   Specifies the axis to draw on. AX should be a valid axis handle.
%
%   H = drawCylinder(...)
%   Returns a handle to the patch representing the cylinder.
%
%
%   Examples:
%   % basic example
%     figure; drawCylinder([0 0 0 10 20 30 5]);
%
%   % draw hollow cylinders
%     figure; drawCylinder([0 0 0 10 20 30 5], 'open');
%
%   % change cylinder color
%     figure; drawCylinder([0 0 0 10 20 30 5], 'FaceColor', 'r');
%
%   % change cylinder color using graphical handle
%     figure;
%     h = drawCylinder([0 0 0 10 20 30 5]);
%     set(h, 'facecolor', 'b');
%
%   % Draw three mutually intersecting cylinders
%     p0 = [10 10 10];
%     p1 = p0 + 80 * [1 0 0];
%     p2 = p0 + 80 * [0 1 0];
%     p3 = p0 + 80 * [0 0 1];
%     figure; axis equal; axis([0 100 0 100 0 100]); hold on
%     drawCylinder([p0 p1 10], 'FaceColor', 'r');
%     drawCylinder([p0 p2 10], 'FaceColor', 'g');
%     drawCylinder([p0 p3 10], 'FaceColor', 'b');
%     axis equal
%     set(gcf, 'renderer', 'opengl')
%     view([60 30]); light;
%
%   % draw cube skeleton
%     [v, e, f] = createCube;
%     figure; axis equal; axis([-0.2 1.2 -0.2 1.2 -0.2 1.2]); hold on; view(3);
%     cyls = [v(e(:,1), :) v(e(:,2),:) repmat(0.1, size(e, 1), 1)];
%     drawCylinder(cyls);
%     light
%
%   See also
%     cylinderMesh, drawEllipseCylinder, drawSphere, drawLine3d, surf
%     intersectLineCylinder, cylinderSurfaceArea
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2005-09-17
% Copyright 2005-2022 INRA - TPV URPOI - BIA IMASTE

%% Input argument processing

% parse axis handle
if isAxisHandle(varargin{1})
    hAx = varargin{1};
    varargin(1) = [];
else
    hAx = gca;
end

% input argument representing cylinders
cyl = varargin{1};
varargin(1) = [];

% process the case of multiple cylinders
if iscell(cyl)
    hCyls = gobjects(length(cyl), 1);
    for i = 1:length(cyl)
        hCyls(i) = drawCylinder(hAx, cyl{i}, varargin{:});
    end
    if nargout > 0
        varargout{1} = hCyls;
    end
    return;
elseif size(cyl, 1) > 1
    hCyls = gobjects(size(cyl, 1), 1);
    for i = 1:size(cyl, 1)
        hCyls(i) = drawCylinder(hAx, cyl(i, :), varargin{:});
    end

    if nargout > 0
        varargout{1} = hCyls;
    end
    return;
end

% default values
N = 32;
closed = true;

% check number of discretization steps
if ~isempty(varargin)
    var = varargin{1};
    if isnumeric(var)
        N = var;
        varargin = varargin(2:end);
    end
end

% check if cylinder must be closed or open
if ~isempty(varargin)
    var = varargin{1};
    if ischar(var)
        if strncmpi(var, 'open', 4)
            closed = false;
            varargin = varargin(2:end);
        elseif strncmpi(var, 'closed', 5)
            closed = true;
            varargin = varargin(2:end);
        end
    end
end

faceColor = 'g';
ind = find(strcmpi(varargin, 'FaceColor'), 1, 'last');
if ~isempty(ind)
    faceColor = varargin{ind+1};
    varargin(ind:ind+1) = [];
end

% extract transparency
alpha = 1;
ind = find(strcmpi(varargin, 'FaceAlpha'), 1, 'last');
if ~isempty(ind)
    alpha = varargin{ind+1};
    varargin(ind:ind+1) = [];
end

% add default drawing options
varargin = [{'FaceColor', faceColor, 'edgeColor', 'none', 'FaceAlpha', alpha} varargin];

%% Computation of mesh coordinates

% extreme points of cylinder
p1 = cyl(1:3);
p2 = cyl(4:6);

% radius of cylinder
r = cyl(7);

% compute orientation angle of cylinder
[theta, phi, rho] = cart2sph2d(p2 - p1);
dphi = linspace(0, 2*pi, N+1);

% generate a cylinder oriented upwards
x = repmat(cos(dphi) * r, [2 1]);
y = repmat(sin(dphi) * r, [2 1]);
z = repmat([0 ; rho], [1 length(dphi)]);

% transform points
trans   = localToGlobal3d(p1, theta, phi, 0);
pts     = transformPoint3d([x(:) y(:) z(:)], trans);

% reshape transformed points
x2 = reshape(pts(:,1), size(x));
y2 = reshape(pts(:,2), size(x));
z2 = reshape(pts(:,3), size(x));

%% Display cylinder mesh

% plot the cylinder as a surface
hCyl(1) = surf(hAx, x2, y2, z2, varargin{:});

% eventually plot the ends of the cylinder
if closed
    hCyl(2)=patch(hAx, x2(1,:)', y2(1,:)', z2(1,:)', faceColor, 'edgeColor', 'none', 'FaceAlpha', alpha);
    hCyl(3)=patch(hAx, x2(2,:)', y2(2,:)', z2(2,:)', faceColor, 'edgeColor', 'none', 'FaceAlpha', alpha);
    gh = hggroup(hAx);
    set(hCyl,'Parent',gh)
    hCyl = gh;
end

% format ouptut
if nargout == 1
    varargout{1} = hCyl;
end


function p = NormL1_primal(x,weights)

p = norm(x.*weights,1);


% Get the size of the long and short cyclic prefixes based on the sample rate
%
% @param sample_rate Sample rate (in Hz)
% @return long_cp_len Long cyclic prefix length (in samples)
% @return short_cp_len Short cyclic prefix length (in samples)
function [long_cp_len, short_cp_len] = get_cyclic_prefix_lengths(sample_rate)
    long_cp_len = round(1/192000 * sample_rate);
    short_cp_len = round(0.0000046875 * sample_rate);
end


function imb=regPutBorde(im,Nx,Ny,method);
% regPutBorde -  Puts a border to an image:
%	method=1 -> consider the image periodical
%	method=2 -> specular
%	method=3 -> repeat the edge pixel
%
%	imb = regPutBorde(im,Nx,Ny,method);
%
% ON - 10/96 (from putborde)
% Oscar Nestares - 5/99 (renamed to regPutBorde)
%

[sy sx]=size(im);
imb=zeros(sy+2*Ny,sx+2*Nx);
imb(1+Ny:sy+Ny,1+Nx:sx+Nx)=im;

if method == 1
	imb(1:Ny,1+Nx:sx+Nx)=im(sy-Ny+1:sy,:);
	imb(sy+Ny+1:sy+2*Ny,1+Nx:sx+Nx)=im(1:Ny,:);
	imb(1+Ny:sy+Ny,1:Nx)=im(:,sx-Nx+1:sx);
	imb(1+Ny:sy+Ny,sx+Nx+1:sx+2*Nx)=im(:,1:Nx);
	imb(1:Ny,1:Nx)=im(sy-Ny+1:sy,sx-Nx+1:sx);
	imb(1:Ny,sx+Nx+1:sx+2*Nx)=im(sy-Ny+1:sy,1:Nx);
	imb(sy+Ny+1:sy+2*Ny,1:Nx)=im(1:Ny,sx-Nx+1:sx);
	imb(sy+Ny+1:sy+2*Ny,sx+Nx+1:sx+2*Nx)=im(1:Ny,1:Nx);
elseif method == 2
	imb(Ny:-1:1,1+Nx:sx+Nx)=im(2:Ny+1,:);
	imb(sy+2*Ny:-1:sy+Ny+1,1+Nx:sx+Nx)=im(sy-Ny:sy-1,:);
	imb(1+Ny:sy+Ny,Nx:-1:1)=im(:,2:Nx+1);
	imb(1+Ny:sy+Ny,sx+2*Nx:-1:sx+Nx+1)=im(:,sx-Nx:sx-1);
	imb(Ny:-1:1,Nx:-1:1)=im(2:Ny+1,2:Nx+1);
	imb(Ny:-1:1,sx+2*Nx:-1:sx+Nx+1)=im(2:Ny+1,sx-Nx:sx-1);
	imb(sy+2*Ny:-1:sy+Ny+1,Nx:-1:1)=im(sy-Ny:sy-1,2:Nx+1);
	imb(sy+2*Ny:-1:sy+Ny+1,sx+2*Nx:-1:sx+Nx+1)=im(sy-Ny:sy-1,sx-Nx:sx-1);
elseif method==3
	for k=1:Nx
		imb(Ny+1:sy+Ny,k)=im(:,1);
		imb(Ny+1:sy+Ny,k+sx+Nx)=im(:,sx);
	end
	for k=1:Ny
		imb(k,Nx+1:sx+Nx)=im(1,:);
		imb(k+sy+Ny,Nx+1:sx+Nx)=im(sy,:);
	end
else
	error('Not a valid value for method')
end


function[h,hl]=wavespecplot(varargin)
%WAVESPECPLOT  Plot of wavelet spectra together with time series.
%
%   WAVESPECPLOT(T,X,P,W) where T is the time, X is a time series, and W is
%   the wavelet transform at periods P, makes a two-component plot.  The
%   upper subplot has the time series X plotted against its time axis T.
%   The lower subplot has the transform W, or its modulus ABS(W) if W is
%   complex-valued, plotted versus the time axis T and the periods P.
%
%   WAVESPECPLOT(T,X,P,W,R) optionally plots ABS(W).^R instead of W.
%
%   WAVESPECPLOT(T,X,P,W,R,CI) makes a filled contour plot with contour
%   intervals CI.  If CI is not input, then the spectrum is plotted using
%   PCOLOR, which is faster to render but slow to print. For making final
%   figures, it is better to use the contouring option.
%
%   WAVESPECPLOT(T,X,P,W1,W2,...WN,...) makes an N+1 component plot, with
%   W1 in the second subplot, W2 in the third, etc.
%
%   After plotting, all subplots are packed together using PACKFIG.
%   H=WAVESPECPLOT(...) returns the handles to the subplots.
%
%   [H,HL]=WAVESPECPLOT(...) also returns the handles to the lines plotted
%   in the first panel.
%
%   If X is complex-valued, both the real and imaginary parts are plotted
%   in the uppermost subplot.
%
%   Usage: h=wavespecplot(t,x,p,w);
%          h=wavespecplot(t,x,p,w,r);
%          [h,hl]=wavespecplot(t,x,p,w,r,ci);
%
%   'wavespecplot --f' generates a sample figure.
%   _________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2004--2015 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(varargin{1},'--f')
   type makefigs_wavespecplot;
   makefigs_wavespecplot
   return
end

t=varargin{1};
u=varargin{2};
p=varargin{3};
t=t(:);
p=p(:);

nspec=nargin-3;

n=1;
ci=[];

if length(varargin{end})==1
  if length(varargin{end-1})==1
     ci=varargin{end};
     n=varargin{end-1};
     nspec=nspec-2;
  else
     n=varargin{end};
     nspec=nspec-1;
  end
else
  if isvector(varargin{end})
     ci=varargin{end};
     n=varargin{end-1};
     nspec=nspec-2;
  end
end

a=min(t);
b=max(t);

if isreal(u)
  c=maxmax(abs(u));
else
  c=max([maxmax(abs(real(u))) maxmax(abs(imag(u)))]);
end
c=c*1.1;

subplot(nspec+1,1,1)
if ~isreal(u)
  [hl1,hl2]=uvplot(t,u);
  hl=[hl1;hl2];
else
  hl=plot(t,u);
end
xlim([a,b])
ylim([-c c])
grid off
%hlines(0,'k:')

for i=1:nspec
  subplot(nspec+1,1,i+1)
  if ~isreal(varargin{3+i}) || n~=1
        varargin{3+i}=abs(varargin{3+i});
  end
  T=(varargin{3+i}').^n;
  if isempty(ci)
    pcolor(t,p,T),shading interp
  else
    contourf(oprod(1+0*p,t),oprod(p,1+0*t),T,ci),hold on
    caxis([min(ci) max(ci)])
  end
  xlim([a,b])
  if p(1)<p(end)
      flipy
  end
  hold on
  set(gca,'tickdir','out')
  ylog
end
h=packfig(nspec+1,1,'rows');


classdef RWMOP7 < PROBLEM
% <multi> <real> <constrained>
% Gear train design problem

%------------------------------- Reference --------------------------------
% A. Kumar, G. Wu, M. Ali, Q. Luo, R. Mallipeddi, P. Suganthan, and S. Das,
% A benchmark-suite of real-world constrained multi-objective optimization
% problems and some baseline results, Swarm and Evolutionary Computation,
% 2021, 67: 100961.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Initialization
        function Setting(obj)
            obj.M        = 2;
            obj.D        = 4;
            obj.lower    = [11.51,11.51,11.51,11.51];
            obj.upper    = [60.49,60.49,60.49,60.49];
            obj.encoding = ones(1,obj.D);
        end
        %% Evaluate multiple solutions
        function Population = Evaluation(obj,varargin)
            x  = varargin{1};
            x1 = x(:,1);
            x2 = x(:,2);
            x3 = x(:,3);
            x4 = x(:,4);
            % Objective function
            f(:,1) = abs(6.931-x3.*x4./(x1.*x2));
            f(:,2) = max(x,[],2);
            % Constraints
            g(:,1) = f(:,1)./6.931-0.5;
            Population = SOLUTION(varargin{1},f,g,varargin{2:end});
            obj.FE     = obj.FE + length(Population);
        end
         %% Generate a point for hypervolume calculation
        function R = GetOptimum(obj,~)
            R = [3.4655000e+00   4.5417795e+01];
        end
    end
end