function [] = threed_to_vu(fname,x,e_conn,u,var)

%*****************************************************************************80
%
%  threed_to_vu.m - Writes out a 3D finite element solution to a format
%                readable by Vu
%                (http://www.invisu.ca/e/index.php)
%
%  Copyright (c) 2002, Jeff Borggaard, Virginia Tech
%  Version: 1.0
%
%  Usage:    [] = threed_tovu(fname,x,e_conn,u,var)
%
%  Variables:     fname
%                        Name of the output file (default="femvu.vu")
%                 x
%                        Nodal coordinates
%                 e_conn
%                        Element connectivity
%                 u
%                        Columns of scalar variables to write to Vu file
%                 var(n_var).names
%                        List of scalar variable names (default="var#")
%-----------------------------------------------------------------------
  [n_nodes,    n_dim      ] = size(x);
  [n_elements, nel_dof    ] = size(e_conn);
  [temp      , n_variables] = size(u);
  var(n_variables+1).name = '';

  if ( isempty(fname) )
    fname = 'femvu.vu';
  end
  fid = fopen(fname,'w');

  % Write out coordinate values
  fprintf(fid,'FIELD x_coord( ) =\n {\n');
  for i=1:n_nodes
    fprintf(fid,' %10.6e %10.6e %10.6e\n',x(i,1),x(i,2),x(i,3));
  end
  fprintf(fid,'};\n\n');

  % Write out element connectivity
  fprintf(fid,'FIELD<int> e_conn( ) =\n{\n');
  if ( nel_dof==4 ) % Test for linear tetrahedra
    for i=1:n_elements
      fprintf(fid,' %6i %6i %6i %6i\n',e_conn(i,:));
    end
  elseif ( nel_dof==10 ) % Test for quadratic tetrahedra
    for i=1:n_elements
      fprintf(fid,' %6i %6i %6i %6i %6i %6i %6i %6i %6i %6i\n',...
              e_conn(i,[1 2 3 4 5 6 7 10 8 9]));
    end
  else
    error('threed_to_vu: element type not currently implemented\n')
    return
  end
  fprintf(fid,'};\n\n');

  % Write out variables
  for n_var=1:n_variables
    if ( isempty(var(n_var).name) )
      var(n_var).name = strcat('Var',int2str(n_var));
    end

    temp = strcat('FIELD@',var(n_var).name,'( ) = {\n');
    temp = strrep(temp,'D@','D ');
    fprintf(fid,temp);
    fprintf(fid,' %10.5e %10.5e %10.5e %10.5e %10.5e %10.5e \n',...
                  u(:,n_var) );
    fprintf(fid,'\n};\n');
  end
  fprintf(fid,'\n\n');

  % Write out zones and connectivity
  if ( nel_dof==4 )
    fprintf(fid,'MESH MyMesh( ) =\n{\n');
    fprintf(fid,'   ZONE Zone1( LagrTetra04, x_coord, e_conn );\n' );
    fprintf(fid,'};\n\n');
  elseif ( nel_dof==10 )
    fprintf(fid,'MESH MyMesh( ) =\n{\n');
    fprintf(fid,'   ZONE Zone1( LagrTetra10, x_coord, e_conn );\n' );
    fprintf(fid,'};\n\n');
  end

  fprintf(fid,'SOLUTION MySolution( ) =\n{\n');
  if ( nel_dof==4 )
    for n_var=1:n_variables
      temp = strcat('   VARIABLE@'   , var(n_var).name,...
                    '( LagrTetra04,@', var(n_var).name,...
                    ', e_conn, Zone1 );\n');
      temp = strrep(temp,'@',' ');
      fprintf(fid,temp);
    end
  else
    for n_var=1:n_variables
      temp = strcat('   VARIABLE@'   , var(n_var).name,...
                    '( LagrTetra10,@', var(n_var).name,...
                    ', e_conn, Zone1 );\n');
      temp = strrep(temp,'@',' ');
      fprintf(fid,temp);
    end
  end
  fprintf(fid,'};\n');

  fclose(fid);


function [type,beats,beatsuct] = beat_class(ecg,qrs,T_LENGTH)
% this function is used to contruct a template ecg based on the location of
% the R-peaks. A series of peaks that match with each other are stacked to
% build a template. This template can then be used for ecg morphological
% analysis or further processing. Note that the qrs location inputed must
% be as precise as possible. This approach for building the template ECG
% seems to be the best of the alternatives that were tested and leaves the
% freedom of having more than one mode (i.e. multiple ECG template can be
% built if there are different cycle morphology such as PVC)  but it is not
% particularly fast.
% The procedure for building the template is:
% 1. create average wrapped template
% 2. identify different modes present
%
% inputs
%   ecg:            the ecg channel(s)
%   qrs:            qrs location [number of samples]
%   T_LENGTH        template length
%
% --
% ECG classification from single-lead segments using Deep Convolutional Neural
% Networks and Feature-Based Approaches - December 2017
%
% Released under the GNU General Public License
%
% Copyright (C) 2017  Fernando Andreotti, Oliver Carr
% University of Oxford, Insitute of Biomedical Engineering, CIBIM Lab - Oxford 2017
% fernando.andreotti@eng.ox.ac.uk
%
%
% For more information visit: https://github.com/fernandoandreotti/cinc-challenge2017
%
% Referencing this work
%
% Andreotti, F., Carr, O., Pimentel, M.A.F., Mahdi, A., & De Vos, M. (2017).
% Comparing Feature Based Classifiers and Convolutional Neural Networks to Detect
% Arrhythmia from Short Segments of ECG. In Computing in Cardiology. Rennes (France).
%
% Last updated : December 2017
%
% 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/>.

% == manage inputs
if nargin<2; error('ecg_template_build: wrong number of input arguments \n'); end;

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

% == constants
% qrs(1) = []; qrs(end) = [];                                % remove extremity peaks
extremities = (qrs <= 2*T_LENGTH | qrs >= length(ecg)-2*T_LENGTH);        % test if there are peaks on the border that may lead to error
qrs = round(qrs(~extremities)); % remove extremity peaks

%% Phase wrap to get all within 2*pi
NB_BINS = 250; % number of bins for wrapping
[M,RES] = stackbeats(ecg,qrs,T_LENGTH,NB_BINS);

% beatsuct = median(M')';
% dem = round((length(beatsuct)-2*T_LENGTH-1)/2);
% beats{1} =  beatsuct(dem+1:end-dem);
% type = ones(length(qrs),1);

%% Clustering
Nclus1 = 5;
Nclus2 = 2;
M = bsxfun(@times,M,hamming(size(M,1)));  % do it twice, it's the
M = bsxfun(@times,M,hamming(size(M,1)));  % magic sauce
M = bsxfun(@times,M,hamming(size(M,1)));  % magic sauce
M = M';
% [~, clusters, ~] = fkmeans(M',Nclus);
rng(1);
[~,clusters,~] = kmeans(M,Nclus1); % may need to provide centroid..
[~,clusters,~] = kmeans(clusters,Nclus2); % may need to provide centroid..
clusters = clusters';

% !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
% TODO: allow more than two clusters!!!!!!!!!!!!!!!!!!!!
% !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

corel = corrcoef(clusters(50:200,1),clusters(50:200,2));

%% In case beats are correlated to each other
if corel(1,2) > 0.7
    type = ones(size(qrs));
    beats{1} = resample(mean([clusters(:,1), clusters(:,2)],2),RES,NB_BINS);
    dem = (length(beats{1})-2*T_LENGTH-1)/2;
    beatsuct = beats;
    beats = cellfun(@(x) x(dem+1:end-dem),beats,'UniformOutput',0);
    return;
end

%% In case one cluster has low amplitude

idx = var(clusters)<0.1*var(ecg); % remove clusters with variance less than 10% of signal
if any(idx)
   type = ones(size(qrs));
   beats{1} = resample(clusters(:,~idx),RES,NB_BINS);
   dem = round((length(beats{1})-2*T_LENGTH-1)/2);
   beatsuct = beats;
   beats = cellfun(@(x) x(dem+1:end-dem),beats,'UniformOutput',0);
   return;
end

%% Classifying beats

beats{1} = resample(clusters(:,1),RES,NB_BINS);
beats{2} = resample(clusters(:,2),RES,NB_BINS);
beatsuct = beats;
dem = (length(beats{1})-2*T_LENGTH-1)/2;
beats = cellfun(@(x) x(dem+1:end-dem),beats,'UniformOutput',0);

for i = 1:length(qrs)
    cor = corrcoef(ecg(qrs(i)-T_LENGTH:qrs(i)+T_LENGTH),beats{1});
    corel(1,i) = cor(1,2);
    cor = corrcoef(ecg(qrs(i)-T_LENGTH:qrs(i)+T_LENGTH),beats{2});
    corel(2,i) = cor(1,2);
end
[~,type]=max(corel);

if sum(type==2)>sum(type==1)
    type(type==2) = 3;
    type(type==1) = 2;
    type(type==3) = 2;
end

if all(type==2)
    type = ones(size(type));
end
type = type';

end


function glm = glmCreate(tr,nh,whiten)
%Create a GLM model for managing the GLM parameters
%
%   glm = glmCreate(tr,nh,whiten)
%
%

% It would be better to go and get the TR from the SESSION data.
if notDefined('tr'),     error('You must specify tr for the GLM'); end
if notDefined('nh'),     nh = 22; end
if notDefined('whiten'), whiten = 0; end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initialize GLM model %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
glm.betas    = [];
glm.residual = [];
glm.stdevs   = [];
glm.sems     = [];
glm.nh       = nh;       % Number of time samples
glm.tr       = tr;       %
glm.C        = [];       % Covariance matrix
glm.voxDepNoise    = [];
glm.voxIndepNoise  = [];
glm.designMatrix   = [];
glm.dof            = []; % Degrees of freedom
glm.whiten         = whiten;  % Do not whiten the covariance

% glmSet() ...
if nh>1, glm.type = 'selective averaging';
else     glm.type = 'applied HRF';
end

return;


function [out] = random(msg,BSID,DIUC,Frame);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%                                                                       %
%%     Name: random.m                                                    %
%%                                                                       %
%%     Description: This function is realisez uniquely by the way        %
%%      defined in the standard. It is one more stage in the encoding.   %
%%                                                                       %
%%                                                                       %
%%     Parameters:                                                       %
%%       msg  --> Sequence of bits to encode with the algorithm BSID,    %
%%       DIUC, Frame--> Identifiers of the connection that is carried    %
%%       out.(Base Station, Downlink and the number of frames)           %
%%                                                                       %
%%                                                                       %
%%     Result: It gives back the chain of bits encoded accordin to       %
%%      the need (following DIUC, BSID and Frame). The encoding and      %
%%      decoding are an identical process.                               %
%%                                                                       %
%%                                                                       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

seed = zeros (1,15);
seed=[de2bi(BSID,4,'left-msb') 1 1 de2bi(DIUC,4,'left-msb') 1 de2bi(Frame,4,'left-msb')];

out=zeros(1,length(msg));

for i=1:length(msg)
    next = xor(seed(14),seed(15));
    out(i) = xor( msg(i),next);
    seed = [next seed(1,1:14)];
end

return;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   FANUC MATE M710iC/50, FANUC Robotics Europe.
%
%   Authors: Carlos Carrazoni Perez, Juan Jose Perez Hernandez & David
%   Martinez Pascual
%   Universidad Miguel Hernandez de Elche.
%   date:   7/1/2019
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2019, by  Carlos Carrazoni Perez, Juan Jose Perez Hernandez
% & David Martinez Pascual
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function robot = parameters()

robot.name= 'Fanuc_M_710iC_50';

%Path where everything is stored for this robot
robot.path = 'robots/fanuc/M_710iC_50';
%Tabla de D-H
robot.DH.theta= '[q(1)+pi/2  q(2)+pi/2  q(3)  q(4)  q(5)  q(6)]';
robot.DH.d='[0.565 0 0 1.016 0 0.175]';
robot.DH.a='[0.15 0.87 0.17 0 0 0]';
robot.DH.alpha= '[pi/2  0  pi/2  pi/2  -pi/2  0]';
robot.J=[];

robot.inversekinematic_fn = 'inversekinematic_fanuc_m710(robot, T)';

%number of degrees of freedom
robot.DOF = 6;

%rotational: 0, translational: 1
robot.kind=['R' 'R' 'R' 'R' 'R' 'R'];

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-180) deg2rad(180); %Axis 1, minimum, maximum
                deg2rad(-135) deg2rad(90); %Axis 2, minimum, maximum
                deg2rad(-160) deg2rad(280); %Axis 3
                deg2rad(-360) deg2rad(360); %Axis 4
                deg2rad(-125) deg2rad(125); %Axis 5
                deg2rad(-360) deg2rad(360)]; %Axis 6

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(175); %Axis 1, rad/s
                deg2rad(175); %Axis 2, rad/s
                deg2rad(175); %Axis 3, rad/s
                deg2rad(250); %Axis 4, rad/s
                deg2rad(250); %Axis 5, rad/s
                deg2rad(355)];%Axis 6, rad/s
% end effectors maximum velocity
robot.linear_velmax = 1.0; %m/s, not specified
robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time

%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);
robot.tool_activated=1;

% GRAPHICS
robot.graphical.has_graphics=1;
robot.graphical.color = [0.9 0.9 0]%./255;
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=1;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot
robot.axis=[-1.5 1.5 -1.5 1.5 0 2];
%read graphics files
robot = read_graphics(robot);

%robot.tool=load_robot('equipment/end_tools','water_cutter');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DYNAMIC PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
robot.has_dynamics=1;

%consider friction in the computations
robot.dynamics.friction=0;

%link masses (kg)
robot.dynamics.masses=[191.865 71.802 111.5 27.55 5.705 0.674];

%COM of each link with respect to own reference system
robot.dynamics.r_com=[-0.109  -0.099  0.053; %(rx, ry, rz) link 1
                      -0.466  -0.001 -0.287; %(rx, ry, rz) link 2
                      -0.086   0.032  0.007;  %(rx, ry, rz) link 3
                       0       0.342  0.011;  %(rx, ry, rz) link 4
                       0      -0.015  0.076 ; %(rx, ry, rz) link 5
                       0       0      0.010];%(rx, ry, rz) link 6

%Inertia matrices of each link with respect to its D-H reference system.
% Ixx	Iyy	Izz	Ixy	Iyz	Ixz, for each row
robot.dynamics.Inertia=[8.411      9.434	9.499   -2.810	0.691	1.396;
                        6.297     28.162	22.396  -0.049	-0.012	-9.398;
                        2.313      3.367	2.986	0.467	0.232	-0.285;
                        4.863	0.072	4.846	-0.002	0	0;
                        0.065	0.058	0.022	0	0.004   0;
                        0.000768	0.000767	0.001368	0	0	0];

%Speed reductor at each joint
robot.motors.G=[10 20 120 200 300 1];


% Gtomo_nufft_test.m
% Test the Gtomo_nufft object

% create Gtomo_nufft class object
if ~isvar('An'), printm 'setup'
	ig = image_geom('nx', 64, 'ny', 60, 'fov', 480);
	ig.mask = ig.circ > 0;
	if 1
		sg = sino_geom('par', 'nb', 70, 'na', 80, 'dr', 7, ...
			'orbit_start', -15, 'strip_width', 'd');
		fan_arg = {}; % parallel beam
	else
		sg = sino_geom('ge1', 'orbit_start', -15, 'down', 8, ...
			'strip_width', 'd');
		fan_arg = { % fan beam
			'offset_s', sg.offset_s, ...
			'dis_src_det', sg.dsd, ...
			'dis_iso_det', sg.dod, ...
		};
	end

	% test non-rect image basis function and beam shape (S. Matej)
	if 0
		basis.type  = 'KB';
		basis.diam  = 4;
		basis.shape = 10.4;
		basis.m     = 2;
		basis.dim   = 2;
		basis.kernel=[];

		% KB: FWHM~1.0 : J=6, alpha=40, m=2
		beam.type  = 'KB';
		beam.diam  = 6;
		beam.shape = 40.;
		beam.m     = 2;
	end

	cpu etic

%{
	arg_geom = { % for old version
		fan_arg{:}, ...
		'orbit', sg.orbit, ...
		'orbit_start', sg.orbit_start, ...
		'dx', ig.dx, ...
		'ds', sg.d, ...
		'yscale', -ig.dy/ig.dx, ...
		'strip_width', sg.strip_width, ...
	};
%}

	arg = {
		'interp', {'table', 2^11, 'minmax:kb'}, ...
...%		'basis', basis, ...
...%		'beam', beam, ...
		'is.complex', 0, ...
...%		'kaiser', ...	% use KB interpolator
...%		'uniform', ...
...%		'bilin', ...
...%		'is.test', 1, ...
		'class', 'fatrix2', ...
	};

%	Ao = Gtomo_nufft(ig.mask, [sg.nb sg.na], arg_geom{:}, arg{:});
	An = Gtomo_nufft_new(sg, ig, arg{:});
%	jf_equal(struct(Ao), struct(An)), return

	cpu etoc 'An pre time:'
end

%
% make test image
%
if 0
%	x = ig.zeros;
%	x(12, 8) = 1;
%	x((nx/4+1):(3*nx/4),(ny/4+1):(3*ny/4)) = 1;
	x = ig.unitv(ig.nx/4,ig.ny/3);	% point source
elseif 1
	x = ellipse_im(ig, [0 0 [1 1]*ig.ny*ig.dx*0.9/2 0 1], 'oversample', 2);
	x(end/4:end/2,end/4:end/2) = 1.25;
else
	ix = [-(nx-1)/2:(nx-1)/2]/nx;
	iy = [-(ny-1)/2:(ny-1)/2]/ny;
	[ix iy] = ndgrid(ix, iy);
	x = exp(-(ix.^2 + iy.^2) / 10^2);
end

% check real / imaginary
if 1
	y = An * x(ig.mask);
	y = sg.shape(y);
	im plc 3 3
	im(1, x, 'x')
	im(2, real(y), 'real(An*x)'), cbar
	im(3, imag(y), 'imag(An*x)'), cbar
prompt
end

% DSFT version for comparison
if ~isvar('Ad'), printm 'Ad'
%	Ao = Gtomo_nufft(ig.mask, [sg.nb sg.na], arg_geom{:}, arg{:}, ...
%		'is.dsft2', 1, 'is.dsft1', 1);
	Ad = Gtomo_nufft_new(sg, ig, arg{:}, 'is.dsft2', 1, 'is.dsft1', 1);
%	jf_equal(struct(Ao), struct(Ad)), return
end

% create a "strip integral" system object
if ~isvar('As'), printm 'As'
%	Ao = aspire_pair(sg, ig, 'strip_width', An.arg.strip_width, ...
%		'support', 'all');
%		'support', ig.mask);
%	Ao = Gtomo2_dscmex(Ao, 'nthread', 2); % multiprocessor!
	As = Gtomo2_dscmex(sg, ig, 'nthread', 2); % multiprocessor!
%	jf_equal(struct(Ao), struct(As)), return
end

% compare back projectors
if 0
	y = sg.ones;
	y = sg.unitv(sg.nb/2+7, 9);
%	y = yd / 100;
	xn = An' * y;
	xd = Ad' * y;
	xs = As' * y;
	im(4, xn, 'An''*y'), cbar
	im(5, xd, 'Ad''*y'), cbar
	im(6, xs, 'As''*y'), cbar
	im(8, xn-xd, 'An''*y - Ad''*y'), cbar
	im(9, xn-xs, 'An''*y - As''*y'), cbar
%	printf('back nrms = %g%%', nrms(x2(:), x1(:)) * 100)
prompt
end

% compare projectors
if 1
	yn = An * x;
	yd = Ad * x;
	ys = As * x;
	im(4, yd, 'Ad*x'), cbar
	im(5, yn-yd, 'An*x-Ad*x'), cbar
	im(7, ys, 'As*x'), cbar
	im(8, yn-ys, 'An*x-As*x'), cbar
%	printf('forward nrms = %g%%', nrms(y2(:), y1(:)) * 100)
prompt
end

if 1, printm 'single ray'
	y = sg.unitv(sg.nb/2+9,1);
	im pl 2 3
	im(1, As'*y, 'back ray strip'), cbar h
	im(2, An'*y, 'back ray nufft'), cbar h
	im(3, Ad'*y, 'back ray dsft'), cbar h
	im(4, An'*y - Ad'*y, 'nufft - dsft'), cbar h
	im(5, An'*y - As'*y, 'nufft - strip'), cbar h
	im(6, Ad'*y - As'*y, 'dsft - strip'), cbar h
return
end


% File Name: resampling.m
% Author: Heung-Il Suk
% Cite: H.-I. Suk and S.-W. Lee, "A Novel Bayesian Framework for Discriminative
% Feature Extraction in Brain-Computer Interfaces," IEEE Trans. on PAMI,
% 2012. (Accepted)

function newBSSFO = resamplingBSSFO( oldBSSFO )

accWeight = cumsum( oldBSSFO.weight );

for i=1:oldBSSFO.numBands
    oldBSSFO.selected( i ) = 0;
end

newBSSFO = oldBSSFO;
for i=1:oldBSSFO.numBands
    idx = find( accWeight > rand );
    newBSSFO.sample(:, i) = oldBSSFO.sample( :, idx(1) );
    if oldBSSFO.selected( idx(1) ) == 0
        oldBSSFO.selected( idx(1) ) = 1;
        newBSSFO.selected( i ) = 1;   % keep current state, do not add random noise
    end
end

newBSSFO.weight = ones(newBSSFO.numBands, 1)./ newBSSFO.numBands;


function plotActivityTrace( inferred, filtered, raw, plotControl)

% This code plots the activity trace for all the ROIs
% Author: Weijian Yang

% input parameters:
% inferred, filtered, raw (from the output of "signalExtraction.m")
% plotControl.frameRate: frame rate in fps
% plotControl.normalization: normalize the amplitude of the temporal trace for plotting purpose
% plotControl.sep: vertical (amplitude) offset between adjacent temporal traces
% plotControl.displayLabel: whether to display ROI ID for the temporal traces
% plotControl.plotInferred: whether the inferred signal is plotted
% plotControl.plotFiltered: whether the filtered signal is plotted
% plotControl.plotRaw: whether the raw signal is plotted
% plotControl.rollingView: whether the display is set such that only 20 temporal traces are displayed at a time, followed by any key to move onto the next 20 traces, and etc.

if isfield(plotControl,'frameRate') frameRate=plotControl.frameRate;
else frameRate=1; end

if isfield(plotControl,'normalization') normalization=plotControl.normalization;
else normalization=1; end

if isfield(plotControl,'sep') sep=plotControl.sep;
else sep=1.2; end

if isfield(plotControl,'displayLabel') displayLabel=plotControl.displayLabel;
else displayLabel=1; end

if isfield(plotControl,'plotInferred') plotInferred=plotControl.plotInferred;
else plotInferred=1; end

if isfield(plotControl,'plotFiltered') plotFiltered=plotControl.plotFiltered;
else plotFiltered=1; end

if isfield(plotControl,'plotRaw') plotRaw=plotControl.plotRaw;
else plotRaw=0; end

if isfield(plotControl,'rollingView') rollingView=plotControl.rollingView;
else rollingView=0; end

t=1/frameRate*(1:size(inferred,2));
K=size(inferred,1);

figure;
hold on;

if normalization==1
    for idx=1:K
        if plotRaw  plot(t,raw(idx,:)/max(raw(idx,:))-idx*sep,'color',[0.8 0.8 0.8],'linewidth',2); end
        if plotFiltered  plot(t,filtered(idx,:)/max(filtered(idx,:))-idx*sep,'color',[1 0.7 0.7],'linewidth',1.5); end
        if plotInferred  plot(t,inferred(idx,:)/max(inferred(idx,:))-idx*sep,'color',[0 0 1],'linewidth',1); end
    end
    ylabel('Normalized \DeltaF/F');
else
    for idx=1:K
        if plotRaw  plot(t,raw(idx,:)-idx*sep,'color',[0.8 0.8 0.8],'linewidth',2); end
        if plotFiltered  plot(t,filtered(idx,:)-idx*sep,'color',[1 0.7 0.7],'linewidth',1.5); end
        if plotInferred  plot(t,inferred(idx,:)-idx*sep,'color',[0 0 1],'linewidth',1); end
    end
    ylabel('\DeltaF/F');
end
xlabel('Time (s)');

if displayLabel
    for idx=1:K
        text(max(t)*1.02,-idx*sep, num2str(idx));
    end
end

h=gca;
if ~rollingView
    axis(h,[0 max(t) -sep*(K+1) sep]);
else
    idx=1;
    displayNum=20;
    while idx+displayNum<K
        axis(h,[0 max(t) -sep*(idx+displayNum) sep*(-idx+1)]);
        idx=idx+displayNum;
        input('Press ENTER to continue...');
    end
    axis(h,[0 max(t) -sep*(idx+displayNum) sep*(-idx+1)]);
    input('Press ENTER to continue...');
    axis(h,[0 max(t) -sep*(K+1) sep]);
end


function D = spm_eeg_avgfreq(S)
% Average a TF-dataset over frequency to get a time-domain dataset
% FORMAT D = spm_eeg_avgfreq(S)
%
% S        - input struct
%  fields of S:
%   D        - MEEG object or filename of M/EEG mat-file with epoched data
%   freqwin  - frequency window to average over [default: [-Inf, Inf]]
%   prefix   - prefix for the output file [default: 'P']
%
% Output:
% D        - MEEG object
%__________________________________________________________________________
% Copyright (C) 2012-2017 Wellcome Trust Centre for Neuroimaging

% Vladimir Litvak
% $Id: spm_eeg_avgfreq.m 7132 2017-07-10 16:22:58Z guillaume $

SVNrev = '$Rev: 7132 $';

%-Startup
%--------------------------------------------------------------------------
spm('FnBanner', mfilename, SVNrev);
spm('FigName','Average over frequency'); spm('Pointer','Watch');

if ~isfield(S, 'prefix'),       S.prefix   = 'P';           end
if ~isfield(S, 'freqwin'),      S.freqwin  = [-Inf Inf];    end

D = spm_eeg_load(S.D);

if ~strncmpi(D.transformtype,'TF',2)
    error('This function only works on TF datasets.');
end

freqind = D.indfrequency(min(S.freqwin)):D.indfrequency(max(S.freqwin));
if isempty(freqind) || any(isnan(freqind))
    error('Selected frequency window is invalid.');
end

%-Generate new MEEG object with new files
%--------------------------------------------------------------------------
Dnew = clone(D, [S.prefix fname(D)], [D.nchannels D.nsamples D.ntrials]);

%-Averaged data
%--------------------------------------------------------------------------
spm_progress_bar('Init', D.ntrials, 'Trials processed');
if D.ntrials > 100, Ibar = floor(linspace(1, D.ntrials, 100));
else Ibar = 1:D.ntrials; end

for i = 1:D.ntrials

    Dnew(:, :, i) = spm_squeeze(mean(D(:, freqind, :, i), 2), 2);

    if any(Ibar == i), spm_progress_bar('Set', i); end
end

spm_progress_bar('Clear');

%-Save the new M/EEG dataset
%--------------------------------------------------------------------------
Dnew = Dnew.history(mfilename, S);
save(Dnew);

D = Dnew;

%-Cleanup
%--------------------------------------------------------------------------
fprintf('%-40s: %30s\n','Completed',spm('time'));                       %-#
spm('FigName','Average over frequency: done'); spm('Pointer','Arrow');


classdef ExistProbCalculatorX < BaseX
% ExistProbCalculatorX class
%
% Summary of ExistProbCalculatorX:
% This class can be used to compute (recursively) the existence probability
% of tracks.
%
% ExistProbCalculatorX Properties:
%   TrackList
%   AssocWeightsMatrix
%   ProbOfDetection
%   ProbOfDeath
%   ProbOfGating
%
% ExistProbCalculatorX Methods:
%   + ExistProbCalculatorX - Constructor method
%   + calculate - Calculate the existence probabilities
%
% (+) denotes public properties/methods
%
% March 2018 Lyudmil Vladimirov, University of Liverpool.

    properties
       TrackList
       AssocWeightsMatrix
       ProbOfDetection
       ProbOfDeath
       ProbOfGating
    end

    methods
        function this = ExistProbCalculatorX(varargin)
        % EXISTPROBCALCULATORX Constructor method
        %
        % Parameters
        % ----------
        % ProbOfDetection: scalar
        %   A scalar value in the range [0,1] which specifies the probability
        %   of detection for all targets
        % ProbOfDeath: scalar
        %   A scalar value in the range [0,1] which specifies the probability
        %   of death for all targets
        % ProbOfGating: scalar
        %   A scalar value in the range [0,1] which specifies the probability
        %   of gating for all targets
        %
        % Usage
        % -----
        % * ExistProbCalculatorX() returns a ExistProbCalculatorX object handle.
        % * ExistProbCalculatorX(ProbOfDetection,ProbOfDeath,ProbOfGating) returns
        %   a ExistProbCalculatorX( object handle configured with the provided
        %   probability of detection ProbOfDetection, probability of death ProbOfDeath
        %   and probability of gating ProbOfGating. This is particularly
        %   useful when all targets are characterised by the same parameters.
        % * ExistProbCalculatorX(config) can also be used, where config is a
        %   structure with fields config.ProbOfDetection, config.ProbOfDeath and
        %   config.ProbOfGating.
        % * PhdTrackInitiatorX(___,Name,Value) instantiates an object
        %   handle, configured with additional options specified by one or
        %   more Name,Value pair arguments.

            if(nargin==0)
                return;
            end

            % First check to see if a structure was received
            if(nargin==1)
                if(isstruct(varargin{1}))
                    config = varargin{1};
                    this.ProbOfDetection = config.ProbOfDetection;
                    this.ProbOfDeath = config.ProbOfDeath;
                    this.ProbOfGating = config.ProbOfGating;
                end
                return;
            elseif(nargin==3)
                this.ProbOfDetection = varargin{1};
                this.ProbOfDeath = varargin{2};
                this.ProbOfGating = varargin{3};
                return;
            end

            % Otherwise, fall back to input parser
            parser = inputParser;
            parser.KeepUnmatched = true;
            parser.parse(varargin{:});
            config = parser.Unmatched;
            this.ProbOfDetection = config.ProbOfDetection;
            this.ProbOfDeath = config.ProbOfDeath;
            this.ProbOfGating = config.ProbOfGating;
        end

        function argout = calculate(this,varargin)
        % CALCULATE Calculates the probability(ies) of existence
        %
        % Parameters
        % ----------
        % varargin{1}: (1 x NumTracks) cell array or TrackX object
        %   Either, a cell array of TrackX objects or a single TrackX
        %   object.
        % varargin{2}: (NumTracks x Nm+1) or (1 x Nm+1) matrix
        %   In the case that varargin{1} is a cell array, varargin{2}
        %   should be a (NumTracks x Nm+1) matrix, else it should be a
        %   (1 x Nm+1) row vector, where in both cases, each column contains
        %   the association weight between a measurement and a track. The
        %   first column corresponds to the dummy measurement.
        %
        % Returns
        % -------
        % argout: (1 x NumTracks) cell array or TrackX object
        %   Either, a cell array of TrackX objects or a single TrackX
        %   object, with their/its ProbOfExist field updated
        %
        % Usage
        % -----
        % * TrackList = calculate(this,TrackList,AssocWeightsMatrix)
        %   computes the new probability of existence for each TrackX
        %   object in TrackList and returns the updated TrackList
        % * Track = calculate(this,Track,AssocWeightsMatrix)
        %   computes the new probability of existence for the TrackX
        %   object Track and returns the updated Track
            if(isempty(varargin{1}))
                argout = varargin{1};
                return
            end
            if(iscell(varargin{1}))
                % Compute Existence Probabilities
                this.TrackList = varargin{1};
                this.AssocWeightsMatrix = varargin{2};
                numTracks = numel(this.TrackList);
                for trackInd = 1:numTracks
                    this.TrackList{trackInd}.ProbOfExist = (1 - this.ProbOfDeath)*this.TrackList{trackInd}.ProbOfExist;
                    denom = sum(this.AssocWeightsMatrix(trackInd,:))*this.TrackList{trackInd}.ProbOfExist + this.AssocWeightsMatrix(trackInd,1)*(1-this.TrackList{trackInd}.ProbOfExist)/((1-this.ProbOfDetection*this.ProbOfGating));
                    this.TrackList{trackInd}.ProbOfExist = (sum(this.AssocWeightsMatrix(trackInd,:))*this.TrackList{trackInd}.ProbOfExist)/denom;
                end
                argout = this.TrackList;
            elseif(isa(varargin{1},'TrackX'))
                Track = varargin{1};
                AssocWeights = varargin{2};
                Track.ProbOfExist = (1 - this.ProbOfDeath)*Track.ProbOfExist;
                denom = sum(AssocWeights)*Track.ProbOfExist + AssocWeights(1)*(1-Track.ProbOfExist)/((1-this.ProbOfDetection*this.ProbOfGating));
                Track.ProbOfExist = (sum(AssocWeights)*Track.ProbOfExist)/denom;
                argout = Track;
            end
        end
    end
end


function x = triu( x, k )

%   Disciplined convex/geometric programming information for TRIU:
%       TRIU imposes no convexity restrictions on its arguments.

%
% Check inputs
%

if nargin < 2, k = 0; end
s = x.size_;
if length( s ) > 2,
    error( 'The first argument must be 2-D.' );
elseif ~isnumeric( k ) || length( k ) ~= 1,
    error( 'The second argument must be an integer scalar.' );
end

%
% Zero out the elements outside of the desired triangle
%

b = x.basis_;
b( :, ~triu(ones(s),k) ) = 0;
x = cvx( s, b );

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


function [imageCha, ssim_map, metrics] = algo_SB_proxA_2D_real( obj,input )
% 2D variant of SB
% based on Goldstein and Oshers paper on Split Bregman
%
% input:
% obj           CS reconstruction object (holding all parameters)
% input         struct containing recon parameters and image
%
% output:
% imageCha      reconstructed channel individual image
% ssim_map      structural similarity map
% metrics       evaluation metrics
%
% (c) Marc Fischer, Thomas Kuestner, May 2015
% -------------------------------------------------------------------------

%%
timer_proxA = tic;

%% variables:
% internal flags
flag_wavetree = true;
flag_fast = true;
flag_extendImage = true;

% internal variables:
L = 1;
t_old = 1;
NLTV_struct.kernelratio = 3;
NLTV_struct.windowratio = 6;
NLTV_struct.nThreads = 1;  % mind this option if used with -singleCompThread on BWCluster
itrNLTV = obj.maxitr - 20;
% chambolle tv:
parsin.MAXITER=100; parsin.tv='iso'; % 'iso' or 'l1'

% from obj:
% maxitr = obj.maxitr;
maxitr = obj.iNINNER;
% maxitrOUTER = obj.maxitrOUTER;
maxitrOUTER = obj.iNOUTER;
maxitrINNER = ceil( maxitr / maxitrOUTER );
% n1 = obj.measPara.dim(1);
% n2 = obj.measPara.dim(2);
n1 = input.n1;
n2 = input.n2;
% nSlices = obj.measPara.dim(3);
nCha = obj.measPara.dim(5);
lambdaWave = obj.lambda;
lambdaTV = obj.lambdaTV;
lambdaGroup = obj.lambdaGroup;
NLTV_struct.filterstrength = obj.lambdaNLTV_h; % 0.03 % converted from NLTV h = 0.01 %old: used: 3e-10
lambdaNLTV = obj.lambdaNLTV;
lambdaNLTV_h = obj.lambdaNLTV_h;
mue = obj.mue;
regularizerWeights = obj.regularizerWeights;
flagTV = obj.flagTV;
flagTV_iso = obj.flagTV_iso;
flagWave = obj.flagWave;
flagGroup = obj.flagGroup;
flagNLTV = obj.flagNLTV;
flagSBNLTV = obj.flagSBNLTV;
waveletStages = obj.trafo.waveletStages;
waveletFilterName_l1 = obj.trafo.waveletFilterName_l1;
waveletFilterName_l12 = obj.trafo.waveletFilterName_l12;

% from input:
b=input.b;
mask = input.mask;
G_prox = input.G_prox;
Gt_prox = input.Gt_prox;
groupnorm_index = input.groupnorm_index;
waveS_l1 = input.waveS_l1;
waveS_l12 = input.waveS_l12;
waveS_l12proxA = input.waveS_l12proxA;
proxA_extend_y = waveS_l12proxA(waveletStages+2,1) - waveS_l12(waveletStages+2,1);
proxA_extend_x = waveS_l12proxA(waveletStages+2,2) - waveS_l12(waveletStages+2,2);
x_proxA = cell(1,nCha);

% im_ref = input.im_ref;
% im_ref_full = zeros(n1,n2);
% for j = 1:nCha
%     im_ref_full = im_ref_full + abs(im_ref{1,j}).^2;
% end;
% im_ref_full = sqrt(im_ref_full);
clear input

% initialize cells/vectors
for j=1:nCha
    FTb{1,j} = real(iFFT2D(b{1,j}));
    % y{1,j} = real(FTb{1,j});
    % y{1,j+nCha} = imag(FTb{1,j});
end;
% starting point:
x = FTb; % (x0 = FTb)
rhs = FTb; % different start (x0 = F^-1 K^-1 F FTb); %used atm
b_outer = b;
% rhs: right hand side

x_wave =  cell(1,nCha);
x_g_proxA = x_wave;
x_g_helper = cell(2,nCha);
x_wave_helper = rhs;
v_wave = rhs;
d_wave = rhs;
x_tv = rhs;
v_tv = rhs;
d_tv = rhs;
x_group = rhs;
v_group = rhs;
d_group = rhs;
x_nltv = rhs;
v_nltv = rhs;
d_nltv = rhs;
K_inv = rhs;
for j = 1:nCha
    x_wave{1,j} = 0;
    v_wave{1,j} = 0;
    d_wave{1,j} = 0;
    x_tv{1,j} = zeros(n1,n2);
    v_tv{1,j} = 0;
    d_tv{1,j} = 0;
    x_group{1,j} = 0;
    v_group{1,j} = 0;
    d_group{1,j} = 0;
    x_nltv{1,j} = 0;
    v_nltv{1,j} = 0;
    d_nltv{1,j} = 0;
end;

%% MAD dependent lambdas:
flag_MAD = true;
if flag_MAD
    x_wavedec = cell(1,nCha);
    threshold = zeros(1:nCha);
    for j=1:nCha % 2*nCha
        x_wavedec{1,j} = wavedec2(x{1,j},waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
        x_wave_fine_scale = size(x_wavedec{1,j},2) - (3*waveS_l1(waveletStages+1,1)*waveS_l1(waveletStages+1,2));
        threshold(j) = mad(x_wavedec{1,j}(x_wave_fine_scale:end),1);
    end;
    clear x_wavedec
else
    threshold(1:nCha) = 1;
end;

threshold_wave(j) = lambdaWave * threshold(j) * 2/L;
threshold_TV(j) = lambdaTV * threshold(j) * 2/L;
threshold_group(j) = lambdaGroup * threshold(j) * 2/L;
threshold_NLTV(j) = lambdaNLTV; % * threshold(j) * 2/L;
threshold_NLTV_h(j) = lambdaNLTV_h; %*threshold(j); % adjust carefully or NLTV won't find a solution. lambdaNLTV_h should be < 0.01

%% initialize metrics:
itr = 0;
metrics.xtime(itr+1)= 0;
% [metrics, ssim_map{1,1}] = get_metrics_itr( im_ref, im_ref_full, x, itr, maxitr, nCha, n1, n2, metrics, obj.K_1, obj.K_2, obj.W_size, obj.W_sigma );
ssim_map = [];

%% recon
% inverse for x - step:
for j = 1:nCha
    K_inv{1,j} = 1./(mue*mask + (flagWave*regularizerWeights(1) + flagTV*regularizerWeights(2) +  flagGroup*regularizerWeights(3))*ones(n1,n2)); % flagNLTV changes K_inv -> in separate
end;

dispProgress('Proximal Average', 0, maxitrOUTER*maxitrINNER);
itr = 0;
for itrOUTER = 1:maxitrOUTER  % total iter counter
    for itrINNER = 1:maxitrINNER

        %% x - step
        if itr > 1 % for i = 1 already set (x{1,j} = FTb{1,j})
            for j = 1:nCha
                % rhs part:
                if flagWave
                    x_wave{1,j} = waverec2((v_wave{1,j} - d_wave{1,j}),waveS_l1,waveletFilterName_l1); end;
                if flagTV
                    x_tv{1,j} = v_tv{1,j} - d_tv{1,j}; end;
                if flagGroup
                    x_g_proxA{1,j} = waverec2((v_group{1,j} - d_group{1,j}),waveS_l12proxA,waveletFilterName_l12);
                    x_group{1,j} =  x_g_proxA{1,j}(1:end-proxA_extend_y,1:end-proxA_extend_x);
                end;
                if flagNLTV
                    if itr == itrNLTV
                        K_inv{1,j} = 1./(mue*mask + (flagWave*regularizerWeights(1) + flagTV*regularizerWeights(2) +  flagGroup*regularizerWeights(3) + flagNLTV*regularizerWeights(4))*ones(n1,n2));
                        x_nltv{1,j} = x{1,j};
                    elseif itr >= itrNLTV
                        x_nltv{1,j} = v_nltv{1,j} - d_nltv{1,j};
                    end;
                end;
                rhs{1,j} = mue*FTb{1,j} + ( flagWave*regularizerWeights(1).*(x_wave{1,j}) + flagTV*regularizerWeights(2).*(x_tv{1,j}) + flagGroup*regularizerWeights(3).*(x_group{1,j}) + flagNLTV*regularizerWeights(4).*(x_nltv{1,j}));
                x{1,j} = real(iFFT2D(K_inv{1,j}.*FFT2D(rhs{1,j})));
            end;
        end;

        %% auxiliary variables:
        %% l1-Wavelet
        if flagWave
            for j = 1:nCha
                x_wave_helper{1,j} = wavedec2(x{1,j},waveletStages,waveletFilterName_l1);
                v_wave{1,j} = softthresh_real((x_wave_helper{1,j} + d_wave{1,j}),threshold_wave(j));
                d_wave{1,j} = d_wave{1,j} + x_wave_helper{1,j} - v_wave{1,j};
            end;
        end;

        %% TV:
        if flagTV
            if ~flagTV_iso
                for j = 1:nCha
                    v_tv{1,j} = MTV_2D((x{1,j} + d_tv{1,j}), threshold_TV(j), n1, n2);
                end;
            else
                for j = 1:nCha
                    if (itr < 2)
                        [v_tv{1,j}, P]=denoise_TV_One((x{1,j} + d_tv{1,j}), threshold_TV(j),-inf,inf,[],parsin);
                    else
                        [v_tv{1,j}, P]=denoise_TV_One((x{1,j} + d_tv{1,j}), threshold_TV(j),-inf,inf,P,parsin);
                    end;
                end;
            end;
            d_tv{1,j} = d_tv{1,j} + x{1,j} - v_tv{1,j};
        end;

        %% NLTV
        if flagNLTV
            if itr >= itrNLTV
                if flagSBNLTV
                    for j = 1:nCha
                        v_nltv{1,j} = SB_NLTVfunc_slim_rescale( (x{1,j} + d_nltv{1,j}),n1,n2, threshold_NLTV(j), threshold_NLTV_h(j) );
                    end;
                else
                    for j = 1:nCha
                        % if mod(itr,5) == 0 || itr == 1
                        v_nltv{1,j} = NLMF((x{1,j} + d_nltv{1,j}),NLTV_struct);
                        v_nltv{1,j} = (L.*(x{1,j} + d_nltv{1,j}) + 2*threshold_NLTV(j)*v_nltv{1,j})./(L+2*threshold_NLTV(j));
                        % end;
                    end;
                end;
                d_nltv{1,j} = d_nltv{1,j} + x{1,j} - v_nltv{1,j};
            else
                v_nltv{1,j} = 0;
                d_nltv{1,j} = 0;
            end;

        end;

        %% l12-Wavelet
        if flagGroup
            for j = 1:nCha
                if flag_extendImage
                    x_proxA{1,j} = extend_image(x{1,j}, waveS_l12proxA, waveletStages, proxA_extend_y, proxA_extend_x);
                    x_g_helper{1,j} = wavedec2(x_proxA{1,j},waveletStages,waveletFilterName_l12) + d_group{1,j};
                else
                    x_g_helper{1,j} = wavedec2(x{1,j},waveletStages,waveletFilterName_l12) + d_group{1,j};
                end;

                if flag_wavetree
                    x_g_helper{2,j} = (G_prox*x_g_helper{1,j}')';
                else
                    x_g_helper{2,j} = zeros(1,size(x_g_helper{1,j},2));
                end;
            end;

            % softthresh_proxA_cha returns cell! hence (1,:) assignement
            v_group(1,:) = softthresh_proxA_cha(x_g_helper,threshold_group(j),nCha,waveS_l12proxA,groupnorm_index);

            for j = 1:nCha
                d_group{1,j} = d_group{1,j} + x_g_helper{1,j} - v_group{1,j};
            end;
        end;

        %% metrics of current itr:
        itr = itr + 1;
%         disp(itr);
        dispProgress('Proximal Average', itr/(maxitrOUTER*maxitrINNER));

        metrics.xtime(itr+1)= toc(timer_proxA);
%         [metrics, ssim_map{1,2}] = get_metrics_itr( im_ref, im_ref_full, x, itr, maxitr, nCha, n1, n2, metrics, obj.K_1, obj.K_2, obj.W_size, obj.W_sigma );

        if itr >= maxitr break; end;
    end;
    if itr >= maxitr break; end;
    for j=1:nCha
        b_outer{1,j} = b_outer{1,j} + b{1,j} - FFT2D_mask(x{1,j},mask);
        FTb{1,j} = real(iFFT2D(b_outer{1,j}));
    end;
end;
dispProgress('Proximal Average', 'Close');

imageCha = x;
for j = 1:nCha
    imageCha{1,j} = turn_image( imageCha{1,j} );
end;
% for j = 1:nCha+1
%     ssim_map{1,1}{1,j} = turn_image( ssim_map{1,1}{1,j} );
%     ssim_map{1,2}{1,j} = turn_image( ssim_map{1,2}{1,j} );
% end;

end


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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: updates the spring attributes!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function springs_info = update_Springs(dt,current_time,xLag,yLag,springs_info)

%springs_info: col 1: starting spring pt (by lag. discretization)
%              col 2: ending spring pt. (by lag. discretization)
%              col 3: spring stiffness
%              col 4: spring resting lengths

%RL = springs_info(:,4); % resting-length vector

freq = 0.875;
%theta=freq*current_time*2*pi;
%phase = sin(theta);

%size(springs_info)

%springs_info(1:3,:)
%pause();

% CHANGE RESTING LENGTH BTWN SIDES OF JELLYFISH BELL
%springs_info(319:end,4) = phase;

springs_info(319:end,4) = abs( sin(0.875*current_time*2*pi) );

% if ( mod(current_time,0.875) < 0.875/2 )
%     springs_info(319:end,4) = abs( sin(0.875*current_time*2*pi) );
% else
%     springs_info(319:end,4) = 0;%abs( sin(0.875*current_time*2*pi) );
% end


%% Machine Learning Online Class
%  Exercise 8 | Anomaly Detection and Collaborative Filtering
%
%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  exercise. You will need to complete the following functions:
%
%     estimateGaussian.m
%     selectThreshold.m
%     cofiCostFunc.m
%
%  For this exercise, you will not need to change any code in this file,
%  or any other files other than those mentioned above.
%

%% =============== Part 1: Loading movie ratings dataset ================
%  You will start by loading the movie ratings dataset to understand the
%  structure of the data.
%
fprintf('Loading movie ratings dataset.\n\n');

%  Load data
load ('ex8_movies.mat');

%  Y is a 1682x943 matrix, containing ratings (1-5) of 1682 movies on
%  943 users
%
%  R is a 1682x943 matrix, where R(i,j) = 1 if and only if user j gave a
%  rating to movie i

%  From the matrix, we can compute statistics like average rating.
fprintf('Average rating for movie 1 (Toy Story): %f / 5\n\n', ...
        mean(Y(1, R(1, :))));

%  We can "visualize" the ratings matrix by plotting it with imagesc
imagesc(Y);
ylabel('Movies');
xlabel('Users');

%fprintf('\nProgram paused. Press enter to continue.\n');
%pause;

%% ============ Part 2: Collaborative Filtering Cost Function ===========
%  You will now implement the cost function for collaborative filtering.
%  To help you debug your cost function, we have included set of weights
%  that we trained on that. Specifically, you should complete the code in
%  cofiCostFunc.m to return J.

%  Load pre-trained weights (X, Theta, num_users, num_movies, num_features)
load ('ex8_movieParams.mat');

%  Reduce the data set size so that this runs faster
num_users = 4; num_movies = 5; num_features = 3;
X = X(1:num_movies, 1:num_features);
Theta = Theta(1:num_users, 1:num_features);
Y = Y(1:num_movies, 1:num_users);
R = R(1:num_movies, 1:num_users);

%  Evaluate cost function
J = cofiCostFunc([X(:) ; Theta(:)], Y, R, num_users, num_movies, ...
               num_features, 0);

fprintf(['Cost at loaded parameters: %f '...
         '\n(this value should be about 22.22)\n'], J);

fprintf('\nProgram paused. Press enter to continue.\n');
pause;

%% ============== Part 3: Collaborative Filtering Gradient ==============
%  Once your cost function matches up with ours, you should now implement
%  the collaborative filtering gradient function. Specifically, you should
%  complete the code in cofiCostFunc.m to return the grad argument.
%
fprintf('\nChecking Gradients (without regularization) ... \n');

%  Check gradients by running checkNNGradients
checkCostFunction;

fprintf('\nProgram paused. Press enter to continue.\n');
pause;

%% ========= Part 4: Collaborative Filtering Cost Regularization ========
%  Now, you should implement regularization for the cost function for
%  collaborative filtering. You can implement it by adding the cost of
%  regularization to the original cost computation.
%

%  Evaluate cost function
J = cofiCostFunc([X(:) ; Theta(:)], Y, R, num_users, num_movies, ...
               num_features, 1.5);

fprintf(['Cost at loaded parameters (lambda = 1.5): %f '...
         '\n(this value should be about 31.34)\n'], J);

fprintf('\nProgram paused. Press enter to continue.\n');
pause;

%% ======= Part 5: Collaborative Filtering Gradient Regularization ======
%  Once your cost matches up with ours, you should proceed to implement
%  regularization for the gradient.
%

%
fprintf('\nChecking Gradients (with regularization) ... \n');

%  Check gradients by running checkNNGradients
checkCostFunction(1.5);

fprintf('\nProgram paused. Press enter to continue.\n');
pause;

%% ============== Part 6: Entering ratings for a new user ===============
%  Before we will train the collaborative filtering model, we will first
%  add ratings that correspond to a new user that we just observed. This
%  part of the code will also allow you to put in your own ratings for the
%  movies in our dataset!
%
movieList = loadMovieList();

%  Initialize my ratings
my_ratings = zeros(1682, 1);

% Check the file movie_idx.txt for id of each movie in our dataset
% For example, Toy Story (1995) has ID 1, so to rate it "4", you can set
my_ratings(1) = 4;

% Or suppose did not enjoy Silence of the Lambs (1991), you can set
my_ratings(98) = 2;

% We have selected a few movies we liked / did not like and the ratings we
% gave are as follows:
my_ratings(7) = 3;
my_ratings(12)= 5;
my_ratings(54) = 4;
my_ratings(64)= 5;
my_ratings(66)= 3;
my_ratings(69) = 5;
my_ratings(183) = 4;
my_ratings(226) = 5;
my_ratings(355)= 5;

fprintf('\n\nNew user ratings:\n');
for i = 1:length(my_ratings)
    if my_ratings(i) > 0
        fprintf('Rated %d for %s\n', my_ratings(i), ...
                 movieList{i});
    end
end

fprintf('\nProgram paused. Press enter to continue.\n');
pause;

%% ================== Part 7: Learning Movie Ratings ====================
%  Now, you will train the collaborative filtering model on a movie rating
%  dataset of 1682 movies and 943 users
%

fprintf('\nTraining collaborative filtering...\n');

%  Load data
load('ex8_movies.mat');

%  Y is a 1682x943 matrix, containing ratings (1-5) of 1682 movies by
%  943 users
%
%  R is a 1682x943 matrix, where R(i,j) = 1 if and only if user j gave a
%  rating to movie i

%  Add our own ratings to the data matrix
Y = [my_ratings Y];
R = [(my_ratings ~= 0) R];

%  Normalize Ratings
[Ynorm, Ymean] = normalizeRatings(Y, R);

%  Useful Values
num_users = size(Y, 2);
num_movies = size(Y, 1);
num_features = 10;

% Set Initial Parameters (Theta, X)
X = randn(num_movies, num_features);
Theta = randn(num_users, num_features);

initial_parameters = [X(:); Theta(:)];

% Set options for fmincg
options = optimset('GradObj', 'on', 'MaxIter', 100);

% Set Regularization
lambda = 10;
theta = fmincg (@(t)(cofiCostFunc(t, Y, R, num_users, num_movies, ...
                                num_features, lambda)), ...
                initial_parameters, options);

% Unfold the returned theta back into U and W
X = reshape(theta(1:num_movies*num_features), num_movies, num_features);
Theta = reshape(theta(num_movies*num_features+1:end), ...
                num_users, num_features);

fprintf('Recommender system learning completed.\n');

fprintf('\nProgram paused. Press enter to continue.\n');
pause;

%% ================== Part 8: Recommendation for you ====================
%  After training the model, you can now make recommendations by computing
%  the predictions matrix.
%

p = X * Theta';
my_predictions = p(:,1) + Ymean;

movieList = loadMovieList();

[r, ix] = sort(my_predictions, 'descend');
fprintf('\nTop recommendations for you:\n');
for i=1:10
    j = ix(i);
    fprintf('Predicting rating %.1f for movie %s\n', my_predictions(j), ...
            movieList{j});
end

fprintf('\n\nOriginal ratings provided:\n');
for i = 1:length(my_ratings)
    if my_ratings(i) > 0
        fprintf('Rated %d for %s\n', my_ratings(i), ...
                 movieList{i});
    end
end


function [labels adjlist pE] = trimapGenerateMultipleSegmentations2( image, imsegs, edgeClassifier, ecal, t, size_array )
    adjmat = imsegs.adjmat;
    segimage = imsegs.segimage;

    spFeat = getSuperpixelData_ver2(image, imsegs);
    [edgeFeat adjlist] = getEdgeData_ver2( imsegs, spFeat );
%     spFeat = mcmcGetSuperpixelData( image, imsegs );
%     [edgeFeat adjlist] = mcmcGetEdgeData( imsegs, spFeat );
    pE=test_boosted_dt_mc(edgeClassifier,edgeFeat);
    pE = 1 ./ (1+exp(ecal(1)*pE+ecal(2)));

    nSuperpixel = max(segimage(:));
    % labels = mexMergeAdjacentRegions2( adjlist, pE, nSuperpixel, t );
    labels = mexMergeAdjRegs_Felzenszwalb( adjlist, pE, nSuperpixel, t, size_array );

%     image_lab = rgb2lab( image );
%     bins = [8 16 16];
%     Q = computeQuantMatrix( image_lab, bins );
%     region_hist = computeRegionHist(Q, bins, segimage);
%
%     num_region = max(segimage(:));
%     region_dist = zeros(num_region, num_region);
%
%     ind = find(adjmat);
%     for ix = 1 : length(ind)
%         [x y] = ind2sub([num_region, num_region], ind(ix));
%         region_dist(x, y) = histDist(region_hist(x,:), region_hist(y,:));
%     end
%
%     t = 0.2:0.05:0.8;
%     labels = mexMergeAdjacentRegions(region_dist, t);

    for jx = 1 : size(labels, 2)
        L = labels(:, jx);
        temp_label = unique(L);
        for ix = 1 : length(temp_label)
            idx = find( L == temp_label(ix) );
            labels(idx, jx) = ix;
        end
    end


classdef dme_line3p < mp.dm_element
%MP.DME_LINE3P  MATPOWER data model class for 3-phase line data

%   MATPOWER
%   Copyright (c) 2021-2022, Power Systems Engineering Research Center (PSERC)
%   by 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.

    properties
        fbus    %% bus index vector for "from" port (port 1) (all lines)
        tbus    %% bus index vector for "to" port (port 2) (all lines)
        freq    %% system frequency, in Hz
        lc      %% index into lc_tab for lines that are on
        len     %% length for lines that are on
        lc_tab  %% line construction table
        ys      %% cell array of 3x3 series admittance matrices for lc rows
        yc      %% cell array of 3x3 shunt admittance matrices for lc rows
    end     %% properties

    methods
        function name = name(obj)
            name = 'line3p';
        end

        function label = label(obj)
            label = '3-ph Line';
        end

        function label = labels(obj)
            label = '3-ph Lines';
        end

        function name = cxn_type(obj)
            name = 'bus3p';
        end

        function name = cxn_idx_prop(obj)
            name = {'fbus', 'tbus'};
        end

        function names = main_table_var_names(obj)
            names = horzcat( main_table_var_names@mp.dm_element(obj), ...
                {'bus_fr', 'bus_to', 'lc', 'len', ...
                 'pl1_fr', 'ql1_fr', 'pl2_fr', 'ql2_fr', 'pl3_fr', 'ql3_fr', ...
                 'pl1_to', 'ql1_to', 'pl2_to', 'ql2_to', 'pl3_to', 'ql3_to' ...
                 });
        end

%         function vars = export_vars(obj)
%             vars = {'pl1_fr', 'ql1_fr', ...
%                     'pl2_fr', 'ql2_fr', ...
%                     'pl3_fr', 'ql3_fr', ...
%                     'pl1_to', 'ql1_to', ...
%                     'pl2_to', 'ql2_to', ...
%                     'pl3_to', 'ql3_to' };
%         end

        function obj = initialize(obj, dm)
            initialize@mp.dm_element(obj, dm); %% call parent

            %% get bus mapping info
            b2i = dm.elements.bus3p.ID2i;   %% bus num to idx mapping

            %% set bus index vectors for port connectivity
            obj.fbus = b2i(obj.tab.bus_fr);
            obj.tbus = b2i(obj.tab.bus_to);
        end

        function obj = update_status(obj, dm)
            %% get bus status info
            bs = dm.elements.bus3p.tab.status;  %% bus status

            %% update status of branches connected to isolated/offline buses
            obj.tab.status = obj.tab.status & bs(obj.fbus) & ...
                                              bs(obj.tbus);

            %% call parent to fill in on/off
            update_status@mp.dm_element(obj, dm);
        end

        function obj = build_params(obj, dm)
            nlc = size(obj.lc, 1);
            obj.ys = zeros(nlc, 6);
            obj.yc = zeros(nlc, 6);
            obj.lc = obj.tab.lc(obj.on);
            obj.len = obj.tab.len(obj.on);

            %% build Ys and Yc for relevant lines
            idx = unique(obj.lc);
            rr = obj.lc_tab.r(idx, :);
            xx = obj.lc_tab.x(idx, :);
            cc = obj.lc_tab.c(idx, :);
%             rr = obj.lc_tab{idx, 2:7};
%             xx = obj.lc_tab{idx, 8:13};
%             cc = obj.lc_tab{idx, 14:19};
            for k = 1:length(idx)
                R = obj.vec2symmat(rr(k, :));
                X = obj.vec2symmat(xx(k, :));
                C = obj.vec2symmat(cc(k, :));
                Ys = inv(R + 1j * X);
                Yc = 1j * 2*pi * obj.freq * 1e-9 * C;
                obj.ys(k, :) = obj.symmat2vec(Ys);
                obj.yc(k, :) = obj.symmat2vec(Yc);
            end
        end

        function M = vec2symmat(obj, v)
            % makes a symmetric matrix from a vector of 6 values
            M = [v(1) v(2) v(3);
                 v(2) v(4) v(5);
                 v(3) v(5) v(6) ];
        end

        function v = symmat2vec(obj, M)
            % extracts a vector of 6 values from a matrix assumed to be symmetric
            v = [M(1, :) M(2,2:3) M(3,3)];
        end

        function obj = pretty_print(obj, dm, section, out_e, mpopt, fd, pp_args)
            switch section
                case 'det'
                    %% compute currents/powers for pp_args
                    s_fr = [  obj.tab.pl1_fr + 1j * obj.tab.ql1_fr ...
                            obj.tab.pl2_fr + 1j * obj.tab.ql2_fr ...
                            obj.tab.pl3_fr + 1j * obj.tab.ql3_fr    ];
                    s_to = [  obj.tab.pl1_to + 1j * obj.tab.ql1_to ...
                            obj.tab.pl2_to + 1j * obj.tab.ql2_to ...
                            obj.tab.pl3_to + 1j * obj.tab.ql3_to    ];
                    t = dm.elements.bus3p.tab;    %% bus3p table
                    vm = [ t.vm1 t.vm2 t.vm3 ] .* (t.base_kv/sqrt(3) * [1 1 1]);
                    va = [ t.va1 t.va2 t.va3 ];
                    v_ = vm .* exp(1j * va * pi/180);
                    i_fr = conj( s_fr ./ v_(obj.fbus, :));
                    i_to = conj( s_to ./ v_(obj.tbus, :));
                    c_fr = struct('cm', abs(i_fr), 'ca', angle(i_fr) * 180/pi);
                    c_to = struct('cm', abs(i_to), 'ca', angle(i_to) * 180/pi);

                    pp_args.line3p = {'c', 'f', abs(i_fr), angle(i_fr) * 180/pi};
                    pretty_print@mp.dm_element(obj, dm, section, out_e, mpopt, fd, pp_args);

                    pp_args.line3p = {'c', 't', abs(i_to), angle(i_to) * 180/pi};
                    pretty_print@mp.dm_element(obj, dm, section, out_e, mpopt, fd, pp_args);

                    pp_args.line3p = {'s', 'f', real(s_fr), imag(s_fr)};
                    pretty_print@mp.dm_element(obj, dm, section, out_e, mpopt, fd, pp_args);

                    pp_args.line3p = {'s', 't', real(s_to), imag(s_to)};
                    pretty_print@mp.dm_element(obj, dm, section, out_e, mpopt, fd, pp_args);
                otherwise
                    pretty_print@mp.dm_element(obj, dm, section, out_e, mpopt, fd, pp_args);
            end
        end

        function TorF = pp_have_section_sum(obj, mpopt, pp_args)
            TorF = true;
        end

        function obj = pp_data_sum(obj, dm, rows, out_e, mpopt, fd, pp_args)
            %% call parent
            pp_data_sum@mp.dm_element(obj, dm, rows, out_e, mpopt, fd, pp_args);

            %% print generation summary
            t = obj.tab;
            ploss = [ t.pl1_fr t.pl2_fr t.pl3_fr ] + ...
                    [ t.pl1_to t.pl2_to t.pl3_to ];
            qloss = [ t.ql1_fr t.ql2_fr t.ql3_fr ] + ...
                    [ t.ql1_to t.ql2_to t.ql3_to ];
            fprintf(fd, '  %-29s %12.1f kW %12.1f kVAr\n', 'Total 3-ph line loss', ...
                sum(sum(ploss(obj.on, :))), sum(sum(qloss(obj.on, :))) );
        end

        function TorF = pp_have_section_det(obj, mpopt, pp_args)
            TorF = true;
        end

        function h = pp_get_headers_det(obj, dm, out_e, mpopt, pp_args)
            cs = pp_args.line3p{1};
            ft = pp_args.line3p{2};
            if cs == 'c' && ft == 'f'
                h1 = pp_get_headers_det@mp.dm_element(obj, dm, out_e, mpopt, pp_args);
            else
                h1 = {};
            end
            if cs == 'c'
                if ft == 'f'
                    h2 = {'-->  Current Injections at "From" Bus' };
                else    %% ft == 't'
                    h2 = {'', ...
                        '<--  Current Injections at "To" Bus' };
                end
            else    %% cs == 's'
                if ft == 'f'
                    h2 = {'', ...
                        '-->  Power Injections at "From" Bus' };
                else    %% ft == 't'
                    h2 = {'', ...
                        '<--  Power Injections at "To" Bus' };
                end
            end
            switch cs
                case 'c'
                    h = [ h1 h2 ...
                        {   '  3-ph    3-ph Bus  3-ph Bus          Phase A Current  Phase B Current  Phase C Current', ...
                            'Line ID    From ID   To ID    Status   (A)    (deg)     (A)    (deg)     (A)    (deg)', ...
                            '--------  --------  --------  ------  ------  ------   ------  ------   ------  ------' } ];
                    %%       1234567 123456789 123456789 -----1 123456.89 12345.7 12345.78 12345.7 12345.78 12345.7
                case 's'
                    h = [ h1 h2 ...
                        {   '  3-ph    3-ph Bus  3-ph Bus          Phase A Power    Phase B Power    Phase C Power', ...
                            'Line ID    From ID   To ID    Status   (kW)   (kVAr)    (kW)   (kVAr)    (kW)   (kVAr)', ...
                            '--------  --------  --------  ------  ------  ------   ------  ------   ------  ------' } ];
                    %%       1234567 123456789 123456789 -----1 1234567.9 12345.7 123456.8 12345.7 123456.8 12345.7
            end
        end

        function str = pp_data_row_det(obj, dm, k, out_e, mpopt, fd, pp_args)
            switch pp_args.line3p{1}    %% cs
                case 'c'
                    cm = pp_args.line3p{3}(k, :);
                    ca = pp_args.line3p{4}(k, :);
                    str = sprintf('%7d %9d %9d %6d %9.2f %7.1f %8.2f %7.1f %8.2f %7.1f', ...
                        obj.tab.uid(k), obj.tab.bus_fr(k), obj.tab.bus_to(k), ...
                        obj.tab.status(k), ...
                        cm(:, 1), ca(:, 1), ...
                        cm(:, 2), ca(:, 2), ...
                        cm(:, 3), ca(:, 3) );
                case 's'
                    p = pp_args.line3p{3}(k, :);
                    q = pp_args.line3p{4}(k, :);
                    str = sprintf('%7d %9d %9d %6d %9.1f %7.1f %8.1f %7.1f %8.1f %7.1f', ...
                        obj.tab.uid(k), obj.tab.bus_fr(k), obj.tab.bus_to(k), ...
                        obj.tab.status(k), ...
                        p(:, 1), q(:, 1), ...
                        p(:, 2), q(:, 2), ...
                        p(:, 3), q(:, 3) );
            end
        end
    end     %% methods
end         %% classdef


clear;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% indicate the super-resolution factor
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
down_factor = 3;    % 2, 3 or 4
image_path = 'data/deepeaf/BSDS500/';
data_chs = 1;   % 1 or 3
training_mat_path = 'data/Shepard_CNN/Shepard_super_res/x3/train_1ch/';
val_mat_path = 'data/Shepard_CNN/Shepard_super_res/x3/val_1ch/';

patch_dim = 48;
num_patches = 20000;
listing = dir(strcat(image_path, '*.jpg'));
mat_num = 51;
for m = 1 : mat_num
    fprintf('Extracting patch batch: %d / %d\n', m, mat_num);
    % extract random patches
    images = zeros(patch_dim, patch_dim, data_chs, num_patches);
    labels = zeros(size(images));
    for i = 1 : num_patches
        if (mod(i,100) == 0)
            fprintf('Extracting patch: %d / %d\n', i, num_patches);
        end

        r_idx = random('unid', size(listing, 1));
        I = im2double(imread(strcat(image_path, listing(r_idx).name)));
        orig_img_size = size(I);
        r = random('unid', orig_img_size(1) - patch_dim + 1);
        c = random('unid', orig_img_size(2) - patch_dim + 1);

        patch = I(r:r+patch_dim-1, c:c+patch_dim-1, :);

        if(data_chs == 1)
            patch = rgb2ycbcr(patch);
            patch = patch(:,:,1);
        end

        patch_down = imresize(patch, [patch_dim/down_factor, patch_dim/down_factor], 'bicubic');
        patch_up = imresize(patch_down, [patch_dim, patch_dim], 'nearest');
        images(:,:,:,i) = patch_up;
        labels(:,:,:,i) = patch;
        %imshow([patch_up patch]); pause;
    end
    % save it
    images = single(images);
    labels = single(labels);

    if(m >= mat_num)
        % save validation data
        filename = strcat(val_mat_path, 'val_1');
        images_t = images;
        labels_t = labels;
        save(filename, '-v7.3', 'images_t', 'labels_t');
    else
        % save training data
        filename = strcat(training_mat_path, 'patches_', num2str(m));
        save(filename, '-v7.3', 'images', 'labels');
    end
end


function [ t_start, y_start ] = p29_start ( neqn )

%*****************************************************************************80
%
%% P29_START returns the starting point for problem p29.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Output, real T_START, Y_START(NEQN,1), the initial data.
%
  y_start = zeros ( neqn, 1 );

  t_start = 0.0;
  y_start = 1.0;

  return
end


function value = i4vec_index ( n, a, aval )

%*****************************************************************************80
%
%% I4VEC_INDEX returns the first location of a given value in an I4VEC
%
%  Discussion:
%
%    An I4VEC is a vector of integer values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer A(N), the vector to be searched.
%
%    Input, integer AVAL, the value to be indexed.
%
%    Output, integer VALUE, the first location in A which has the
%    value AVAL, or -1 if no such index exists.
%
  for i = 1 : n
    if ( a(i) == aval )
      value = i;
      return
    end
  end

  value = -1;

  return
end


function range = p18_range ( )

%*****************************************************************************80
%
%% P18_RANGE returns an interval bounding the root for problem 18.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real RANGE(2), the minimum and maximum values of
%    an interval containing the root.
%
  range(1) = 0.988;
  range(2) = 1.012;

  return
end


function r = YZRectangle(x,y1,y2,z1,z2)

%YZRECTANGLE Rectangle in the YZ plane.

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

s1 = ySegment(x,y1,y2,z1);
s2 = zSegment(x,y2,z1,z2);
s3 = ySegment(x,y2,y1,z2);
s4 = zSegment(x,y1,z2,z1);

r = [s1 s2 s3 s4];

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

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

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

%   # END GPL LICENSE


classdef lowrank
% Generates matrices of low rank

methods(Static)

function A = from_randn(m, n, r)
    % Returns a low rank matrix constructed by randn(m, r) * randn(r, m)
    X = randn(m, r);
    Y  = randn(r, n);
    A = X * Y;
end % function

end % methods

end % classdef


function dcline = psse_convert_hvdc(dc, bus)
%PSSE_CONVERT_HVDC Convert HVDC data from PSS/E RAW to MATPOWER
%   DCLINE = PSSE_CONVERT_HVDC(DC, BUS)
%
%   Convert all two terminal HVDC line data read from a PSS/E
%   RAW data file into MATPOWER format. Returns a dcline matrix for
%   inclusion in a MATPOWER case struct.
%
%   Inputs:
%       DC  : matrix of raw two terminal HVDC line data returned by
%             PSSE_READ in data.twodc.num
%       BUS : MATPOWER bus matrix
%
%   Output:
%       DCLINE : a MATPOWER dcline matrix suitable for inclusion in
%                a MATPOWER case struct.
%
%   See also PSSE_CONVERT.

%   MATPOWER
%   Copyright (c) 2014-2016, Power Systems Engineering Research Center (PSERC)
%   by Yujia Zhu, PSERC ASU
%   and Ray Zimmerman, PSERC Cornell
%   Based on mpdcin.m and mpqhvdccal.m, written by:
%       Yujia Zhu, Jan 2014, yzhu54@asu.edu.
%
%   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 bus, gen, branch 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;
c = idx_dcline;

nb = size(bus, 1);
ndc = size(dc, 1);
e2i = sparse(bus(:, BUS_I), ones(nb, 1), 1:nb, max(bus(:, BUS_I)), 1);
if ~ndc
    dcline = [];
    return;
end

%% extract data
MDC = dc(:,2); % Control mode
SETVL = dc(:,4); % depend on control mode: current or power demand
VSCHD = dc(:,5); % scheduled compounded dc voltage
ANMXR = dc(:,15); % nominal maximum rectifier firing angle
ANMNR = dc(:,16); % nominal minimum rectifier firing angle
GAMMX = dc(:,32); % nominal maximum inverter firing angle
GAMMN = dc(:,33); % nominal minimum inverter firing angle
SETVL = abs(SETVL);
% Convert the voltage on rectifier side and inverter side
% The value is calculated as basekV/VSCHD
% basekV is the bus base voltage, VSCHD is the scheduled compounded
% voltage
dcline = zeros(ndc, c.LOSS1); % initiate the hvdc data format
indr = dc(:,13); % rectifier end bus number
indi = dc(:,30); % inverter end bus number
dcind = [indr indi];
% bus nominal voltage
Vr = bus(e2i(indr), VM);
Vi = bus(e2i(indi), VM);
%% Calculate the real power input at the from end
PMW = zeros(ndc, 1);
for i = 1:ndc
    if MDC(i) == 1
        PMW(i) = SETVL(i); % SETVL is the desired real power demand
    elseif MDC(i) == 2;
        PMW(i) = SETVL(i)*VSCHD(i)/1000; % SETVL is the current in amps (need devide 1000 to convert to MW)
    else PMW(i) = 0;
    end
end
%% calculate reactive power limits
[Qrmin,Qrmax] = psse_convert_hvdc_Qlims(ANMXR,ANMNR,PMW);    %% rectifier end
[Qimin,Qimax] = psse_convert_hvdc_Qlims(GAMMX,GAMMN,PMW);    %% inverter end
%% calculate the loss coefficient (Only consider the l1)
% l1 = P'.*RDC;

%% conclude all info
status = ones(ndc, 1);
status(MDC==0) = 0;     %% set status of blocked HVDC lines to zero
% dcline(:,[1 2 3 4 5 8 9 10 11 12 13 14 15]) = [indr,indi,status,PMW, PMW, Vr, Vi,0.85*PMW, 1.15*PMW, Qrmin, Qrmax, Qimin, Qimax];
dcline(:, [c.F_BUS c.T_BUS c.BR_STATUS c.PF c.PT c.VF c.VT ...
            c.PMIN c.PMAX c.QMINF c.QMAXF c.QMINT c.QMAXT]) = ...
    [indr indi status PMW PMW Vr Vi 0.85*PMW 1.15*PMW Qrmin Qrmax Qimin Qimax];

function [Qmin, Qmax] = psse_convert_hvdc_Qlims(alphamax,alphamin,P)
%PSSE_CONVERT_HVDC_QLIMS calculate HVDC line reactive power limits
%
%   [Qmin, Qmax] = psse_convert_hvdc_Qlims(alphamax,alphamin,P)
%
% Inputs:
%       alphamax :  maximum firing angle
%       alphamin :  minimum steady-state rectifier firing angle
%       P :         real power demand
% Outputs:
%       Qmin :  lower limit of reactive power
%       Qmax :  upper limit of reactive power
%
% Note:
%   This function calculates the reactive power at the rectifier or inverter
%   end. It is assumed the maximum overlap angle is 60 degree (see
%   Kimbark's book). The maximum reactive power is calculated with the
%   power factor:
%       pf = acosd(0.5*(cosd(alphamax(i))+cosd(60))),
%   where, 60 is the maximum delta angle.

len = length(alphamax);
phi = zeros(size(alphamax));
Qmin = phi;
Qmax = phi;
for i = 1:len
    %% minimum reactive power calculated under assumption of no overlap angle
    %% i.e. power factor equals to tan(alpha)
    Qmin(i) = P(i)*tand(alphamin(i));

    %% maximum reactive power calculated when overlap angle reaches max
    %% value (60 deg). I.e.
    %%      cos(phi) = 1/2*(cos(alpha)+cos(delta))
    %%      Q = P*tan(phi)
    phi(i) = acosd(0.5*(cosd(alphamax(i))+cosd(60)));
    Qmax(i) = P(i)*tand(phi(i));
    if Qmin(i)<0
        Qmin(i) = -Qmin(i);
    end
    if Qmax(i)<0
        Qmax(i) = -Qmax(i);
    end
end


%script v0730
while 1
  if or(x(k)==si,y(k)==1)|(v==2)
    break
  end
  xk1=x(k)+1;  yk1=y(k)-1;
  if I(xk1,yk1)
    break
  end
  xk2=xk1-1;yk2=yk1;
  tr=(I(xk2,yk2)>0)|I(xk1,y(k));
  if tr
    per=per+1;
    pr=pr+sqrt(2);
    if xk1==si
      per=per+1;
    else
      per=per+I(xk1+1,yk1);
    end
    if yk1==1
      per=per+1;
    else
      per=per+I(xk1,yk1-1);
    end
    k=k+1;v=6;
    x=[x xk1];  y=[y yk1];
  else
    break
  end
end


function wevent = segment_event_waveforms(w, cobj, pretrigger, posttrigger, arrivalTimeCorrection)
%SEGMENT_EVENT_WAVEFORMS take a continuous vector of waveforms, and based
% arrival times in the infrasoundEvent vector, generate a cell vector where
% each element is a waveform vector extracted around the arrival times
% given
% Usage:
%     wevent = segment_event_waveforms(w, infrasoundEvent, pretrigger,
%                                      posttrigger,arrivalTimeCorrection)
% pretrigger and posttrigger are in seconds (e.g. 1)
if ~exist('arrivalTimeCorrection','var')
    arrivalTimeCorrection = 0.0;
end
disp('Segmenting event waveforms...')
numEvents = numel(infrasoundEvent);
for eventNumber=1:numEvents
    fprintf('- segmenting infrasound event %d of %d\n',eventNumber,numEvents );
    time1 = infrasoundEvent(eventNumber).FirstArrivalTime-pretrigger/86400-arrivalTimeCorrection/86400;
%     if arrivalTimeCorrection==0.0
%         time2 = infrasoundEvent(eventNumber).LastArrivalTime+posttrigger/86400-arrivalTimeCorrection/86400;
%     else
        time2 = infrasoundEvent(eventNumber).FirstArrivalTime+posttrigger/86400-arrivalTimeCorrection/86400;
%     end
    w2 = detrend(extract(w, 'time', time1, time2));
    wevent{eventNumber} = w2;
    clear w2
end


%%*****************************************************************************
%% sqlp: solve an semidefinite-quadratic-linear program
%%       by infeasible path-following method.
%%
%%  [obj,X,y,Z,info,runhist] = sqlp(blk,At,C,b,OPTIONS,X0,y0,Z0);
%%
%%  Input: blk: a cell array describing the block diagonal structure of SQL data.
%%          At: a cell array with At{p} = [svec(Ap1) ... svec(Apm)]
%%         b,C: data for the SQL instance.
%%  (X0,y0,Z0): an initial iterate (if it is not given, the default is used).
%%     OPTIONS: a structure that specifies parameters required in sqlp.m,
%%              (if it is not given, the default in sqlparameters.m is used).
%%
%%  Output: obj  = [<C,X> <b,y>].
%%          (X,y,Z): an approximately optimal solution or a primal or dual
%%                   infeasibility certificate.
%%          info.termcode = termination-code
%%          info.iter     = number of iterations
%%          info.obj      = [primal-obj, dual-obj]
%%          info.cputime  = total-time
%%          info.gap      = gap
%%          info.pinfeas  = primal_infeas
%%          info.dinfeas  = dual_infeas
%%          runhist.pobj    = history of primal objective value.
%%          runhist.dobj    = history of dual   objective value.
%%          runhist.gap     = history of <X,Z>.
%%          runhist.pinfeas = history of primal infeasibility.
%%          runhist.dinfeas = history of dual   infeasibility.
%%          runhist.cputime = history of cputime spent.
%%----------------------------------------------------------------------------
%%  The OPTIONS structure specifies the required parameters:
%%      vers  gam  predcorr  expon  gaptol  inftol  steptol
%%      maxit  printlevel  scale_data ...
%%      (all have default values set in sqlparameters.m).
%%*************************************************************************
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*************************************************************************

function [obj,X,y,Z,info,runhist] = sqlp(blk,At,C,b,OPTIONS,X0,y0,Z0)

if (nargin < 5); OPTIONS = []; end

isemptyAtb = 0;
if isempty(At) && isempty(b);
    %% Add redundant constraint: <-I,X> <= 0
    b = 0;
    At = ops(ops(blk,'identity'),'*',-1);
    numblk = size(blk,1);
    blk{numblk+1,1} = 'l'; blk{numblk+1,2} = 1;
    At{numblk+1,1} = 1; C{numblk+1,1} = 0;
    isemptyAtb = 1;
end
%%
%%-----------------------------------------
%% get parameters from the OPTIONS structure.
%%-----------------------------------------
%%

matlabversion = sscanf(version,'%f');
if strcmp(computer,'PCWIN64') || strcmp(computer,'GLNXA64')
    par.computer = 64;
else
    par.computer = 32;
end
par.matlabversion = matlabversion(1);
par.vers        = 0;
par.predcorr    = 1;
par.gam         = 0;
par.expon       = 1;
par.gaptol      = 1e-8;
par.inftol      = 1e-8;
par.steptol     = 1e-6;
par.maxit       = 100;
par.printlevel  = 3;
par.stoplevel   = 1;
par.scale_data  = 0;
par.spdensity   = 0.4;
par.rmdepconstr = 0;
par.smallblkdim = 50;
par.schurfun     = cell(size(blk,1),1);
par.schurfun_par = cell(size(blk,1),1);
%%
parbarrier  = cell(size(blk,1),1);
for p = 1:size(blk,1)
    pblk = blk(p,:);
    if strcmp(pblk{1},'s') || strcmp(pblk{1},'q')
        parbarrier{p} = zeros(1,length(pblk{2}));
    elseif strcmp(pblk{1},'l') || strcmp(pblk{1},'u' )
        parbarrier{p} = zeros(1,sum(pblk{2}));
    end
end
parbarrier_0 = parbarrier;
%%
if nargin > 4,
    if isfield(OPTIONS,'vers');        par.vers     = OPTIONS.vers; end
    if isfield(OPTIONS,'predcorr');    par.predcorr = OPTIONS.predcorr; end
    if isfield(OPTIONS,'gam');         par.gam      = OPTIONS.gam; end
    if isfield(OPTIONS,'expon');       par.expon    = OPTIONS.expon; end
    if isfield(OPTIONS,'gaptol');      par.gaptol   = OPTIONS.gaptol; end
    if isfield(OPTIONS,'inftol');      par.inftol   = OPTIONS.inftol; end
    if isfield(OPTIONS,'steptol');     par.steptol  = OPTIONS.steptol; end
    if isfield(OPTIONS,'maxit');       par.maxit    = OPTIONS.maxit; end
    if isfield(OPTIONS,'printlevel');  par.printlevel  = OPTIONS.printlevel; end
    if isfield(OPTIONS,'stoplevel');   par.stoplevel   = OPTIONS.stoplevel; end
    if isfield(OPTIONS,'scale_data');  par.scale_data  = OPTIONS.scale_data; end
    if isfield(OPTIONS,'spdensity');   par.spdensity   = OPTIONS.spdensity; end
    if isfield(OPTIONS,'rmdepconstr'); par.rmdepconstr = OPTIONS.rmdepconstr; end
    if isfield(OPTIONS,'smallblkdim'); par.smallblkdim = OPTIONS.smallblkdim; end
    if isfield(OPTIONS,'parbarrier');
        parbarrier = OPTIONS.parbarrier;
        if isempty(parbarrier); parbarrier = parbarrier_0; end
        if ~iscell(parbarrier);
            tmp = parbarrier; clear parbarrier; parbarrier{1} = tmp;
        end
        if (length(parbarrier) < size(blk,1))
            len = length(parbarrier);
            parbarrier(len+1:size(blk,1)) = parbarrier_0(len+1:size(blk,1));
        end
    end
    if isfield(OPTIONS,'schurfun');
        par.schurfun = OPTIONS.schurfun;
        if ~isempty(par.schurfun); par.scale_data = 0; end
    end
    if isfield(OPTIONS,'schurfun_par'); par.schurfun_par = OPTIONS.schurfun_par; end
    if isempty(par.schurfun);     par.schurfun = cell(size(blk,1),1); end
    if isempty(par.schurfun_par); par.schurfun_par = cell(size(blk,1),1); end
end
if (size(blk,2) > 2); par.smallblkdim = 0; end
%%
%%-----------------------------------------
%% convert matrices to cell arrays.
%%-----------------------------------------
%%
if ~iscell(At); At = {At}; end;
if ~iscell(C);  C = {C}; end;
if all(size(At) == [size(blk,1), length(b)]);
    convertyes = zeros(size(blk,1),1);
    for p = 1:size(blk,1)
        if strcmp(blk{p,1},'s') && all(size(At{p,1}) == sum(blk{p,2}))
            convertyes(p) = 1;
        end
    end
    if any(convertyes)
        if (par.printlevel);
            fprintf('\n sqlp: converting At into required format');
        end
        At = svec(blk,At,ones(size(blk,1),1));
    end
end
%%
%%-----------------------------------------
%% validate SQLP data.
%%-----------------------------------------
%%
% tstart = cputime;
[blk,At,C,b,blkdim,numblk,parbarrier] = validate(blk,At,C,b,par,parbarrier);
[blk,At,C,b,iscmp] = convertcmpsdp(blk,At,C,b);
if (iscmp) && (par.printlevel>=2);
    fprintf('\n SQLP has complex data');
end
if (nargin <= 5) || (isempty(X0) || isempty(y0) || isempty(Z0));
    par.startpoint = 1;
    [X0,y0,Z0] = infeaspt(blk,At,C,b);
else
    par.startpoint = 2;
    if ~iscell(X0);  X0 = {X0}; end;
    if ~iscell(Z0);  Z0 = {Z0}; end;
    y0 = real(y0);
    if (length(y0) ~= length(b));
        error('sqlp: length of b and y0 not compatible');
    end
    [X0,Z0] = validate_startpoint(blk,X0,Z0,par.spdensity,iscmp);
end
if (par.printlevel>=2)
    fprintf('\n num. of constraints = %2.0d',length(b));
    if blkdim(1);
        fprintf('\n dim. of sdp    var  = %2.0d,',blkdim(1));
        fprintf('   num. of sdp  blk  = %2.0d',numblk(1));
    end
    if blkdim(2);
        fprintf('\n dim. of socp   var  = %2.0d,',blkdim(2));
        fprintf('   num. of socp blk  = %2.0d',numblk(2));
    end
    if blkdim(3); fprintf('\n dim. of linear var  = %2.0d',blkdim(3)); end
    if blkdim(4); fprintf('\n dim. of free   var  = %2.0d',blkdim(4)); end
end
%%
%%-----------------------------------------
%% detect unrestricted blocks in linear blocks
%%-----------------------------------------
%%
user_supplied_schurfun = 0;
for p = 1:size(blk,1)
    if ~isempty(par.schurfun{p}); user_supplied_schurfun = 1; end
end
if (user_supplied_schurfun == 0)
    [blk2,At2,C2,ublkinfo,parbarrier2,X02,Z02] = ...
        detect_ublk(blk,At,C,parbarrier,X0,Z0,par.printlevel);
else
    blk2 = blk; At2 = At; C2 = C;
    parbarrier2 = parbarrier; X02 = X0; Z02 = Z0;
    ublkinfo = cell(size(blk2,1),1);
end
ublksize = blkdim(4);
for p = 1:size(ublkinfo,1)
    ublksize = ublksize + length(ublkinfo{p});
end
%%
%%-----------------------------------------
%% detect diagonal blocks in semidefinite blocks
%%-----------------------------------------
%%
if (user_supplied_schurfun==0)
    [blk3,At3,C3,diagblkinfo,diagblkchange,parbarrier3,X03,Z03] = ...
        detect_lblk(blk2,At2,C2,b,parbarrier2,X02,Z02,par.printlevel);
else
    blk3 = blk2; At3 = At2; C3 = C2;
    parbarrier3 = parbarrier2; X03 = X02; Z03 = Z02;
    diagblkchange = 0;
    diagblkinfo = cell(size(blk3,1),1);
end
%%
%%-----------------------------------------
%% main solver
%%-----------------------------------------
%%
%    exist_analytic_term = 0;
%    for p = 1:size(blk3,1);
%       idx = find(parbarrier3{p} > 0);
%       if ~isempty(idx); exist_analytic_term = 1; end
%    end
%
if (par.vers == 0);
    if blkdim(1); par.vers = 1; else par.vers = 2; end
end
par.blkdim = blkdim;
par.ublksize = ublksize;
[obj,X3,y,Z3,info,runhist] = ...
    sqlpmain(blk3,At3,C3,b,par,parbarrier3,X03,y0,Z03);
%%
%%-----------------------------------------
%% recover semidefinite blocks from linear blocks
%%-----------------------------------------
%%
if any(diagblkchange)
    X2 = cell(size(blk2,1),1); Z2 = cell(size(blk2,1),1);
    count = 0;
    for p = 1:size(blk2,1)
        pblk = blk2(p,:);
        n = sum(pblk{2});
        blkno      = diagblkinfo{p,1};
        idxdiag    = diagblkinfo{p,2};
        idxnondiag = diagblkinfo{p,3};
        if ~isempty(idxdiag)
            len = length(idxdiag);
            Xtmp = [idxdiag,idxdiag,X3{end}(count+1:count+len); n, n, 0];
            Ztmp = [idxdiag,idxdiag,Z3{end}(count+1:count+len); n, n, 0];
            if ~isempty(idxnondiag)
                [ii,jj,vv] = find(X3{blkno});
                Xtmp = [Xtmp; idxnondiag(ii),idxnondiag(jj),vv]; %#ok
                [ii,jj,vv] = find(Z3{blkno});
                Ztmp = [Ztmp; idxnondiag(ii),idxnondiag(jj),vv]; %#ok
            end
            X2{p} = spconvert(Xtmp);
            Z2{p} = spconvert(Ztmp);
            count = count + len;
        else
            X2(p) = X3(blkno); Z2(p) = Z3(blkno);
        end
    end
else
    X2 = X3; Z2 = Z3;
end
%%
%%-----------------------------------------
%% recover linear block from unrestricted block
%%-----------------------------------------
%%
numblk = size(blk,1);
numblknew = numblk;
X = cell(numblk,1); Z = cell(numblk,1);
for p = 1:numblk
    n = blk{p,2};
    if isempty(ublkinfo{p,1})
        X{p} = X2{p}; Z{p} = Z2{p};
    else
        Xtmp = zeros(n,1); Ztmp = zeros(n,1);
        Xtmp(ublkinfo{p,1}) = max(0,X2{p});
        Xtmp(ublkinfo{p,2}) = max(0,-X2{p});
        Ztmp(ublkinfo{p,1}) = max(0,Z2{p});
        Ztmp(ublkinfo{p,2}) = max(0,-Z2{p});
        if ~isempty(ublkinfo{p,3})
            numblknew = numblknew + 1;
            Xtmp(ublkinfo{p,3}) = X2{numblknew};
            Ztmp(ublkinfo{p,3}) = Z2{numblknew};
        end
        X{p} = Xtmp; Z{p} = Ztmp;
    end
end
%%
%%-----------------------------------------
%% recover complex solution
%%-----------------------------------------
%%
if (iscmp)
    for p = 1:numblk
        pblk = blk(p,:);
        n = sum(pblk{2})/2;
        if strcmp(pblk{1},'s');
            X{p} = X{p}(1:n,1:n) + sqrt(-1)*X{p}(n+1:2*n,1:n);
            Z{p} = Z{p}(1:n,1:n) + sqrt(-1)*Z{p}(n+1:2*n,1:n);
            X{p} = 0.5*(X{p}+X{p}');
            Z{p} = 0.5*(Z{p}+Z{p}');
        end
    end
end
if (isemptyAtb)
    X = X(1:end-1); Z = Z(1:end-1);
end
%%*****************************************************************************


function newRxnScores = groupRxnScores(model, origRxnScores, origRxnIds, groupIds, origRxnsToZero)
% groupRxnScores
% This function sums up the reaction scores for all reactions that were merged
% into one by the linear merge.
%
% model          The model with linearly merged rxns.
% origRxnScores  The rxnScores from the model before the linear merge.
% origRxnIds     The rxn ids of the model before the linear merge.
% groupIds       The groupIds vector output from linearMerge.
%                There is one integer for each rxn in origRxnIds. 0 means
%                the reaction was merged. A non-zero integer means that the
%                reaction was merged with all other rxns having the same integer.
% origRxnsToZero A logical vector saying which of the original rxns that should not
%                be part of the problem. The way this is solved is that all
%                such reactions have a rxnScore of 0. If any original rxnScore
%                value should be zero (which is very unlikely) it is changed to 0.01.
%                If the sum of the rxnScores for a merged rxn becomes zero
%                while some of them are nonzero, the new value will also be 0.01,
%                to distinguish the rxn from rxns with only rxns to zero.
%                There are two reasons why we don't want zeros in the reaction
%                scores unless these reactions should be ignored:
%                  1) we want to be able to separate those
%                  2) it is difficult to handle a zero value in the MILP -
%                     the on/off of such a reaction can be random, so better
%                     to fix it in one direction.

newRxnScores = zeros(length(model.rxns),1);
[~,ia,ib] = intersect(model.rxns,origRxnIds);
grpIdsMerged = nan(length(model.rxns),1);
grpIdsMerged(ia) = groupIds(ib);
%check if any of the original scores are 0, in that case change them to 0.01 (unlikely)
origRxnScores(origRxnScores == 0) = 0.01;
%Then set the rxn scores for rxns to zero to 0
origRxnScores(origRxnsToZero) = 0;

%fill in original scores
newRxnScores(ia) = origRxnScores(ib);

for i = 1:length(model.rxns)
    %for reactions that are not merged with anything, just keep score as it is
    if grpIdsMerged(i) ~= 0
        %find all original rxns in the group
        sel = groupIds == grpIdsMerged(i);
        newRxnScores(i) = sum(origRxnScores(sel));
        if (newRxnScores(i) == 0 && any(origRxnScores(sel) ~= 0))
           %special unfortunate case, where the reactions happen to sum to 0 while some of them are nonzero
           %set to 0.01 in this case (proabably pretty unusual)
           newRxnScores(i) = 0.01;
        end
    end
end

end


 function y = ir_phantom_resize(x, nx, ny)
%function y = ir_phantom_resize(x, nx, ny)
%| resize a [mx,my] phantom image to be [nx,ny]
%| by combination of downsampling and cropping

if nargin == 1 && streq(x, 'test'), ir_phantom_resize_test, return, end
if nargin < 3, ir_usage, end

mx = size(x,1);
my = size(x,2);
if nx == mx && ny == my
	y = x;
	return
end

if nx > mx || ny > my
	error 'requested size too large, implement interpolation!'
end

% if power of 2 size, then downsample
if 2^floor(log2(nx)) == nx && nx == ny
	y = downsample2(x, mx/nx);
return
end

nn = 2^max(ceil(log2([nx ny])));
y = downsample2(x, mx/nn);

% trim if non power of 2
if nx < nn
	y = y([1:nx]+round((nn-nx)/2),:);
end
if ny < nn
	y = y(:,[1:ny]+round((nn-ny)/2));
end

function ir_phantom_resize_test
x = zeros(64, 60);
x(20:30,10:15) = 1;
y = ir_phantom_resize(x, 32, 28);
im plc 1 2
im(1, x)
im(2, y)


function sdn = utc2sdn(utc)
% UTC2SDN   - Convert UTC seconds to matlab date format
%
% Use as: sdn = utc2sdn(utc);
%
% sdn = Matlab datenumber (se DATENUM and DATESTR)
% utc = Seconds since 01 Jan 1970 00:00:00

% Brian Schlining
% 12 Apr 2000

%datenum('01 Jan 1970 00:00:00') = 719529
sdn = utc/60/60/24 + 719529;


function y = apply_3d_func(f,x)

% apply_3d_func - apply a function to 3D arrays
%
%   y = apply_3d_func(f,x);
%
%   f should take a 2D NxNxN image and ouput a 2D NxNxN image
%   x is an N^3 x Q input
%   x is an N^3 x Q input
%
%   This function is used to adapt 3D functions (e.g. filtering) for the
%   barycenter code that assume 1D vectors.
%
%   Copyright (c) 2014 Gabriel Peyre

N = round( (size(x,1))^(1/3) );
P = size(x,2);

resh = @(x)reshape(x,[N N N]);
flat = @(x)x(:);

y = zeros(size(x));
for i=1:P
   y(:,i) = flat(f(resh(x(:,i))));
end

end


function hmm = states_supdate(hmm,hmm_noisy,rho,update)
% update==1, W; update==2, Omega; update==3, sigma; update==4, alpha
K = length(hmm_noisy.state);
Sind = hmm.train.Sind==1;
regressed = sum((hmm.train.Sind==1),1)>0;

for k = 1:K
    if any(update==1) && isfield(hmm_noisy.state(1),'W') && ~isempty(hmm_noisy.state(1).W.Mu_W)
        hmm.state(k).W.Mu_W = (1-rho) * hmm.state(k).W.Mu_W + ...
            rho * hmm_noisy.state(k).W.Mu_W;
        hmm.state(k).W.iS_W = (1-rho) * hmm.state(k).W.iS_W + ...
            rho * hmm_noisy.state(k).W.iS_W;
        if length(size(hmm.state(k).W.S_W))==2 && ... % full, uniquefull, uniqueAR or 1 dimension
               size(hmm.state(k).W.S_W,1)==size(hmm.state(k).W.S_W,2)
            hmm.state(k).W.S_W = inv(hmm.state(k).W.iS_W);
        else % diag or shareddiag
            for n = 1:size(hmm.state(k).W.S_W,1)
                hmm.state(k).W.S_W(n,Sind(:,n),Sind(:,n)) = ...
                    inv(permute(hmm.state(k).W.iS_W(n,Sind(:,n),Sind(:,n)),[2 3 1]));
            end
        end
    elseif any(update==2) && isfield(hmm_noisy.state(1),'Omega')
        hmm.state(k).Omega.Gam_rate = (1-rho) * hmm.state(k).Omega.Gam_rate + ...
            rho * hmm_noisy.state(k).Omega.Gam_rate;
        hmm.state(k).Omega.Gam_shape = (1-rho) * hmm.state(k).Omega.Gam_shape + ...
            rho * hmm_noisy.state(k).Omega.Gam_shape;
        if strcmp(hmm_noisy.train.covtype,'full') || ...
                strcmp(hmm_noisy.train.covtype,'uniquefull') || ...
                strcmp(hmm_noisy.train.covtype,'sharedfull')
            hmm.state(k).Omega.Gam_irate(regressed,regressed) = ...
                inv(hmm.state(k).Omega.Gam_rate(regressed,regressed));
        end
    elseif any(update==3) && isfield(hmm_noisy.state(1),'sigma')
        hmm.state(k).sigma.Gam_rate = (1-rho) * hmm.state(k).sigma.Gam_rate + ...
            rho * hmm_noisy.state(k).sigma.Gam_rate;
        hmm.state(k).sigma.Gam_shape = (1-rho) * hmm.state(k).sigma.Gam_shape + ...
            rho * hmm_noisy.state(k).sigma.Gam_shape;
    elseif any(update==4) && isfield(hmm_noisy.state(1),'alpha')
        hmm.state(k).alpha.Gam_rate = (1-rho) * hmm.state(k).alpha.Gam_rate + ...
            rho * hmm_noisy.state(k).alpha.Gam_rate;
        hmm.state(k).alpha.Gam_shape = (1-rho) * hmm.state(k).alpha.Gam_shape + ...
            rho * hmm_noisy.state(k).alpha.Gam_shape;
    elseif any(update==5) && isfield(hmm_noisy.state(1),'beta')
        hmm.state(k).beta.Gam_rate = (1-rho) * hmm.state(k).beta.Gam_rate + ...
            rho * hmm_noisy.state(k).beta.Gam_rate;
        hmm.state(k).beta.Gam_shape = (1-rho) * hmm.state(k).beta.Gam_shape + ...
            rho * hmm_noisy.state(k).beta.Gam_shape;
    end
end

if any(update==2) && isfield(hmm_noisy,'Omega')
    hmm.Omega.Gam_rate = (1-rho) * hmm.Omega.Gam_rate + ...
        rho * hmm_noisy.Omega.Gam_rate;
    hmm.Omega.Gam_shape = (1-rho) * hmm.Omega.Gam_shape + ...
        rho * hmm_noisy.Omega.Gam_shape;
    if strcmp(hmm_noisy.train.covtype,'uniquefull') || ...
            strcmp(hmm_noisy.train.covtype,'sharedfull')
        hmm.Omega.Gam_irate(regressed,regressed) = ...
            inv(hmm.Omega.Gam_rate(regressed,regressed));
    end
end

end