function I2 = im_fusion(I,wl,I1,options)
%IM_FUSION Summary of this function goes here
%   Detailed explanation goes here
% Input:
%   I - rows by cols by B hyperspectral image, in the range [0 1]
%   wl - 1 by B wavelength
%   I1 - rows1 by cols1 by b RGB image, in the range [0 255] or [0 1]
if nargin < 4
    options = [];
end

t_start = tic;

I1 = double(I1);
scale_rgb = 0;
if max(I1(:)) > 10 % in the range 0 - 255
    I1 = I1/255;
    scale_rgb = 1;
end

has_reg_params = parse_param(options,'has_reg_params',0);
bbl = parse_param(options,'bbl',true(1,size(I,3)));
show_fig = parse_param(options,'show_fig',0);
has_PSF_SRF = parse_param(options,'has_PSF_SRF',0);
regularization = parse_param(options,'regularization','LLMM'); %LMM or LLMM
sylvester_solver_mode = parse_param(options,'sylvester_solver_mode','adaptive_accurate');

%% automatically infer s and extra and check size match
[rows,cols,B] = size(I);
[rows1,cols1,b] = size(I1);

s = parse_param(options,'s', [floor(cols1/cols),floor(rows1/rows)]);
extra = parse_param(options,'extra',max(([cols1,rows1]-s.*[cols,rows])/2, [1,1]));

pad_color = 0;
if all([cols,rows].*s + extra*2 == [cols1,rows1])
    disp('Image sizes match exactly');
elseif all([cols,rows].*s == [cols1,rows1])
    % add padding for extra margin
    I1 = pad_rgb_margin(I1, extra);
    disp('Pad color image with extra margin');
    [rows1,cols1,b] = size(I1);
    pad_color = 1;
else
    error('Image sizes do not match');
end

%% decode PSF and SRF
if has_PSF_SRF
    H1 = options.H1;
    G = options.G;
    S = options.S;
    s = options.s;
    g = options.g;
    extra = options.extra;
    if scale_rgb
        H1 = H1/255;
    end
    I(:,:,~bbl) = [];
    wl(~bbl) = [];
elseif has_reg_params
    reg = options.reg_params;
    % bad bands are removed in interpret_reg_params
    [I,wl,I1,g,H1,extra,s,G,S] = interpret_reg_params(I,wl,bbl,...
        I1,reg.U2,reg.V2,reg.rho,reg.s2,reg.T2,reg.sigma2);
else
    % bad bands are removed in estimate_PSF_SRF
    [I,wl,I1,g,H1,extra,s,G,S] = estimate_PSF_SRF(I,wl,bbl,I1,s,extra,options);
end

%% adjust parameters
[rows,cols,B] = size(I);
[rows1,cols1,b] = size(I1);

N = rows * cols;
N1 = rows1 * cols1;

h0 = H1(1,:)';
H = H1(2:end,:);
Y = reshape_hsi(I);
X = reshape(I1, [N1, b]);

beta = parse_param(options,'beta',1);
% beta = max(beta, 1e-12);
%
% beta2 = parse_param(options,'beta2',0.1);
% tau = beta2/beta;

beta = beta * (b/B);
% options.tau = tau;

gamma = parse_param(options,'gamma',0.5);%weight
gamma = max(gamma,1e-4);
gamma = 1/(N*B*(1-gamma)/(N1*b*gamma) + 1);

%% Do fusion
disp('Start image fusion');

F = S*H;
X1 = X - ones(N1,1) * h0';
Z = gamma*G'*Y + (1-gamma)*X1*F';

options.regularization = regularization;
L = calc_regularization(reshape(X1,[rows1,cols1,b]), options);

options = insert_into_options(options,'g',g, ...
    'beta',beta,'gamma',gamma,'G',G,'Y',Y,'F',F,'X1',X1,'Z',Z, ...
    'extra',extra,'s',s,'L',L);

% R = optimize_gradient_descent(I, I1, options);
% R = fusion_with_couple_Local_LMM(I,I1,options);

if strcmp(regularization, 'LLMM')
    if 1 % reconstruct full reflectance data
        se_A = gamma*(G'*G) + beta*L;
        se_B = (1-gamma)*(F*F');
        se_C = Z;
        R = sylvester_krylov(se_A, se_B, se_C, sylvester_solver_mode);
    else % reconstruct abundances
        num_endm = vd(Y',5*10^-2); % M can be automatically defined, for example, by VD
        [~, indicies] = vca(Y', num_endm);
        M = Y(indicies,:);
        [U,Lambda] = eig(M*M');
        tmp = M*(1-gamma)*F*F'*M';
        [V,S] = eig((tmp + tmp')/2);
        Lambda1 = diag(sqrt(1./diag(Lambda)));
        se_A = gamma*(G'*G) + beta*L;
        se_B = Lambda1*U'*V*S*V'*U*Lambda1;
        se_B = (se_B + se_B) / 2;
        se_C = Z*M'*U*Lambda1;
        Z = sylvester_krylov(se_A, se_B, se_C, 'adaptive');
        A = Z*Lambda1*U';
        R = A*M;
    end
elseif strcmp(regularization, 'LMM')
    kappa = parse_param(options,'kappa',0.1);
    kappa = kappa * (b/B);
    options.kappa = kappa;

    R = fusion_with_LMM_constraint(I, I1, options);
end

%% pad bad bands
if isfield(options,'bbl') && ~all(options.bbl)
    R1 = nan(size(R,1),length(options.bbl));
    R1(:,options.bbl) = R;
    R = R1;
end

I2 = reshape(R, [rows1,cols1,size(R,2)]);

if pad_color
    I2 = remove_rgb_margin(I2, extra);
end

t_elapsed = toc(t_start);
disp(['Elapsed time for image fusion is ', num2str(t_elapsed)]);
end


function jed = ymdf_to_jed_alexandrian ( y, m, d, f )

%*****************************************************************************80
%
%% YMDF_TO_JED_ALEXANDRIAN converts an Alexandrian YMDF date to a JED.
%
%  Discussion:
%
%    This code needs to be adjusted to fit the Alexandrian model.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, M, D, real F, the YMDF date.
%
%    Output, real JED, the corresponding Julian Ephemeris Date.
%

%
%  Convert the calendar date to a computational date.
%
  y_prime = y + 4690 - floor ( ( 13 - m ) / 13 );
  m_prime = mod ( m + 12, 13 );
  d_prime = d - 1;
%
%  Convert the computational date to a JED.
%
  jed = floor ( ( 1461 * y_prime ) / 4 ) + 30 * m_prime + d_prime - 124 - 0.5;
  jed = jed + f;

  return
end


function  opt = FindOpt(model,Population,BU,BD)
% Find the minimum of the surrogate

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    [~,I]  = sort(Population.objs,'ascend');
    init   = 1;
    preObj = rbf_predict(model,Population.decs,Population(I(init)).decs);
    while isnan(preObj)
        init   = init + 1;
        preObj = rbf_predict(model,Population.decs,Population(I(init)).decs);
    end
    opt = fmincon(@(x)rbf_predict(model,Population.decs,x),Population(I(init)).decs,[],[],[],[],BD,BU,[],optimoptions('fmincon','Display','off'));
end


function straddling_graph(blind_time,frame_rate,pulse_distance,laser_energy,num_exposures_to_show,is_dbl_shutter,f1exp_cam)
if is_dbl_shutter % a double shutter camera like the pco panda and pixelfly capture 2 images for each frame.
	frame_rate=frame_rate*2;
end
period = 1/frame_rate*1000^2; % µs
cam_on = period-blind_time; % µs
pulse_length = laser_energy/100*pulse_distance; % µs
max_possible_pulse_distance = pulse_distance - (1/frame_rate*1000^2-cam_on); % µs
if pulse_length > max_possible_pulse_distance
	pulse_length = max_possible_pulse_distance;
end
duty_cycle=pulse_length/period;
if duty_cycle > 0.5
	pulse_length = period / 2;
	duty_cycle=pulse_length/period;
end

exposure_active_x=[];
exposure_active_y=[];
laser_active_x=[];
laser_active_y=[];

start_of_pulse1 = period-pulse_distance/2-pulse_length/2-blind_time/2;
end_of_pulse1 = period-pulse_distance/2-pulse_length/2-blind_time/2+pulse_length;

start_of_pulse2 = period-pulse_distance/2-pulse_length/2-blind_time/2+pulse_distance;
end_of_pulse2 = period-pulse_distance/2-pulse_length/2-blind_time/2+pulse_distance+pulse_length;

bot_cam = 0.45;
bot_las = 0.2;
amp = 0.2;

if is_dbl_shutter
	for i=0:2:num_exposures_to_show+4
		exposure_active_y= [exposure_active_y  [  bot_cam  bot_cam      bot_cam                            bot_cam+amp                    bot_cam+amp                        bot_cam                         bot_cam                      bot_cam+amp                 bot_cam+amp       bot_cam          ]];
		exposure_active_x =[exposure_active_x  [  0           0     start_of_pulse1-blind_time  start_of_pulse1-blind_time  start_of_pulse1+f1exp_cam-blind_time  start_of_pulse1+f1exp_cam-blind_time  start_of_pulse1+f1exp_cam-blind_time   start_of_pulse1+f1exp_cam-blind_time   end_of_pulse2+5000  end_of_pulse2+5000    ] +  period* i];
	end
else
	for i=0:num_exposures_to_show+4
		exposure_active_y=[exposure_active_y  [  bot_cam   bot_cam+amp       bot_cam+amp                      bot_cam               bot_cam    ]];
		exposure_active_x =[exposure_active_x [  0              0          period-blind_time            period-blind_time            period    ] +  period* i];
	end
end

for i=0:2:num_exposures_to_show+4
	laser_active_y= [laser_active_y  [  bot_las  bot_las      bot_las        bot_las+amp    bot_las+amp       bot_las         bot_las         bot_las+amp      bot_las+amp       bot_las          ]];
	laser_active_x =[laser_active_x  [  0           0     start_of_pulse1  start_of_pulse1  end_of_pulse1  end_of_pulse1  start_of_pulse2   start_of_pulse2   end_of_pulse2   end_of_pulse2    ] +  period* i];
end

straddling_figure=findobj('tag','straddling_figure');
if isempty(straddling_figure)
	hf = figure('numbertitle','off','MenuBar','figure','DockControls','off','Name','Camera exposure and pulse timing visualization','Toolbar','figure','CloseRequestFcn', @straddling_figure_CloseRequestFcn,'tag','straddling_figure','visible','on');
else
	hf = figure(straddling_figure);
	clf(hf)
end

plot(exposure_active_x,exposure_active_y,'linewidth',2)
hold on
plot(laser_active_x,laser_active_y,'linewidth',2)
axis tight
xlim([0 period*num_exposures_to_show])
%xlim([-period period*(num_cycles_to_show)+period])
ylim([0 1])
xlabel('time in µs')
ylabel([])
yticks([])

%remove unnecessary Toolbar stuff
% Get a handle to the standard plot toolbar.
tbh = findall(hf,'Type','uitoolbar');
% Get handles to each button we don't want on the standard toolbar and DELETE
ptPlotToolsOn    = findall(hf,'Tag','Plottools.PlottoolsOn');     delete(ptPlotToolsOn);
ptPlotToolsOff   = findall(hf,'Tag','Plottools.PlottoolsOff');    delete(ptPlotToolsOff);
ttInsertLegend   = findall(hf,'Tag','Annotation.InsertLegend');   delete(ttInsertLegend);
ttInsertColorbar = findall(hf,'Tag','Annotation.InsertColorbar'); delete(ttInsertColorbar);
ttLinking        = findall(hf,'Tag','DataManager.Linking');       delete(ttLinking);
ttRotate         = findall(hf,'Tag','Exploration.Rotate');        delete(ttRotate);
ttEditPlot       = findall(hf,'Tag','Standard.EditPlot');         delete(ttEditPlot);
ttEditPlot       = findall(hf,'Tag','Standard.PropertyInspector');         delete(ttEditPlot);
ptPrintFigure    = findall(hf,'Tag','Standard.PrintFigure');      delete(ptPrintFigure);
ptSaveFigure     = findall(hf,'Tag','Standard.SaveFigure');       delete(ptSaveFigure);
ptFileOpen       = findall(hf,'Tag','Standard.FileOpen');         delete(ptFileOpen);
ptNewFigure      = findall(hf,'Tag','Standard.NewFigure');        delete(ptNewFigure);

%Pulse_length
ha = annotation('doublearrow','Head1Style','plain','Head2Style','plain','Head1Length',5,'Head1Width',5,'Head2Length',5,'Head2Width',5);
ha.Parent=hf.CurrentAxes;
ha.X=[start_of_pulse1 end_of_pulse1];
ha.Y=[bot_las-0.01 bot_las-0.01];
plot ([start_of_pulse1 start_of_pulse1], [bot_las-0.01 bot_las],'LineStyle','--','Color',[1 0.7 0.7])
plot ([end_of_pulse1 end_of_pulse1], [bot_las-0.01 bot_las],'LineStyle','--','Color',[1 0.7 0.7])
text((start_of_pulse1+end_of_pulse1)/2,bot_las-0.01,{'Pulse length' [num2str(pulse_length) ' µs']},'Rotation',0,'HorizontalAlignment','center','VerticalAlignment','top','FontSize',8)

%Pulse distance
ha = annotation('doublearrow','Head1Style','plain','Head2Style','plain','Head1Length',5,'Head1Width',5,'Head2Length',5,'Head2Width',5);
ha.Parent=hf.CurrentAxes;
ha.X=[(start_of_pulse1+end_of_pulse1)/2 (start_of_pulse2+end_of_pulse2)/2];
ha.Y=[bot_las-0.1 bot_las-0.1];
plot ([(start_of_pulse1+end_of_pulse1)/2 (start_of_pulse1+end_of_pulse1)/2], [bot_las-0.1 bot_las+amp-0.01],'LineStyle','--','Color',[1 0.7 0.7])
plot ([(start_of_pulse2+end_of_pulse2)/2 (start_of_pulse2+end_of_pulse2)/2], [bot_las-0.1 bot_las+amp-0.01],'LineStyle','--','Color',[1 0.7 0.7])
text(((start_of_pulse1+end_of_pulse1)/2 + (start_of_pulse2+end_of_pulse2)/2)/2,bot_las-0.1,{'Pulse distance' [num2str(pulse_distance) ' µs']},'Rotation',0,'HorizontalAlignment','center','VerticalAlignment','top','FontSize',8)

%frame rate annotation
ha = annotation('doublearrow','Head1Style','plain','Head2Style','plain','Head1Length',5,'Head1Width',5,'Head2Length',5,'Head2Width',5);
ha.Parent=hf.CurrentAxes;
if is_dbl_shutter
	%frame rate double shutter cameras
	ha.X=[start_of_pulse1-blind_time  start_of_pulse1-blind_time+period*2];
	ha.Y=[bot_cam-0.01 bot_cam-0.01];
	plot ([start_of_pulse1-blind_time start_of_pulse1-blind_time], [bot_cam bot_cam-0.01],'LineStyle','--','Color',[0.7 0.7 1])
	plot ([start_of_pulse1-blind_time+period*2 start_of_pulse1-blind_time+period*2], [bot_cam bot_cam-0.01],'LineStyle','--','Color',[0.7 0.7 1])
	text(period*1.5,bot_cam-0.01,{'Double frame rate' [num2str(frame_rate/2) ' Hz'] [num2str(period*2) ' µs']},'Rotation',0,'HorizontalAlignment','center','VerticalAlignment','top','FontSize',8)
else
	%frame rate non-double shutter cameras
	ha.X=[period period*2];
	ha.Y=[bot_cam-0.01 bot_cam-0.01];
	plot ([period period], [bot_cam bot_cam-0.01],'LineStyle','--','Color',[0.7 0.7 1])
	plot ([period*2 period*2], [bot_cam bot_cam-0.01],'LineStyle','--','Color',[0.7 0.7 1])
	text(period*1.5,bot_cam-0.01,{'Frame rate' [num2str(frame_rate) ' Hz'] [num2str(period) ' µs']},'Rotation',0,'HorizontalAlignment','center','VerticalAlignment','top','FontSize',8)
end

%info top left
text(period/10,0.99,{['Laser duty cycle: ' num2str(round(duty_cycle*100,2)) ' %'] ['PIV data rate: ' num2str(frame_rate/2) ' Hz']},'HorizontalAlignment','left','VerticalAlignment','top')
%info top right
text(num_exposures_to_show*period,0.99,{' Use zoom buttons to see the details '},'HorizontalAlignment','right','VerticalAlignment','top')

margin=period/20*0;
smallmargin=margin/2*0;
for i=0:2:num_exposures_to_show-1

	ha=annotation('textbox',[0,0,0,0],'String',['Image Pair ' num2str(i/2+1)],'HorizontalAlignment','center','VerticalAlignment','top','BackgroundColor','k','FaceAlpha',0.1);
	ha.Parent=hf.CurrentAxes;
	ha.Position=[0+margin+  period* i           bot_cam+amp+0.04              period*2-margin*2                 0.175];

	ha=annotation('textbox',[0,0,0,0],'String',{'Image A' ['PIVlab_' sprintf('%3.3d',i/2) '_A.tif']},'HorizontalAlignment','center','VerticalAlignment','middle','BackgroundColor','k','FaceAlpha',0.1,'LineStyle','-','FontSize',8,'Interpreter','none');
	ha.Parent=hf.CurrentAxes;
	ha.Position=[0+margin+smallmargin + period* i              bot_cam+amp+0.05                period-margin*2-smallmargin*2               0.1];

	ha=annotation('textbox',[0,0,0,0],'String',{'Image B' ['PIVlab_' sprintf('%3.3d',i/2) '_B.tif']},'HorizontalAlignment','center','VerticalAlignment','middle','BackgroundColor','k','FaceAlpha',0.1,'LineStyle','-','FontSize',8,'Interpreter','none');
	ha.Parent=hf.CurrentAxes;
	ha.Position=[period+margin+smallmargin + period* i                    bot_cam+amp+0.05              period-margin*2-smallmargin*2              0.1];

end

legend({'Camera Exposure', 'Laser Active'},'Location','southeast')
set(gca,'InnerPosition',[0.01 0.1 0.98 0.9])
%grid on
%set(gca,'YGrid','off')
%set(gcf,'handlevisibility','callback')
end

function straddling_figure_CloseRequestFcn(hObject, ~, ~)
try
	delete(hObject);
catch
	delete(gcf);
end
end


function m = marginal_family(engine, n, add_ev)
% MARGINAL_FAMILY Compute the marginal on i's family (loopy)
% m = marginal_family(engine, n, add_ev)

if nargin < 3, add_ev = 0; end

bnet = bnet_from_engine(engine);
ns = bnet.node_sizes;
ps = parents(bnet.dag, n);
dom = [ps n];
CPD = bnet.CPD{bnet.equiv_class(n)};

switch engine.msg_type
  case 'd',
   % The method is similar to the following HMM equation:
   % xi(i,j,t) = normalise( alpha(i,t) * transmat(i,j) * obsmat(j,t+1) * beta(j,t+1) )
   % where xi(i,j,t) = Pr(Q(t)=i, Q(t+1)=j | y(1:T))
   % beta == lambda, alpha == pi, alpha from each parent = pi msg
   % In general, if A,B are parents of C,
   % P(A,B,C) = P(C|A,B) pi_msg(A->C) pi_msg(B->C) lambda(C)
   % where lambda(C) = P(ev below and including C|C) = prod incoming lamba_msg(children->C)
   % and pi_msg(X->C) = P(X|ev above) etc

   T = dpot(dom, ns(dom), CPD_to_CPT(CPD));
   for j=1:length(ps)
     p = ps(j);
     pi_msg = dpot(p, ns(p), engine.msg{n}.pi_from_parent{j});
     T = multiply_by_pot(T, pi_msg);
   end
   lambda = dpot(n, ns(n), engine.msg{n}.lambda);
   T = multiply_by_pot(T, lambda);
   T = normalize_pot(T);
   m = pot_to_marginal(T);
   if ~add_ev
     m.T = shrink_obs_dims_in_table(m.T, dom, engine.evidence);
   end
 case 'g',
  if engine.disconnected_nodes_bitv(n)
    m.T = 1;
    m.domain = dom;
    if add_ev
      m = add_ev_to_dmarginal(m, engine.evidence, ns)
    end
    return;
  end

  [m, C, W] = gaussian_CPD_params_given_dps(CPD, dom, engine.evidence);
  cdom = myintersect(dom, bnet.cnodes);
  pot = linear_gaussian_to_cpot(m, C, W, dom, ns, cdom, engine.evidence);
  % linear_gaussian_to_cpot will set the effective size of observed nodes to 0,
  % so we need to do this explicitely for the messages, too,
  % so they are all the same size.
  obs_bitv = ~isemptycell(engine.evidence);
  ps = parents(engine.msg_dag, n);
  for j=1:length(ps)
    p = ps(j);
    msg = engine.msg{n}.pi_from_parent{j};
    if obs_bitv(p)
      pi_msg = mpot(p, 0);
    else
      pi_msg = mpot(p, ns(p), 0, msg.mu, msg.Sigma);
    end
    pot = multiply_by_pot(pot, mpot_to_cpot(pi_msg));
  end
  msg = engine.msg{n}.lambda;
  if obs_bitv(n)
    lambda = cpot(n, 0);
  else
    lambda = cpot(n, ns(n), 0, msg.info_state, msg.precision);
  end
  pot = multiply_by_pot(pot, lambda);
  m = pot_to_marginal(pot);
  if add_ev
    m = add_evidence_to_gmarginal(m, engine.evidence, bnet.node_sizes, bnet.cnodes);
  end
end


% 5g ldpc encoding and decoding test
% reference: 3GPP TS 38.212 section 5.3.2
% author: Xiao, Shaoning 萧少宁
% license: MIT

function pass_flag = test_ldpc(varargin)

if nargin == 0
    base_graph_index = 1;
    Z_c = 384;
elseif nargin == 1
    base_graph_index = varargin{1};
    Z_c = 384;
elseif nargin == 2
    base_graph_index = varargin{1};
    Z_c = varargin{2};
end

SNR_list = 10;

trial_number = 1;

BLER = zeros(1, length(SNR_list));

TxRx.Decoder.LDPC.Iterations = 30;
TxRx.Decoder.LDPC.Type = 'OMS';
% TxRx.Decoder.LDPC.Type = 'MPA';

if base_graph_index == 1
    load base_graph_1_check_node_list
    base_graph_check_node_list = base_graph_1_check_node_list;
    LDPC.inf_bits = 22 * Z_c;
elseif base_graph_index == 2
    load base_graph_2_check_node_list
    base_graph_check_node_list = base_graph_2_check_node_list;
    LDPC.inf_bits = 10 * Z_c;
else
    error('wrong base graph index.');
end

for SNR_list_index = 1:length(SNR_list)

    sigma_square = 10^(-SNR_list(SNR_list_index)/10);

    for trial = 1:trial_number

        tx_bits = randi([0, 1], LDPC.inf_bits, 1);

        %         [encoded_bits, LDPC.H, Z_c] = ldpc_encode(tx_bits, base_graph_index);
        [encoded_bits, LDPC.H, Z_c] = ldpc_encode_optimized(tx_bits, base_graph_index);

        [LDPC.par_bits, LDPC.tot_bits] = size(LDPC.H);

        symbols = 1 - 2 * encoded_bits;

        noise = randn(size(symbols));

        waveform = symbols + noise * sqrt(sigma_square);

        LLR_received = 2 * waveform / sigma_square;

        rx_bits = decLDPC_layered(TxRx, LDPC, LLR_received.', base_graph_check_node_list, Z_c);

%         f0=1./(1+exp(-2*LLR_received));
%         f1 = 1 - f0;
%         f = [f0, f1].';
%         rx_bits_prime = ldpc_decode(f0,f1, full(LDPC.H), TxRx.Decoder.LDPC.Iterations);
%
%         ldpc_dec = comm.LDPCDecoder(LDPC.H);
%         LLR_received_extended = [zeros(2*Z_c, 1); LLR_received];
%         decoded_bits = step(ldpc_dec, LLR_received_extended);
%
%         [rx_bits_prime, success, k] = ldpc_decode_1(f, full(LDPC.H), 2);
        rx_bits = rx_bits(:);

        if ~isequal(tx_bits, rx_bits)
            BLER(SNR_list_index) = BLER(SNR_list_index) + 1;
        end

%         if ~isequal(rx_bits_prime(1:LDPC.inf_bits), tx_bits)
%             BLER(SNR_list_index) = BLER(SNR_list_index) + 1;
%         end
%
%         if ~isequal(double(decoded_bits), tx_bits)
%             BLER(SNR_list_index) = BLER(SNR_list_index) + 1;
%         end

    end

end

fprintf('BLER is %f\n', BLER/trial_number);

if BLER < 1e-10
    pass_flag = true;
else
    pass_flag = false;
end

end


function acq = acqfsn2_vbmc(Xs,vp,gp,optimState,fmu,fs2,fbar,vtot)
%ACQFSN2_VBMC Acquisition fcn. for noisy prospective uncertainty search.

% Xs is in *transformed* coordinates

% Probability density of variational posterior at test points
p = max(vbmc_pdf(vp,Xs,0),realmin);

% Estimate observation noise at test points from nearest neighbor
[~,pos] = min(sq_dist(bsxfun(@rdivide,Xs,optimState.gplengthscale),gp.X_rescaled),[],2);
sn2 = gp.sn2new(pos);

z = optimState.ymax;

% Prospective uncertainty search corrected for noisy observations
acq = -vtot.*(1 - sn2./(vtot+sn2)) .* exp(fbar-z) .* p;

end

%SQ_DIST Compute matrix of all pairwise squared distances between two sets
% of vectors, stored in the columns of the two matrices, a (of size n-by-D)
% and b (of size m-by-D).
function C = sq_dist(a,b)

n = size(a,1);
m = size(b,1);
mu = (m/(n+m))*mean(b,1) + (n/(n+m))*mean(a,1);
a = bsxfun(@minus,a,mu); b = bsxfun(@minus,b,mu);
C = bsxfun(@plus,sum(a.*a,2),bsxfun(@minus,sum(b.*b,2)',2*a*b'));
C = max(C,0);

end


function c = tapas_condhalluc_obs2_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the response model used to analyze data from conditioned
% hallucination paradigm by Powers & Corlett
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The rationale for this model is as follows:
%
% TO BE DESCRIBED...
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2016 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'tapas_condhalluc_obs2';

% Sufficient statistics of Gaussian parameter priors

% Beta
c.logbemu = log(48);
c.logbesa = 1;

% Nu
c.lognumu = log(1);
c.lognusa = 1;

% Gather prior settings in vectors
c.priormus = [
    c.logbemu,...
    c.lognumu,...
         ];

c.priorsas = [
    c.logbesa,...
    c.lognusa,...
         ];

% Model filehandle
c.obs_fun = @tapas_condhalluc_obs2;

% Handle to function that transforms observation parameters to their native space
% from the space they are estimated in
c.transp_obs_fun = @tapas_condhalluc_obs2_transp;

return;


function [m,v,w]=gaussmixt(m1,v1,w1,m2,v2,w2)
%GAUSSMIXT Multiply two GMM pdfs
%
% Inputs: Input mixtures: k1,k2 mixtures, p dimensions
%
%   M(k1,p) = mixture means for mixture 1
%   V(k1,p) or V(p,p,k1) variances (diagonal or full)
%   W(k1,1) = mixture weights
%   M(k2,p) = mixture means for mixture 2
%   V(k2,p) or V(p,p,k2) variances (diagonal or full)
%   W(k2,1) = mixture weights
%
% Outputs:
%
%   M(k1*k2,p) = mixture means
%   V(k1*k2,p) or V(p,p,k1*k2) if p>1 and at least one input has full covariance matrix
%   W(k1*k2,1) = mixture weights
%
% See also: gaussmix, gaussmixg, gaussmixp, randvec

%      Copyright (C) Mike Brookes 2000-2012
%      Version: $Id: gaussmixt.m 5453 2014-11-19 13:10:51Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 2 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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent r13 r21 r22 r31 r312 r112 r1223 r321 ch1h r122 r124
if isempty(r21)
    r13=[1 3];
    r21=[2 1];
    r22=[2 2];
    r31=[3 1];
    r112=[1 1 2];
    r122=[1 2 2];
    r124=[1 2 4];
    r312=[3 1 2];
    r321=[3 2 1];
    r1223=[1 2 2 3];
    ch1h=[-0.5; 1; -0.5];
end
[k1,p]=size(m1);
[k2,p2]=size(m2);
f1=ndims(v1)>2 || size(v1,1)>k1; % full covariance matrix is supplied
f2=ndims(v2)>2 || size(v2,1)>k2; % full covariance matrix is supplied
% ff=f1+2*f2;
if p~=p2
    error('mixtures must have the same vector dimension');
end
k=k1*k2;
j1=repmat((1:k1)',k2,1);
j2=reshape(repmat(1:k2,k1,1),k,1);
if p==1
    % display('1D vectors');
    p1=1./v1(:);
    p2=1./v2(:);
    v=1./(p1(j1)+p2(j2));
    s1=p1.*m1;
    s2=p2.*m2;
    m=v.*(s1(j1)+s2(j2));
    v12=v1(j1)+v2(j2);
    wx=-0.5*(m1(j1)-m2(j2)).^2./v12(:);
    wx=wx-max(wx); % normalize to avoid underflow
    w=w1(j1).*w2(j2).*exp(wx)./sqrt(v12(:));
    w=w/sum(w);
else
    if ~f1 && ~f2 % both diagonal covariances
        % display('both diagonal');
        p1=1./v1;
        p2=1./v2;
        v=1./(p1(j1,:)+p2(j2,:));
        s1=p1.*m1;
        s2=p2.*m2;
        m=v.*(s1(j1,:)+s2(j2,:));
        v12=v1(j1,:)+v2(j2,:);
        wx=-0.5*sum((m1(j1,:)-m2(j2,:)).^2./v12,2);
        wx=wx-max(wx); % normalize to avoid underflow
        w=w1(j1).*w2(j2).*exp(wx)./sqrt(prod(v12,2));
        w=w/sum(w);
    else % at least one full covariances
        m=zeros(k,p);
        v=zeros(p,p,k);
        w=zeros(k,1);
        wx=w;
        idp=1:p+1:p*p; % diagonal elements of p x p matrix
        if p==2                 % special code for 2D vectors
            if ~f2  % GMM 2 is diagonal
                % display('2D GMM 2 diagonal');
                p2=1./v2;
                pm2=p2.*m2;
                vx1=permute(v1,r312);
                vx1=vx1(:,r124);
                px1=vx1./repmat((vx1(:,1).*vx1(:,3)-vx1(:,2).^2),1,3); % [a b; b c] -> [c -b a]
                pm1=m1.*px1(:,r31)-m1(:,r21).*px1(:,r22);
                px=px1(j1,:);
                px(:,r31)=px(:,r31)+p2(j2,:);  % add onto diagonal elements
                vijx=vx1(j1,:);
                vijx(:,r13)=vijx(:,r13)+v2(j2,:);  % add onto diagonal elements
            elseif ~f1 % GMM 1 is diagonal
                % display('2D GMM 1 diagonal');
                p1=1./v1;
                pm1=p1.*m1;
                vx2=permute(v2,r312);
                vx2=vx2(:,r124);
                px2=vx2./repmat((vx2(:,1).*vx2(:,3)-vx2(:,2).^2),1,3); % [a b; b c] -> [c -b a]
                pm2=m2.*px2(:,r31)-m2(:,r21).*px2(:,r22);
                px=px2(j2,:);
                px(:,r31)=px(:,r31)+p1(j1,:);  % add onto diagonal elements
                vijx=vx2(j2,:);
                vijx(:,r13)=vijx(:,r13)+v1(j1,:);  % add onto diagonal elements
            else % both full covariances
                % display('2D both full');
                vx1=permute(v1,r312);
                vx1=vx1(:,r124); % make each 2 x 2 matrix into a row [a b; b c] -> [a b c]
                px1=vx1./repmat((vx1(:,1).*vx1(:,3)-vx1(:,2).^2),1,3); % [a b; b c] -> [c -b a]
                vx2=permute(v2,r312);
                vx2=vx2(:,r124);
                px2=vx2./repmat((vx2(:,1).*vx2(:,3)-vx2(:,2).^2),1,3); % [a b; b c] -> [c -b a]
                pm1=m1.*px1(:,r31)-m1(:,r21).*px1(:,r22);
                pm2=m2.*px2(:,r31)-m2(:,r21).*px2(:,r22);
                px=px1(j1,:)+px2(j2,:);
                vijx=vx1(j1,:)+vx2(j2,:);
            end
            vx=px./repmat((px(:,1).*px(:,3)-px(:,2).^2),1,3);   % divide by determinant to get inverse
            m=pm1(j1,:)+pm2(j2,:);
            m=m.*vx(:,r13)+m(:,r21).*vx(:,r22);                 % multiple by 2 x 2 matrix vx
            v=reshape(vx(:,r1223)',[2 2 k]);                    % convert vx to a 3D array of 2 x 2 matrices
            m12=m1(j1,:)-m2(j2,:);                              % subtract means to calculate weight exponent
            dij=vijx(:,1).*vijx(:,3)-vijx(:,2).^2;              % determinant of V1+V2
            wx=m12(:,r112).*m12(:,r122).*vijx(:,r321)*ch1h./dij;% exponent of weight
            w=w1(j1).*w2(j2)./sqrt(dij);                        % weight is w*exp(wx)
        else
            if ~f2  % GMM 2 is diagonal
                % display('GMM 2 diagonal');
                p2=1./v2;
                pm2=p2.*m2;
                for i=1:k1
                    v1i=v1(:,:,i);
                    p1i=inv(v1i);
                    m1i=m1(i,:);
                    pm1i=m1i*p1i;
                    w1i=w1(i);
                    ix=i;
                    for j=1:k2
                        pij=p1i;
                        pij(idp)=pij(idp)+p2(j,:);
                        vix=inv(pij);
                        vij=v1i;
                        vij(idp)=vij(idp)+v2(j,:);
                        v(:,:,ix)=vix;
                        m(ix,:)=(pm2(j,:)+pm1i)*vix;
                        m12=m2(j,:)-m1i;
                        wx(ix)=-0.5*m12/vij*m12';           % exponent of weight
                        w(ix)=w2(j)*w1i/sqrt(det(vij));     % weight is w*exp(wx)
                        ix=ix+k1;
                    end
                end
            elseif ~f1 % GMM 1 is diagonal
                % display('GMM 1 diagonal');
                p1=1./v1;
                pm1=p1.*m1;
                ix=1;
                for j=1:k2
                    v2j=v2(:,:,j);
                    p2j=inv(v2j);
                    m2j=m2(j,:);
                    pm2j=m2j*p2j;
                    w2j=w2(j);
                    for i=1:k1
                        pij=p2j;
                        pij(idp)=pij(idp)+p1(i,:);
                        vix=inv(pij);
                        vij=v2j;
                        vij(idp)=vij(idp)+v1(i,:);
                        v(:,:,ix)=vix;
                        m(ix,:)=(pm1(i,:)+pm2j)*vix;
                        m12=m1(i,:)-m2j;
                        wx(ix)=-0.5*m12/vij*m12';           % exponent of weight
                        w(ix)=w1(i)*w2j/sqrt(det(vij));     % weight is w*exp(wx)
                        ix=ix+1;
                    end
                end
            else % both full covariances
                % display('both full');
                p1=zeros(p,p,k1);
                pm1=zeros(k1,p);
                for i=1:k1
                    p1i=inv(v1(:,:,i));
                    p1(:,:,i)=p1i;
                    pm1(i,:)=m1(i,:)*p1i;
                end
                ix=1;
                for j=1:k2
                    v2j=v2(:,:,j);
                    p2j=inv(v2j);
                    m2j=m2(j,:);
                    pm2j=m2j*p2j;
                    w2j=w2(j);
                    for i=1:k1
                        pij=p1(:,:,i)+p2j;
                        vix=inv(pij);
                        v(:,:,ix)=vix;
                        vij=v1(:,:,i)+v2j;
                        m(ix,:)=(pm1(i,:)+pm2j)*vix;
                        m12=m1(i,:)-m2j;
                        wx(ix)=-0.5*m12/vij*m12';           % exponent of weight
                        w(ix)=w1(i)*w2j/sqrt(det(vij));     % weight is w*exp(wx)
                        ix=ix+1;
                    end
                end
            end

        end
        wx=wx-max(wx);              % adjust exponents to avoid underflow
        w=w.*exp(wx);               % calculate weights
        w=w/sum(w);                 % normalize weights to sum to unity
        if k==1
            v=reshape(v,size(v,1),size(v,2)); % squeeze last dimension of v if possible
        end
    end
end
if ~nargout
    if p==1
        nxx=256; % number of points to plot
        nsd=3; % number of std deviations
        sd=sqrt([v1(:);v2(:);v]);
        ma=[m1;m2;m];
        xax=linspace(min(ma-nsd*sd),max(ma+nsd*sd),nxx);
        plot(xax,gaussmixp(xax(:),m1,v1,w1),'--b');
        hold on
        plot(xax,gaussmixp(xax(:),m2,v2,w2),':r');
        plot(xax,gaussmixp(xax(:),m,v,w),'-k');
        hold off
        ylabel('Log probability density');
        legend('Mix 1','Mix 2','Product','location','best');
        axisenlarge([-1 -1 -1 -1.05]);
    elseif p==2
        nxx=128; % number of points to plot
        nsd=3;
        if f1
            s1=sqrt([v1(1:4:end)' v1(4:4:end)']); % extract diagonal elements only
        else
            s1=sqrt(v1);
        end
        if f2
            s2=sqrt([v2(1:4:end)' v2(4:4:end)']); % extract diagonal elements only
        else
            s2=sqrt(v2);
        end
        if ndims(v)>2 || size(v,1)>k
            s3=sqrt([v(1:4:end)' v(4:4:end)']); % extract diagonal elements only
        else
            s3=sqrt(v);
        end
        mal=[m1;m2;m];
        sal=[s1;s2;s3];
        xax=linspace(min(mal(:,1)-nsd*sal(:,1)),max(mal(:,1)+nsd*sal(:,1)),nxx);
        yax=linspace(min(mal(:,2)-nsd*sal(:,2)),max(mal(:,2)+nsd*sal(:,2)),nxx);
        xx(:,:,1)=repmat(xax',1,nxx);
        xx(:,:,2)=repmat(yax,nxx,1);
        xx=reshape(xx,nxx^2,2);
        subplot(2,2,1);
        imagesc(xax,yax,reshape(gaussmixp(xx,m1,v1,w1),nxx,nxx)');
        axis 'xy';
        title('Input Mix 1');
        subplot(2,2,2);
        imagesc(xax,yax,reshape(gaussmixp(xx,m2,v2,w2),nxx,nxx)');
        axis 'xy';
        title('Input Mix 2');
        subplot(2,2,3);
        imagesc(xax,yax,reshape(gaussmixp(xx,m,v,w),nxx,nxx)');
        axis 'xy';
        title('Product GMM');
    end
end


function vox=tal2vox(tal,VOL)
% converts from talairach coordinate to voxel coordinate
% based on variables from SPM.M (passed here for
% faster operation)
%
% :Example:
% ::
%
%    foo = tal2vox([-30 28 -30], VOL)

if(isfield(VOL, 'M'))
    M = VOL.M;
elseif(isfield(VOL, 'mat'))
    M = VOL.mat;
else
    error('Error in %s: VOL does not have an "M" or a "mat" field.', mfilename);
end

vox=[0 0 0];
vox(1)=(tal(1)-M(1,4))/M(1,1);
vox(2)=(tal(2)-M(2,4))/M(2,2);
vox(3)=(tal(3)-M(3,4))/M(3,3);

return


function value = f2 ( x )

%*****************************************************************************80
%
%% F2 evaluates a function that can be used for Chebyshev interpolation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 September 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X(), one or more points where the function is to be evaluated.
%
%    Output, real VALUE(), the function values.
%
  value = exp ( x );

  return
end


function [bodyX, bodyY, bodyZ, R_total] = computeBodyAxesFromEuler(ut, rVect, vVect, bodyInfo, rollAng, pitchAng, yawAng)
    R_body_2_ned = eul2rotmARH([yawAng,pitchAng,rollAng],'zyx');
    [R_ned_2_inert, ~, ~, ~] = computeNedFrame(ut, rVect, bodyInfo);
	R_total = R_ned_2_inert * R_body_2_ned;

    bodyX = R_total(:,1);
    bodyY = R_total(:,2);
    bodyZ = R_total(:,3);
end


% Build and initialize the computational graph for regression based speech
% enhancement/dereverberation
%
function [layer, para] = Build_EnhanceNet_Masking(Data_tr, para)
para.output = 'tmp';

layer = genNetworkDereverb_Masking(para.topology);     % generate the network graph
para.preprocessing{1} = {};                     % optional preprocessing for each data stream
para.preprocessing{2} = {};
para.cost_func.layer_idx = length(layer);       % specify which layers are cost function layers
para.cost_func.layer_weight = [1];              % set the weights of each cost function layer
para.IO.inputFeature = [1 1];
para.IO.isTensor = [1 1];
para = ParseOptions2(para);

% generating the scaling factor for the input, as we will need to use a
% small constant in the logarithm. We need to make sure that the power of
% speech are larger than this constant most of the time.
scale = 1e4;        % we hard code the scale to be a constant so that all network will use the same number
scale = scale/2^16; % note that we are using int16 to store waveform samples, so need to scale down
if para.topology.useWav
    layer = InitWavScaleLayer(layer, scale);
end

if strcmpi(para.topology.RegressionNetType, 'DNN')    % if use DNN, splice the frames
    idx = ReturnLayerIdxByName(layer, 'splice');
else
    idx = ReturnLayerIdxByName(layer, 'delta'); % if use LSTM, use dynamic features
end
fft_net_length = idx(1);
fft_net = layer(1:fft_net_length);
paraTmp = para;
paraTmp.out_layer_idx = fft_net_length;
paraTmp.IO = RemoveIOStream(paraTmp.IO, [2]);
paraTmp.IO.nStream = 1;
paraTmp.IO.inputFeature = paraTmp.IO.DynamicDistortion.inputFeature(1);
if isfield(paraTmp.IO, 'fileReader'); paraTmp.IO = rmfield(paraTmp.IO, 'fileReader'); end
paraTmp.IO.fileReader(1)= paraTmp.IO.DynamicDistortion.fileReader(1);
fprintf('Generate global MVN weights for mask subnet - %s\n', datestr(now));
[layer{fft_net_length+1}.W, layer{fft_net_length+1}.b] = computeGlobalCMVN(Data_tr(1), 100, paraTmp, fft_net);
VerifyPreprocessingTree(layer(1:fft_net_length+1), Data_tr(1), paraTmp, 100);

% set weight of static, velocity, and accelration features in the MSE cost
% function.
delta_idx = ReturnLayerIdxByName(layer, 'delta');
weight_idx = delta_idx+1;
for i=1:length(weight_idx)
    if isfield(layer{weight_idx(i)}, 'W') && numel(layer{weight_idx(i)}.W) == prod(layer{weight_idx(i)}.dim)
        continue;
    end
    layer{weight_idx(i)}.W = diag([ones(para.topology.nFreqBin,1)*para.topology.MSECostWeightSDA(1); ...
        ones(para.topology.nFreqBin,1)*para.topology.MSECostWeightSDA(2); ...
        ones(para.topology.nFreqBin,1)*para.topology.MSECostWeightSDA(3)]);
    layer{weight_idx(i)}.b = zeros(para.topology.nFreqBin*3,1);
end

end


function Omega = logm(ori,varargin)
% the logarithmic map that translates a rotation into a spin tensor
%
% Syntax
%   Omega = logm(mori) % spin tensor of a misorientation
%
%   Omega = logm(ori,ori_ref) % spin tensor in crystal coordinates
%   Omega = logm(ori,ori_ref,'left') % spin tensor in specimen coordinates
%
% Input
%  mori - misorientation
%  ori - @orientation
%  ori_ref - @orientation
%
% Output
%  Omega - @spinTensor
%
% See also
% orientation/log spinTensor/exp

m = log(ori,varargin{:});
Omega = spinTensor(m);

%T = logm@quaternion(ori,varargin{:});

%if check_option(varargin,'left')
%  T.CS = ori.SS;
%else
%  T.CS = ori.CS;
%end

end


function [rmse, primal_objs, dual_objs] = run_mbsdca(Xtrain, Ytrain, Xtest, Ytest, lambda, opts)
% Mini-batch SDCA
% Inputs
%   Xtrain: input training data
%   Ytrain: output training data
%   Xtest: input test data
%   Ytest: output test data
%   lambda: regularization parameter
%   opts: optional arguments
% Output
%   Average RMSE across tasks, primal and dual objectives

%% intialize variables
fprintf('Running Mb-SDCA\n');
m = length(Xtrain); % # of tasks
d = size(Xtrain{1}, 2); % # of features
W = zeros(d, m); alpha = cell(m,0);
Sigma = eye(m) * (1/m);
Omega = inv(Sigma);
totaln = 0; n = zeros(m, 1);
for t = 1:m
    n(t) = length(Ytrain{t});
    totaln = totaln + n(t);
    alpha{t} = zeros(n(t), 1);
end

%% intialize counters
rmse = zeros(opts.mbsdca_inner_iters, 1);
dual_objs = zeros(opts.mbsdca_inner_iters, 1);
primal_objs = zeros(opts.mbsdca_inner_iters, 1);

for h = 1:opts.mbsdca_outer_iters
    % update W
    for hh = 1:opts.mbsdca_inner_iters
        rng(hh * 1000);
        if(opts.sys_het)
            sys_iters = (opts.top - opts.bottom) .* rand(m,1) + opts.bottom;
        end

        % compute RMSE
        rmse(hh) = compute_rmse(Xtest, Ytest, W, opts);
        primal_objs(hh) = compute_primal(Xtrain, Ytrain, W, Omega, lambda);
        dual_objs(hh) = compute_dual(alpha, Ytrain, W, Omega, lambda);

        % loop over tasks (in parallel)
        deltaB = zeros(d, m);
        alpha_prev = alpha;
        local_iters = cell(m, 0);
        for t = 1:m
            tperm = randperm(n(t));
            alpha_t = alpha{t};
            curr_sig = Sigma(t,t);
            if(opts.sys_het)
                local_iters{t} = n(t) * sys_iters(t);
            else
                local_iters{t} = n(t) * opts.mocha_sdca_frac;
            end

            % run SDCA locally
            for s=1:local_iters{t}
                % select random coordinate
                idx = tperm(mod(s, n(t)) + 1);
                alpha_old = alpha_t(idx);
                curr_y = Ytrain{t}(idx);
                curr_x = Xtrain{t}(idx, :);

                % compute update
                update = (curr_y * curr_x * W(:,t));
                grad = lambda * n(t) * (1.0 - update) / (curr_sig * (curr_x * curr_x')) + (alpha_old * curr_y);
                alpha_t(idx) = curr_y * max(0.0, min(1.0, grad));
                deltaB(:, t) = deltaB(:, t) + (alpha_t(idx) - alpha_old) * curr_x' / n(t);
                alpha{t} = alpha_t;
            end
        end

        scaling = opts.mbsdca_scaling / sum(cell2mat(local_iters(:)));
        % combine updates globally
        for t = 1:m
            alpha{t} = alpha_prev{t} + scaling * (alpha{t} - alpha_prev{t});
            for tt = 1:m
                W(:, t) = W(:, t) + scaling * deltaB(:, tt) * Sigma(t, tt) * (1.0 / lambda);
            end
        end

    end

    %% make sure eigenvalues are positive
    A = W'*W;
    if(any(eig(A) < 0))
        [V,Dmat] = eig(A);
        dm= diag(Dmat);
        dm(dm <= 1e-7) = 1e-7;
        D_c = diag(dm);
        A = V*D_c*V';
    end

    %% update Omega, Sigma
    sqm = sqrtm(A);
    Sigma = sqm / trace(sqm);
    Omega = inv(Sigma);

end

end


% NTF | betaNTF | Simple beta-NTF implementation (Antoine Liutkus, 2012)
% process_video('NTF', 'betaNTF', 'dataset/demo.avi', 'output/demo_beta-NTF.avi');

% Compute a simple NTF model of 10 components
A = double(T);
r = 10;
[~,~,~,L] = betaNTF(A,r);
S = (A - L);
% For reconstruction
% for i = 1:r, B_hat(:,:,i) = W * diag(Q(i,:)) * H'; end


clear all; close all; clc;

[filename1,pathname]=uigetfile('*.*','select the image');
image1=imread(num2str(filename1));
figure(1);
imshow(image1);	title('original image');     % orginal image for watermarking
image1=double(image1);

[row,col]=size(image1);

imagew=imread('dmg2.tif');

[marked]=blockdwt2(image1,imagew);  % generates the watermarked image

markedmax = max(marked(:));
markscale = marked/markedmax*255;

figure(2);
colormap(gray(256));
image(marked);							% shows the watermarked image
title('Watermarked image');
imwrite(marked,gray(256),'marked_image.bmp');	% saves the watermarked image as a bmp file
figure(3);
watermark=image1-marked;			% image adaptive watermark
watermark=watermark*255/max(watermark(:));

for i = 1:row                       % thresholding
     for j = 1:col
        if watermark(i,j) > 70
            watermark(i,j) = 255;
        end
        if watermark(i,j) < 70
            watermark (i,j) = 0;
        end
    end
end

colormap(gray(256));
image(watermark);						% shows the image adaptive watermark
title('watermark');
imwrite(marked,gray(256),'watermark.bmp');		% saves the image adaptive watermark as a bmp file


function p =  mcmcProposalProbability2(pE, adjlist, pWhole, smap1, origseg, smap2, newseg, niter)
% compute probability of proposing newseg from smap1
% 1) Randomly select superpixel s1, then randomly selects different superpixel
% s2 within same segment (if one exists); remove s1,s2 from s
% 2) Then, for randomly ordered i:
%     if si is adjacent to s1, assign si to s1 with probability pE(si, s1)
%     if si is adjacent to s2, assign si to s2 with probability pE(si, s2)
%     remove si from s
% 3) Repeat (2) until s is empty

nsp = numel(smap1);

ind1 = find(smap1==origseg);
ind2 = find(smap2==newseg);
n1 = numel(ind1);
n2 = numel(ind2);
if n1 == n2 % entire original segment selected
    if n1==1
        p = 1/nsp;
        disp('one')
    else
        p = pWhole*n2/nsp;
        disp('whole')
    end
else % subset of segment selected
    disp('subset')
    p = (1-pWhole)*n2/nsp;

    adjmat = zeros(n1, n1);
    for k = 1:size(adjlist, 1)
        if smap1(adjlist(k, 1))==origseg && smap1(adjlist(k, 2))==origseg
            i1 = logical(ind1==adjlist(k, 1));
            i2 = logical(ind1==adjlist(k, 2));
            adjmat(i1, i2) = k;
            adjmat(i2, i1) = k;
        end
    end

    assntrue = 2*ones(n1, 1);
    for k = 1:n1
        if any(ind2==ind1(k))
            assntrue(k) = 1;
        end
        adj{k} = find(adjmat(k, :));
        adjlog{k} = logical(adjmat(k, :)>0);
    end
    notselectedind = find(assntrue==2);

    p = p * (n1-n2) / (n1-1); % prob of selecting valid starting point for s2

    cpos = 0;
    cneg = 0;
    assn = zeros(n1, 1);
    for k = 1:niter
        s1 = ind2(ceil(rand(1)*n2));
        ts1 = find(ind1==s1);

        ts2 = notselectedind(ceil(rand(1)*numel(notselectedind)));

        if assntrue(ts2)~=2
            cneg = cneg+1;
        else
            assn(:)=0;
            assn(ts1)=1;
            assn(ts2)=2;
            rind = randperm(n1);
            rind([ts1 ts2]) = [];
            c = 0;
            while ~isempty(rind)
                for t = 1:numel(rind)
                    r = rind(t);
                    if ~assn(r)
                        assnr = assn(adjlog{r});
                    end
                    if ~assn(r) && sum(assnr)>0
                        assnr1 = logical(assnr==1);
                        assnr2 = logical(assnr==2);
                        isadj1 = any(assnr1);
                        isadj2 = any(assnr2);
                        if isadj1 && ~isadj2
                            adj1 = adj{r}(assnr1);
                            p = 1-prod(1-pE(adjmat(r, adj1)));
                            if rand(1) < p
                                assn(r) = 1;
                            end
                        elseif isadj2 && ~isadj1
                            adj2 = adj{r}(assnr2);
                            p = 1-prod(1-pE(adjmat(r, adj2)));
                            if rand(1) < p
                                assn(r) = 2;
                            end
                        else
                            adj1 = adj{r}(assnr1);
                            adj2 = adj{r}(assnr2);
                            p1 = 1-prod(1-pE(adjmat(r, adj1)));
                            p2 = 1-prod(1-pE(adjmat(r, adj2)));
                            p1 = p1 / (p1 + p2);
                            if rand(1) < p1
                                assn(r) = 1;
                            else
                                assn(r) = 2;
                            end
                        end
                        if (assn(r)==1 && assntrue(r)==2) || (assn(r)==2 && assntrue(r)==1)
                            assn(logical(assn==0)) = 3;
                            break;
                        end
                    end % end: if unassigned and has assigned adjacent sp
                end % end: loop through randomly ordered indices
                rind = rind(logical(assn(rind)==0));
            end % end: loop until all assigned while any(~assn)

            if any(assn~=assntrue)
                cneg = cneg + 1;
            else
                cpos = cpos + 1;
            end
        end % end: if ts2 matches profile
    end % end: loop over iterations

    p = p * max(cpos/(cpos+cneg), 1/niter);

end % compute prob for subset


function C = competitiveHebb2(C,bmu,secbmu,y,Theta,alpha)
% This function applies type 2 Competitive Hebbian Learning,
% as described by relation (2.4) of reference [1].
% [1] Bruske J., Sommer G., "Dynamic Cell Structure Learns Perfectly Topology Preserving Map",
%                                                Neural Computation, vol. 7, Issue 4, July 1995, pp. 845-865.
c =  max(y(bmu)*y(secbmu),C(bmu,secbmu));

% Reduce all connections by multiplying by alpha:
C = alpha*C;

% Find those connections with: 0<strength<Theta:
[rows cols] = find(C<Theta & C>0);
... and erase them!
NumZeroConnections = length(rows);
for i=1:NumZeroConnections
     C(rows(i),cols(i)) = 0;
end

C(bmu,secbmu) = c;
C(secbmu,bmu) =  C(bmu,secbmu);


function [result_tab, error_code, nb_warm_start, z]=SparseLP_old(c,polyhedron, zeps, max_warm_start)

% Compute the sparsest solution of a linear program (LP)
% min c'x
% s.t.  Sx=b
%       lb <= x <= ub
%

 n=size(c,1);error_code=0;

% Compute the optimal value of the LP
% [x0, err_init]=LPSolve(c,Aeq,beq,Ain,bin,lb,ub);
 [x0, err_init]=LPSolve(c,polyhedron);
 err_init
if err_init~=1
 init_result=length(find(abs(x0)>zeps));
 optvalue=x0'*c;

% Heuristic to get 0 if optimal value is 0.
 if optvalue==0 && max(polyhedron.Aeq*zeros(n,1)-polyhedron.beq)==0 && max(polyhedron.Ain*zeros(n,1)-polyhedron.bin)<=0 && max(polyhedron.lb-zeros(n,1))<=0 && max(zeros(n,1)-polyhedron.ub)<=0
	z=zeros(n,1);result_tab=[0]; error_code=0; nb_warm_start=0;
 else

% Generate final polyhedron with optimal value as a constraint.
 polyhedron=struct('Aeq',[polyhedron.Aeq;c'],'beq',[polyhedron.beq;optvalue],'Ain',polyhedron.Ain,'bin',polyhedron.bin,'lb',polyhedron.lb,'ub',polyhedron.ub);

%#################################################################################
% MAIN : Algorithm call with warm start if we didn't improve l1-norm solution or if we have precision problem

 nb_warm_start=-1;tic;
 [x, err_l1]=L1Solve(polyhedron); l1_result=length(find(abs(x)>zeps));
 err_l1
 if err_l1~=1
	result_tab=[init_result, l1_result];
 else
	result_tab=[init_result];
 end

% Parameters
 nb_itemax_r=50; nb_itemax_sla=2; update_param=0.5;rstop=sqrt(eps); move=1;

 while err_l1~=1 && nb_warm_start<max_warm_start && init_result>0 && ( nb_warm_start==-1 || nnz_z<=min(result_tab)) && move

  [z, nnz_z, error_code] = ThetaL0(polyhedron, @theta1, @dtheta1, nb_itemax_r, rstop, update_param, nb_itemax_sla, zeps, x);
% Update
	nb_warm_start=nb_warm_start+1;
	if nnz_z<min(result_tab)
		result_tab=[result_tab, nnz_z];x=z;
	elseif z==x
		move=0;
	end

 end
 if err_l1==1
	z=x0;nnz_z=-1;error_code=1;
 end
%################################################################################
 toc
 end
else
 result_tab=[]; error_code=1;nb_warm_start=0;z=x0;
end
end

%################################################################################
%# Declaration of functions thetas and their derivatives :

function y = theta1(t,r)
	y=abs(t)./(abs(t)+r);
end

function y = dtheta1(t,r)
	y=sign(t).*r./((abs(t)+r).^2);
end

function y = theta2(t,r)
	y=1-exp(-abs(t)/r);
end

function y = dtheta2(t,r)
	y=sign(t).*exp(-abs(t)./r)./r;
end

%######################################################################################################################
%######################################################################################################################
function [x, nnz_x, error_code] = ThetaL0(polyhedron, theta_func, dtheta_func, nb_itemax_r, rstop, update_param, nb_itemax_sla, zero_precision, init_point)

% Citation :  A Smoothing Method for Sparse Optimization over Polyhedral Sets, Modelling, Computation and Optimization in Information Systems and Management Sciences Advances in Intelligent Systems and Computing Volume 359, 2015, pp 369-379   Tangi Migot, Mounir Haddou, 2015

% error_code :
% -2 : maximum iteration in r
% -1 : maximum iteration in SLA
% 0 : success
% 1 : unbounded
% 2 : precision

 error_code=0;
% code d'erreur SLA : (init in 1 : in case no SLA are done)
 err_code=1;

% for debug :
minx=[];rtab=[];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialization

% if no initial point we take the solution of l1-norm problem
 if isempty(init_point)
	init_point=L1Solve(polyhedron);
 end

 sol=init_point;
 nnz_current=length(find(abs(sol)>zero_precision));nnzero=[];soltab=[];minx=[min(sol(sol>0))];
% initialization of parameter r
 i=1;r0=1;rold=r0;r=rold;rtab=[r0];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%  Main Loop

 while i<nb_itemax_r && theta_func(min(sol(sol>zero_precision)),rold)<nnz_current/(nnz_current+1) && r>rstop
% Scaling to put variables in [0,1] box
	scal=max(abs(sol))/2;z=sol/scal;

	spolyhedron=struct('Aeq',polyhedron.Aeq,'beq',polyhedron.beq/scal,'Ain',polyhedron.Ain,'bin',polyhedron.bin/scal,'lb',polyhedron.lb/scal,'ub',polyhedron.ub/scal);
% Concave program
	[z, err_code] = ThetaSLA(spolyhedron, z,r, nb_itemax_sla, theta_func, dtheta_func);

	    if err_code==1
	     error_code=1;i=nb_itemax_r; % unbounded
	    else
	     	sol=z*scal;soltab=[soltab;sol'];minx=[minx;min(sol(sol>0))];rtab=[rtab,r];
	     	nnzk=length(find(abs(sol)>zero_precision));nnzero=[nnzero, nnzk];
% Update
	     	i=i+1;rold=r;r=r*update_param;
	    end
	    if err_code==-1
	     error_code=-1; % max iteration SLA
	    end

 end
%%%%% End Main Loop
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Best solution find :
if err_code ~=1
 nnz_x=min(nnzero);index=find(nnzero==nnz_x);x=soltab(index(1),:)';
else
 nnz_x=length(find(abs(sol)>zero_precision));x=init_point;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Error code :
 if i==nb_itemax_r && error_code~=1
	error_code=-2; % max iteration in r
 end

 if ( (~isempty(polyhedron.Aeq) && norm(polyhedron.Aeq*x-polyhedron.beq,Inf)>sqrt(eps)) || (~isempty(polyhedron.Ain) && max(polyhedron.Ain*x-polyhedron.bin)>sqrt(eps)) || (~isempty(polyhedron.lb) && max(polyhedron.lb-x)>sqrt(eps)) || (~isempty(polyhedron.ub) && max(x-polyhedron.ub)>sqrt(eps))) && error_code~=1
	error_code=2; % precision
 end

end
%######################################################################################################################
%
% Solve concave program of minimization Theta with Successiv Linearization Algorithm
%
%######################################################################################################################
function [x, error_code] =ThetaSLA(polyhedron,init_point,r_param,ite_max, theta_func, dtheta_func)

% err_code :
% -1 : maximum iteration
%  0 : success
%  1 : unbounded linear program
%  2 : precision
 error_code=0;

 j=1;y=init_point;zero_precision=sqrt(eps);

 while j<ite_max && (j==1 || abs(c'*yold-c'*x)>zero_precision)
	% Objectiv theta function with scaling :

%	c=dtheta_func(y,r_param)/(theta_func(1,r_param))
	c=dtheta_func(y,r_param)*r_param;

	[x,bool_LP]=LPSolve(c,polyhedron);
	% update :
	yold=y;y=x;j=j+1;

	if bool_LP==1
		error_code=1;j=ite_max;
	end
 end

 if error_code~=1
	 if j==ite_max
		error_code=-1;
	 end

	 if (~isempty(polyhedron.Aeq) && norm(polyhedron.Aeq*x-polyhedron.beq,Inf)>zero_precision) || (~isempty(polyhedron.Ain) && max(polyhedron.Ain*x-polyhedron.bin)>zero_precision) || (~isempty(polyhedron.lb) &&max(polyhedron.lb-x)>sqrt(eps)) || (~isempty(polyhedron.ub) &&max(x-polyhedron.ub)>sqrt(eps))
		error_code=2;
	 end
 end
end
%######################################################################################################################
%
% Solve convex program of minimization l1-norm
%
%######################################################################################################################
function [x,error_code]=L1Solve(polyhedron)
%{
% here we use SeDuMi from CVX Matlab package
 n=size(polyhedron.lb,1);error_code=0;
% min c'x
% s.t.  Aeq x == beq
%       Ain x <= bin
%       lb <= x <= ub

 cvx_begin quiet
     cvx_precision best
%     cvx_solver sdpt3
     variable x(n)
     minimize(norm(x,1))
     subject to
         polyhedron.Aeq*x == polyhedron.beq;
         polyhedron.Ain*x <= polyhedron.bin;
         x >= polyhedron.lb;
         x <= polyhedron.ub;
 cvx_end

% error 1 : unbounded
 if cvx_optval<=-Inf || cvx_optval>=Inf
	error_code=1;
 end

% error 2 : precision
 if norm(polyhedron.Aeq*x-polyhedron.beq,Inf)>sqrt(eps) || max(polyhedron.Ain*x-polyhedron.bin)>sqrt(eps) || max(polyhedron.lb-x)>sqrt(eps) || max(x-polyhedron.ub)>sqrt(eps)
	error_code=2;
 end
%}
%######################################################################################################################
% Solveur GUROBI

% solver with Gurobi (reformulation comme un LP)
if isempty(polyhedron.Aeq)
	%On a pas besoin des égalités
	AEQ=[];
else
	m1=size(polyhedron.beq,1);n=size(polyhedron.Aeq,2);AEQ=[polyhedron.Aeq, zeros(m1,n)];
end
if isempty(polyhedron.Ain)
	n=size(polyhedron.lb,1);
	AIN=[eye(n), -eye(n); -eye(n), -eye(n)];
	BIN=[zeros(n,1); zeros(n,1)];
else
	m2=size(polyhedron.bin,1);n=size(polyhedron.Ain,2);
	AIN=[polyhedron.Ain, zeros(m2,n); eye(n), -eye(n); -eye(n), -eye(n)];
	BIN=[polyhedron.bin;zeros(n,1); zeros(n,1)];
end
if isempty(polyhedron.lb)
	LB=[];
else
	n=size(polyhedron.lb,1);LB=[polyhedron.lb; zeros(n,1)];
end
if isempty(polyhedron.ub)
	UB=[];
else
	n=size(polyhedron.ub,1);temp=max(abs(polyhedron.lb),abs(polyhedron.ub));UB=[polyhedron.ub; temp];
end

polyhedron_l1=struct('Aeq',AEQ,'beq',polyhedron.beq,'Ain',AIN,'bin',BIN,'lb',LB,'ub',UB);
c=[zeros(n,1);ones(n,1)];

[x,error_code]=LPSolve(c,polyhedron_l1);
x=x(1:n);

end
%######################################################################################################################
%
% Solve linear Program
%
%######################################################################################################################
function [x,error_code]=LPSolve(c,polyhedron)
%{
% Solveur SEDUMI
 n=size(c,1);error_code=0;
% min c'x
% s.t.  Aeq x == beq
%       Ain x <= bin
%       lb <= x <= ub

 cvx_begin quiet
    cvx_precision best
    cvx_solver sedumi
    variable x(n)
    minimize(c'*x)
    subject to
	polyhedron.Aeq*x == polyhedron.beq;
	polyhedron.Ain*x <= polyhedron.bin;
	x >= polyhedron.lb;
	x <= polyhedron.ub;
 cvx_end

% error 1 : unbounded
 if cvx_optval<=-Inf || cvx_optval>=Inf
	error_code=1;
 end

% error 2 : precision
 if norm(polyhedron.Aeq*x-polyhedron.beq,Inf)>sqrt(eps) || max(polyhedron.Ain*x-polyhedron.bin)>sqrt(eps) || max(polyhedron.lb-x)>sqrt(eps) || max(x-polyhedron.ub)>sqrt(eps)
	error_code=2;
 end
%}
%######################################################################################################################
% Solveur GUROBI

[x, fval, exitflag] = linprog_gurobi(c, polyhedron.Ain, polyhedron.bin, polyhedron.Aeq, polyhedron.beq, polyhedron.lb, polyhedron.ub);

if exitflag==1 || exitflag==0
 error_code=0;
elseif exitflag==-2 || exitflag==-3
 error_code=1;
end

% error 2 : precision
 if (~isempty(polyhedron.Aeq) && norm(polyhedron.Aeq*x-polyhedron.beq,Inf)>sqrt(eps)) || (~isempty(polyhedron.Ain) && max(polyhedron.Ain*x-polyhedron.bin)>sqrt(eps)) || (~isempty(polyhedron.lb) &&max(polyhedron.lb-x)>sqrt(eps)) || (~isempty(polyhedron.ub) &&max(x-polyhedron.ub)>sqrt(eps))
	error_code=2;
 end

end
function [x, fval, exitflag] = linprog_gurobi(f, A, b, Aeq, beq, lb, ub)
%LINPROG A linear programming example using the Gurobi MATLAB interface
%
%   This example is based on the linprog interface defined in the
%   MATLAB Optimization Toolbox. The Optimization Toolbox
%   is a registered trademark of The MathWorks, Inc.
%
%   x = LINPROG(f,A,b) solves the linear programming problem:
%
%   minimize     f'*x
%   subject to   A*x <= b
%
%
%   x = LINPROG(f,A,b,Aeq,beq) solves the problem:
%
%   minimize     f'*x
%   subject to     A*x <= b,
%                Aeq*x == beq.
%
%   x = LINPROG(f,A,b,Aeq,beq,lb,ub) solves the problem:
%
%   minimize     f'*x
%   subject to     A*x <= b,
%                Aeq*x == beq,
%          lb <=     x <= ub.
%
%   You can set lb(j) = -inf, if x(j) has no lower bound,
%   and ub(j) = inf, if x(j) has no upper bound.
%
%   [x, fval] = LINPROG(f, A, b) returns the objective value
%   at the solution. That is, fval = f'*x.
%
%   [x, fval, exitflag] = LINPROG(f, A, b) returns an exitflag
%   containing the status of the optimization. The values for
%   exitflag and corresponding status codes are:
%      1 - OPTIMAL,
%      0 - ITERATION_LIMIT,
%     -2 - INFEASIBLE,
%     -3 - UNBOUNDED.
%

if nargin < 3
    error('linprog(f, A, b)')
end

if nargin > 7
    error('linprog(f, A, b, Aeq, beq, lb, ub)');
end

if ~isempty(A)
    n = size(A, 2);
elseif nargin > 4 && ~isempty(Aeq)
    n = size(Aeq, 2);
else
    error('No linear constraints specified')
end

if ~issparse(A)
    A = sparse(A);
end

if nargin > 3 && ~issparse(Aeq)
    Aeq = sparse(Aeq);
end

model.obj = f;

if nargin < 4
    model.A = A;
    model.rhs = b;
    model.sense = '<';
else
    model.A = [A; Aeq];
    model.rhs = [b; beq];
    model.sense = [repmat('<', size(A,1), 1); repmat('=', size(Aeq,1), 1)];
end

if nargin < 6
    model.lb = -inf(n,1);
else
    model.lb = lb;
end

if nargin == 7
   model.ub = ub;
end

params.outputflag = 0;
result = gurobi(model, params);

if strcmp(result.status, 'OPTIMAL')
    exitflag = 1;
elseif strcmp(result.status, 'ITERATION_LIMIT')
    exitflag = 0;
elseif strcmp(result.status, 'INF_OR_UNBD')
    params.dualreductions = 0;
    result = gurobi(model, params);
    if strcmp(result.status, 'INFEASIBLE')
        exitflag = -2;
    elseif strcmp(result.status, 'UNBOUNDED')
        exitflag = -3;
    else
        exitflag = nan;
    end
elseif strcmp(result.status, 'INFEASIBLE')
    exitflag = -2;
elseif strcmp(result.status, 'UNBOUNDED')
    exitflag = -3;
else
    exitflag = nan;
end

if isfield(result, 'x')
    x = result.x;
else
    x = nan(n,1);
end

if isfield(result, 'objval')
    fval = result.objval;
else
    fval = nan;
end
end


function [A, b, Aeq, beq, lb, ub] = l_constraints(mpcModel, k, varargin)

        A   = []; %A*u<=b
        b   = [];
        Aeq = [1 1];
        beq = mpcModel.net_load(k);

        ub  = [10, mpcModel.battery.power(1,1)];
        lb  = [-5, mpcModel.battery.power(1,2)];

end


%
% SCRIPT TO TEST THE KINEMATICS OF THE 5R robot
%
% Copyright (C) 2012, by Arturo Gil Aparicio
%
% 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/>.

close all

%load the robot
%the robot is represented (interiorly) by two planar 2dof arms
robot=load_robot('example','5R');

adjust_view(robot)

%Represent a final point in space that will be achieved by the end effector
%P=[Px, Py]=[0.6 0.4]
T=eye(4);
T(1,4)=1.0;%change, for example, to 1.25
%T(2,4)=0.967; %change, for example to 0.967
T(2,4)=0.8; %change, for example to 0.967

%find 4 solutions for the INVERSE KINEMATIC in position
q=inversekinematic(robot, T)

drawrobot3d(robot, q(:,1)), pause(1);
drawrobot3d(robot, q(:,2)), pause(1);
drawrobot3d(robot, q(:,3)), pause(1);
drawrobot3d(robot, q(:,4)), pause(1);
close all
%now solve the DIRECTKINEMATIC problem for each of the above computed
% q
for i=1:4,
    drawrobot3d(robot, q(:,i))
    figure, hold
    % 1 and 3 correspond to the joint variables of the robot
    T=directkinematic(robot, [q(1,i) q(3,i)])
    pause(2);
    close all
end


% This file is part of TREEQSM.
%
% TREEQSM 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.
%
% TREEQSM 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 TREEQSM.  If not, see <http://www.gnu.org/licenses/>.

function [R,dR1,dR2] = form_rotation_matrices(theta)

% --------------------------------------------------------------------------
% FORM_ROTATION_MATRICES.M      Forms rotation matrices R = R2*R1 and its
%                                   derivatives
%
% Input
% theta    Plane rotation angles (t1, t2)
%
% Output
% R        Rotation matrix
% R1       Plane rotation [1 0 0; 0 c1 -s1; 0 s1 c1]
% R2       Plane rotation [c2 0 s2; 0 1 0; -s2 0 c2]

c = cos(theta);
s = sin(theta);

R1 = [1 0 0; 0 c(1) -s(1); 0 s(1) c(1)];
R = R1;

R2 = [c(2) 0 s(2); 0 1 0; -s(2) 0 c(2)];
R = R2*R;

if nargout > 1
    dR1 = [0 0 0; 0 -R1(3,2) -R1(2,2); 0 R1(2,2) -R1(3,2)];
end

if nargout > 2
    dR2 = [-R2(1,3) 0 R2(1,1); 0 0 0; -R2(1,1) 0 -R2(1,3)];
end


function [VecTot, TOT, timei] = ierm(P, Mat, NumDesc)

%ierm returns a matrix containing the best models for all the paths of the
%Enhanced Replacement Method using an initial set of descriptors with a
%very high S, given by an inverse RM.
%TOT contains all the relative results of each step showing the evolution of the method.
%
%
%	   Input:
%             P             Property vector
%             Mat           Descriptors matrix with descriptors pool
%             NumDesc       Number of descriptors that the model will have
%
%     Returns:
%
%            VecTot           vector containing the best model for all the
%                               paths of the Replacement Method
%            TOT               contains all the relative results
%                               showing the evolution of the method.
%
% Andrew G. Mercader, Pablo R. Duchowicz
% INIFTA, La Plata, Argentina
% Created: 12 Nov 2007

timei=cputime;

if (nargin < 3)
   error('the function requires at least 5 input variables. Type ''help ierm''.');
end

[c_m, r_m] = size(Mat);

NTot=r_m;

VecI=1:NumDesc;
lindep = ld(P, VecI,Mat);

if lindep==100
    for j=1:1000000000000000000000000000000000000
     VecI = randint(1,NumDesc,[1, NTot]);
     lindep = ld(P, VecI,Mat);
        if lindep==0
            break;
        end
    end
end

[VecT] = rmt_inv(P, VecI, Mat);
VecJ=VecT(NumDesc,3:end);

[VecTot, TOT, time] = erm(P, VecJ, Mat);

timei=cputime-timei
end

%End


function zData = constructMissingSurvey(zDataTrue, Gdraw, PsiAll, dataValues)

%PsiAll=PsiDraw_prop;
% Initialize
Ppsi = dataValues.Ppsi; %load selection matrix for the states (local mean), where survey data is available
PsiZ = PsiAll * Ppsi'; %only use those variables
[T, Mz] = size(zDataTrue);
zData = zDataTrue;

% for each survey variable
for iM = 1:Mz

    % determine time of incoming survey data
    Tz              = find(isfinite(zData(:,iM)),1,'first');
    iNaNs           = find(isnan(zData(:,iM)));
    iNaNs(iNaNs<Tz) = [];

    zDataDraw_i     = PsiZ(:, iM) + sqrt(Gdraw(:,iM)).*randn(T,1);
    % fill missing values (to previous values)
    zData(iNaNs,iM) = zDataDraw_i(iNaNs);
end

end


% MOVE_INDR - Removes the indices indc from the indices inda
%             and adds them to the reserve vector index list
%             if necessary.
%
% Syntax: [inda,i] = move_indr(inda,indc)
%
%      i: when i > 0, the i-th element was removed from ind{RESERVE}
%
% Version 3.22e -- Comments to diehl@alumni.cmu.edu
%

function [inda,removed_i] = move_indr(inda,indc)

% flags for example state
MARGIN    = 1;
ERROR     = 2;
RESERVE   = 3;
UNLABELED = 4;

% define global variables
global g;							% partial derivatives of cost function w.r.t. alpha coefficients
global ind;							% cell array containing indices of margin, error, reserve and unlearned vectors
global max_reserve_vectors;   % maximum number of reserve vectors

removed_i = 0;
num_RVs_orig = length(ind{RESERVE});

% shift indc from inda to ind{RESERVE}
[inda,ind{RESERVE}] = move_ind(inda,ind{RESERVE},indc);

% if we need to remove some reserve vectors
if (length(ind{RESERVE}) > max_reserve_vectors)

   % sort g(ind{RESERVE})
   [g_sorted,i] = sort(g(ind{RESERVE}));

   % reserve vectors that need to be removed
   removed = i(max_reserve_vectors+1:length(i));

   % find any original reserve vectors that need to be removed
   k = find(removed <= num_RVs_orig);
   if (length(k) > 0)
      removed_i = removed(k);
   end;

   % remove the necessary reserve vectors
   ind{RESERVE}(removed) = [];

end;


% STD_FINDSAMEICA - find groups of datasets with identical ICA decomposiotions
%                     (search identical weight*sphere matrices)
%
% Usage:
%        >> clusters = std_findsameica(ALLEEG);
%        >> clusters = std_findsameica(ALLEEG,icathreshold);
% Inputs:
%   ALLEEG           - a vector of loaded EEG dataset structures of all sets
%                      in the STUDY set.
%   icathreshold     - Threshold to compare icaweights. Default 2e-4.
%
% Outputs:
%   cluster - cell array of groups of datasets
%   indices - cluster index for each dataset
%
% Authors:  Arnaud Delorme, SCCN, INC, UCSD, July 2009-
% 2016 change: as of May 2016, the function now compares the product of the
%              weight and the sphere matrices instead of just the weight
%              matrices.

% Copyright (C) Arnaud Delorme, SCCN, INC, UCSD, arno@sccn.ucsd.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

% Coding notes: Useful information on functions and global variables used.

function [cluster, inds] = std_findsameica(ALLEEG, varargin)

% 6/2/2014 Ramon : Allow ica threshold as input.
if nargin == 1
    icathreshold = 2e-4;
elseif nargin == 2
    icathreshold = varargin{1};
end

cluster = { [1] };
inds = [1];
for index = 2:length(ALLEEG)

    found = 0;
    for c = 1:length(cluster)
        w1 = ALLEEG(cluster{c}(1)).icaweights*ALLEEG(cluster{c}(1)).icasphere;
        w2 = ALLEEG(index).icaweights*ALLEEG(index).icasphere;
        if all(size(w1) == size(w2))
            %if isequal(ALLEEG(cluster{c}(1)).icaweights, ALLEEG(index).icaweights)
            if sum(sum(abs(w1-w2))) < icathreshold
                cluster{c}(end+1) = index;
                inds(index) = c;
                found = 1;
                break;
            end
        end
    end
    if ~found
        cluster{end+1} = index;
        inds(index) = index;
    end
end


function [voxAmps, tS] = twEventPrediction(mv, rmParams, anal);
% twEventPrediction - compute a set of predicted voxel amplitudes
% and time series for a  set of voxels, given retinotopy parameters estimated
% from a traveling wave analysis.
%
%  [voxAmps tS] = twEventPrediction(mv, rmParams, anal);
%
% The 'mv' struct is a multivoxel struct, created by mv_init or
% multiVoxelUI.
%
% The anal struct is the output from rmVisualizeRFs. The voxels flag
% specifies the voxels for which to make predictions (defaulting to all
% voxels in the current ROI), and the roi flag specifies which ROI to make
% predictions for in the anal struct (default is the first one).
%
% ras, 09/2006.
if nargin<3, error('Not enough input args.');               end
if notDefined('roi'),    roi = 1;                           end
if notDefined('voxels'), voxels = 1:length(anal.x0{roi});   end

nConds = sum(mv.trials.condNums>0);
nVoxels = length(voxels);

% to properly normalize the voxel amplitudes, we need
% to divide by a measure of the number of samples per square degree:
% the idea is that the density per deg^2 is constant over time, but
% sampling may change.
dX = unique(diff(anal.X(:))); dY = unique(diff(anal.Y(:)));
dX = dX(2); dY = dY(2); % should be at least 2 unique vals, 0 and step size
samplesPerDeg2 = ceil( [1/dX] * [1/dY] );

% convert stimuli in rmParams to a more useable format
xRange = unique(anal.X); yRange = unique(anal.Y);
[rmParams images] = rmStimulusMatrix(rmParams, xRange, yRange, 0);

% compute voxAmps
fprintf('[%s] Computing predicted voxel amplitudes ', mfilename);
for c = 1:nConds
    mask = logical( round(images(:,:,c)) );

    for v = 1:nVoxels
        ii = voxels(v);
        sigma = anal.sigma{roi}(ii) .* 2;
        x0 = anal.x0{roi}(ii);
        y0 = anal.y0{roi}(ii);
        beta = anal.beta{roi}(ii);
%         sig = anal.log10p{roi}(ii);
        RF = beta * rfGaussian2D(anal.X, anal.Y, sigma, sigma, 0, x0, y0);

        % multiply by the two stimulus representations
        voxAmps(v,c) = sum( RF(mask) ); % / [sum(mask(:))/samplesPerDeg2];
    end

    fprintf('.');
end
fprintf(' done.\n');

% using voxAmps as scaling betas, compute predicted tSeries
erParams = mv.params; erParams.glmHRF = -1; % delta function only
X = glm_createDesMtx(mv.trials, erParams);
X = X(:,2:end); % remove null condition onsets
nFrames = size(X, 1);
betas = ones(1, size(X,2)); % note this is beta for the prediction, not the
                            % retinotopy model
for v = 1:nVoxels
    betas(1:nConds) = voxAmps(voxels(v),:);
%     timeSeries = conv2(X * betas', rmParams.analysis.Hrf{1}, 'full');
    timeSeries = filter(rmParams.analysis.Hrf{1}, 1, X * betas');
    tS(:,v) = timeSeries(1:nFrames);
end

% remove NaNs
voxAmps( isnan(voxAmps) ) = 0;
tS( isnan(tS) ) = 0;

return


function perm0_uniform_test ( )

%*****************************************************************************80
%
%% PERM0_UNIFORM_TEST tests PERM0_UNIFORM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 January 2015
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'PERM0_UNIFORM_TEST\n' );
  fprintf ( 1, '  PERM0_UNIFORM randomly selects a permutation of 0,...,N-1.\n' );
  fprintf ( 1, '\n' );

  seed = 123456789;

  for test = 1 : 5

    [ p, seed ] = perm0_uniform ( n, seed );

    fprintf ( 1, '  ' );
    for i = 1 : n
      fprintf ( 1, '%4d', p(i) );
    end
    fprintf ( 1, '\n' );

  end

  return
end


%% t_meshFromClass
%
%   Convert itkGray Class file to mesh using Matlab tools.
%
% See also: t_meshShow
%
% BW (c) Stanford VISTA Team

%% You might first check whether you can visualize a VISTASOFT mesh
% Either run: t_meshShow
% Or, just run this code
%  load(fullfile(mrvDataRootPath,'anatomy','anatomyNIFTI','leftMesh.mat'));
%  meshVisualize(msh);

%% Build a matlab mesh from the itkGray class file

niCFile = fullfile(mrvDataRootPath,'anatomy','anatomyV','t1_class.nii.gz');
niClass = niftiRead(niCFile);
Ds = uint8(niClass.data);

% The ITKGRAY class labels are
%    0: unlabeled
%    1: CSF
%    2: Subcortical
%    3: left white matter
%    5: left gray matter
%    4: right white matter
%    6: right gray matter

% Set the labels for everything that is not left gray or white to
% unlabeled.
Ds(Ds == 1) = 0;
Ds(Ds == 2) = 0;
Ds(Ds == 4) = 0;
Ds(Ds == 5) = 0;
Ds(Ds == 6) = 0;
% showMontage(double(Ds))

%% Matlab calculations reducing the patches and computing normals

% Make a picture of the cortical mesh.  We have all the information.
fv = isosurface(Ds,1);

% Reduce to about 30K faces.  That's enough
nFaces = size(fv.faces,1);
maxFaces = 4*10^4;    % 40,000 faces?
if nFaces > maxFaces
    p = maxFaces/nFaces;
    fv = reducepatch(fv.faces,fv.vertices, p);
end

% Smooth
smoothMode = 1; nIter = 3;
fv = smoothpatch(fv,smoothMode,nIter);

% mrvNewGraphWin;
% patch(fv, 'FaceColor','red','EdgeColor','none');
% view(3); daspect([1,1,1]); axis tight
% camlight; camlight(-80,-10); lighting phong;
% title('Iso surface via isonormals')

%% Now, take the Matlab information and put it into VISTASOFT mesh
mmPerVox = [1 1 1];
windowID = 1000;
actor = 33;
msh = meshFV2msh(fv,mmPerVox,windowID, actor);
meshVisualize(msh);

%% End


function [im_h] = backprojection(im_h, im_l, maxIter)

[row_l, col_l] = size(im_l);
[row_h, col_h] = size(im_h);

p = fspecial('gaussian', 5, 1);
p = p.^2;
p = p./sum(p(:));

im_l = double(im_l);
im_h = double(im_h);

for ii = 1:maxIter,
    im_l_s = imresize(im_h, [row_l, col_l], 'bicubic');
    im_diff = im_l - im_l_s;

    im_diff = imresize(im_diff, [row_h, col_h], 'bicubic');
    im_h = im_h + conv2(im_diff, p, 'same');
end


function [ node_num, triangle_num, hole_num ] = ...
  triangulation_order6_example2_size ( )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER6_EXAMPLE2_SIZE sets sizes for a sample triangulation.
%
%  Diagram:
%
%   21-22-23-24-25
%    |\  6 |\  8 |
%    | \   | \   |
%   16 17 18 19 20
%    |   \ |   \ |
%    | 5  \| 7  \|
%   11-12-13-14-15
%    |\  2 |\  4 |
%    | \   | \   |
%    6  7  8  9 10
%    | 1 \ | 3 \ |
%    |    \|    \|
%    1--2--3--4--5
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer NODE_NUM, the number of points.
%
%    Output, integer TRIANGLE_NUM, the number of triangles.
%
%    Output, integer HOLE_NUM, the number of holes.
%
  node_num = 25;
  triangle_num = 8;
  hole_num = 0;

  return
end


%% PIVOT AND UNPIVOT: a user's guide
% The scope of these two functions is to organize a _flat_ dataset into a PIVOT table (and eventually undo it).
% This guide will illustrate how to use them.

%% CONCEPT: flat dataset
% A _flat_ dataset usually contains a column of values and "n" columns of features. So, each row
% hs its value with its features.
%
% *1. A _flat_ dataset with two features (Region and Year)*
load Demodata.mat
display([{'Year','Region','People'}; Demodata.ex1])
%% CONCEPT: pivot table
% A pivot table as intended by the T-SQL command _pivot_ or as implemented by spreadsheet softwares is a data
% visualization tool which groups a _flat_ dataset by two chosen dimension.
%
% *2. Format Demodata.ex1 into a _pivot_ table*
warning off warnPivot:funGroup      % we'll come back later on the warning
display(Pivot(Demodata.ex1))
%% PIVOT input constraints
% $$Pivot(In, Fun, noHeaders, Pad)$$
%
% Not every type of input is supported by PIVOT. Only a _flat_ dataset with max three columns is supported. The first
% column should be the grouping dimension which will appear as the column  header and the second column the dimension
% that will appear as the row header. The last column should contain the values that will be "intersected" and grouped.
%
% Here are listed all the constraints to the inputs (please mail me if something is left out):
%
    %%
    %
    % * IN should have 3 columns
    % * IN can be numeric or a cell array of scalars or strings if headers (no mixing)
    % * Nested cells are not allowed
    % * NaNs or empty cells in the first two columns of IN are not allowed
    % * If IN is a cell the column with the values must be a cell array of scalars
    % * FUN must be a function handle
    % * FUN applied to a vector must return a scalar

try Pivot([1,3;2,4]); catch; err = lasterror; display(err.message); end %#ok
try Pivot('try'); catch; err = lasterror; display(err.message); end %#ok
try Pivot({'1999',1,2;1999,1,2}); catch; err = lasterror; display(err.message); end %#ok
try Pivot({1,2,3;1,{2},3}); catch; err = lasterror; display(err.message); end %#ok
try Pivot({NaN,1,9;1,3,4}); catch; err = lasterror; display(err.message); end %#ok
try Pivot([1,NaN,9;1,3,4]); catch; err = lasterror; display(err.message); end %#ok
try Pivot({[],23,9;1,3,4}); catch; err = lasterror; display(err.message); end %#ok
try Pivot({23,[],9;1,3,4}); catch; err = lasterror; display(err.message); end %#ok
try Pivot({23,1,9;1,3,'9'}); catch; err = lasterror; display(err.message); end %#ok
try Pivot(Demodata.ex1(2:end,:),'sum'); catch; err = lasterror; display(err.message); end %#ok
try Pivot(Demodata.ex1(2:end,:),@diff); catch; err = lasterror; display(err.message); end %#ok
%% PIVOT examples
% How to use this function:
%
% *1. IN numeric or cellarray of scalars. No grouping involved. Padded by default with NaN*
disp(Pivot(Demodata.ex2))
disp(Pivot(num2cell(Demodata.ex2))) % OUT will be automatically converted to numeric
%%
% *2. Grouping involved using default FUN (@sum) or @min*
disp(Pivot(Demodata.ex3))
disp(Pivot(Demodata.ex3,@min))
%%
% *3. OUT without headers*
disp(Pivot(Demodata.ex3,[],true))
%%
% *4. Pad with zeros or with '#' and no headers (OUT will be cell).*
disp(Pivot(Demodata.ex3,[],false,0))
disp(Pivot(Demodata.ex3,[],1,'#'))
%%
% *5. The same syntaxes could be used with IN headers as cellarrays of strings*
disp(Pivot(Demodata.ex4))
disp(Pivot(Demodata.ex4,@max))
disp(Pivot(Demodata.ex4,@max,1))
disp(Pivot(Demodata.ex4,@max,1,'???'))
%%
% *6. Full output*
[Out,colHeader,rowHeader,Settings] = Pivot(Demodata.ex4,@max,1,'???');
disp([{'colHeader','rowHeader'};[colHeader,rowHeader]])
disp('    Settings')
disp(Settings)
%% PIVOT warning
% We set *warning off warnPivot:funGroup* before. This was meant to avoid the following message
disp(lastwarn)
    %%
    % being thrown each time PIVOT is applied to a dataset which has multiple values on the
    % same intersection between the two grouping dimensions. The warning is meant
    % to remember the user that some values are being grouped
%% UNPIVOT input constraints
% $$unPivot(In, dim, rmPad)$$
%
% Only _pivot_ tables are accepted as inputs. The first elements of IN should be NaN. The first row and the
% first column (except the IN(1) element) are the headers/features that will define the values in
% IN(2:end,2:end).
%
% Here are listed all the constraints to the inputs (please mail me if something is left out):
%
    %%
    %
    % * IN should be a _pivot_ table
    % * IN(1) must be NaN
    % * Nested cells are not allowed
    % * DIM can be 1 or 2 or empty
    % * RMPAD can be a char or a scalar
try unPivot([1,3]); catch; err = lasterror; display(err.message); end %#ok
try unPivot({'G1',1999;'G2',24}); catch; err = lasterror; display(err.message); end %#ok
try unPivot({NaN,1999;'G2',{24}}); catch; err = lasterror; display(err.message); end %#ok
try unPivot(Demodata.ex5,3); catch; err = lasterror; display(err.message); end %#ok
try unPivot(Demodata.ex5,'1'); catch; err = lasterror; display(err.message); end %#ok
try unPivot(Demodata.ex5,[],{23}); catch; err = lasterror; display(err.message); end %#ok
%% UNPIVOT examples
% How to use this function:
%
% *1. Simplest case*
    %%
    % IN numeric or cellarray of scalars. Removes by default the NaNs. Sorts by default
    % according the column headers (second column in the _unPivoted_ table)
A = unPivot(Demodata.ex5); disp(A)
B = unPivot(Demodata.ex5,[]);
C = unPivot(Demodata.ex5,1);
if isequal(A,B,C); fprintf('A, B and C are equivalent'); else error('something''s wrong'); end
%%
% *2. Sort by row header (first column in the _unPivoted_ table)*
disp(unPivot(Demodata.ex5,2))
%%
% *3. Remove padded value other than NaN*
disp(unPivot(Demodata.ex5,1,3174))
%%
% *4. Same syntaxes can be applied to a cell array. Empty cells removed by default.*
disp(unPivot(Demodata.ex6))
disp(unPivot(Demodata.ex6,2))
disp(unPivot(Demodata.ex6,[],20))
disp(unPivot(Demodata.ex6,2,'???'))
%% Performance issues
% Lets test the performances of PIVOT and UNPIVOT.
    %%
    % First we create our Test input (which isn't included in Demodata since its about 35 MB)
    Nums1 = ceil(rand(100,1)*100);
    Nums2 = num2str(ceil(rand(100,1)*100));
    Test = [cellstr(strcat('Year', num2str(Nums1+1900))),...
            cellstr(strcat('Group', Nums2)),...
            num2cell(Nums1)];
    Test = repmat(Test,1000,1);
    %%
    % Then using the profile...
    profile on
    Testpivoted = Pivot(Test,[],[],' ');
    Testback = unPivot(Testpivoted,1,' ');
    profile off
    stats = profile('info');
    stats = [{stats.FunctionTable.FunctionName}',{stats.FunctionTable.TotalTime}'];
    disp(stats([strmatch('Pivot',stats(:,1)); strmatch('unPivot',stats(:,1))],:))
    %%
    % While UNPIVOT is fast enough PIVOT is slowed down by cell2mat like behaviour.
    % Here is implemented cell2floatmod which is a compact version of the cell2float by Jos(10584)
    %%
    % Also notice that the Test that was originally created is different from the unpivoted
    % version of the same Test because of the grouping.
    fprintf('Are they equal: ''%d''', isequalwithequalnans(Test, Testback))
%% Other Pivot-like functions
% *Statistics Toolbox owners:*
%%
% pivoting could be accomplished on a dataset obj by the functions _stack_ and _unstack_.
% These functions use a similar engine approach to Pivot and unPivot and the performances
% lie on the same scale but the syntax is different.
%%
% *FEX submission 20963-reshape-a-matrix by Dimitri Shvorob (ID:17777):*
%%
% The functions are called _wide2tall_ and _tall2wide_ and are probably the foundations for the
% statistics TB functions because of the syntax and the names. Has drawbacks on performance.


function P_matrix = Initialize_LC_P_matrix(LC_KF_config)
%Initialize_LC_P_matrix - Initializes the loosely coupled INS/GNSS KF
%error covariance matrix
%
% Software for use with "Principles of GNSS, Inertial, and Multisensor
% Integrated Navigation Systems," Second Edition.
%
% This function created 12/4/2012 by Paul Groves
%
% Inputs:
%   TC_KF_config
%     .init_att_unc           Initial attitude uncertainty per axis (rad)
%     .init_vel_unc           Initial velocity uncertainty per axis (m/s)
%     .init_pos_unc           Initial position uncertainty per axis (m)
%     .init_b_a_unc           Initial accel. bias uncertainty (m/s^2)
%     .init_b_g_unc           Initial gyro. bias uncertainty (rad/s)
%
% Outputs:
%   P_matrix              state estimation error covariance matrix

% Copyright 2012, Paul Groves
% License: BSD; see license.txt for details

% Begins

% Initialize error covariance matrix
P_matrix =  zeros(15);
P_matrix(1:3,1:3) = eye(3) * LC_KF_config.init_att_unc^2;
P_matrix(4:6,4:6) = eye(3) * LC_KF_config.init_vel_unc^2;
P_matrix(7:9,7:9) = eye(3) * LC_KF_config.init_pos_unc^2;
P_matrix(10:12,10:12) = eye(3) * LC_KF_config.init_b_a_unc^2;
P_matrix(13:15,13:15) = eye(3) * LC_KF_config.init_b_g_unc^2;

% Ends


function pass = test_size( pref )
% Test SIZE

if ( nargin == 0 )
    pref = chebfunpref;
end

% Check two components:
F = chebfun2v(@(x,y) cos(x), @(x,y) sin(y));
pass(1) = all( size(F) == [2 inf inf]);
pass(2) = ( size(F, 1) == 2 );
pass(3) = ( size(F, 2) == inf);
pass(4) = ( size(F, 3) == inf);

% Check three components:
F = chebfun2v(@(x,y) cos(x), @(x,y) sin(y), @(x,y) cos(x));
pass(5) = all( size(F) == [3 inf inf]);
pass(6) = ( size(F, 1) == 3 );
pass(7) = ( size(F, 2) == inf);
pass(8) = ( size(F, 3) == inf);

end


%SAD	Stereo matching using Sum of Squared Differences metric
%
%	[DISP SCORES] = SSD(LEFT, RIGHT, LEFT_RIGHT, WIN_SIZE, DISP_RANGE)
%
%       DISP = Disparity map
%       SCORES = SSD scores at each disparity
%       LEFT, RIGHT = images
%       LEFT_RIGHT = 'l' if wrt left image, 'r' if wrt right image
%       WIN_SIZE = window size or [x_size y_size]
%       DISP_RANGE = [START_DISPARITY END_DISPARITY]
% Author: Jasmine E. Banks (jbanks@ieee.org)

% Copyright in this software is owned by CSIRO.  CSIRO grants permission to
% any individual or institution to use, copy, modify, and distribute this
% software, provided that:
%
% (a)     this copyright and permission notice appears in its entirety in or
% on (as the case may be) all copies of the software and supporting
% documentation;
%
% (b)     the authors of papers that describe software systems using this
% software package acknowledge such use by citing the paper as follows:
%
%     "Quantitative Evaluation of Matching Methods and Validity Measures for
%     Stereo Vision" by J. Banks and P. Corke, Int. J. Robotics Research,
%     Vol 20(7), 2001; and
%
% (c)     users of this software acknowledge and agree that:
%
%   (i) CSIRO makes no representations about the suitability of this software
%   for any purpose;
%
%   (ii) that the software is provided "as is" without express or implied
%   warranty; and
%
%   (iii) users of this software use the software entirely at their own risk.


function [datafit,Amps,freqs,Fval,sig]=fitlinesc(data,params,p,plt,f0,fscanbw)
% fits significant sine waves to data (continuous data).
%
% Usage: [datafit,Amps,freqs,Fval,sig]=fitlinesc(data,params,p,plt,f0)
%
%  Inputs:
% Note that units of Fs, fpass have to be consistent.
%       data        (data in [N,C] i.e. time x channels/trials or a single
%       vector) - required.
%       params      structure containing parameters - params has the
%       following fields: tapers, Fs, fpass, pad
%           tapers : precalculated tapers from dpss or in the one of the following
%                    forms:
%                   (1) A numeric vector [TW K] where TW is the
%                       time-bandwidth product and K is the number of
%                       tapers to be used (less than or equal to
%                       2TW-1).
%                   (2) A numeric vector [W T p] where W is the
%                       bandwidth, T is the duration of the data and p
%                       is an integer such that 2TW-p tapers are used. In
%                       this form there is no default i.e. to specify
%                       the bandwidth, you have to specify T and p as
%                       well. Note that the units of W and T have to be
%                       consistent: if W is in Hz, T must be in seconds
%                       and vice versa. Note that these units must also
%                       be consistent with the units of params.Fs: W can
%                       be in Hz if and only if params.Fs is in Hz.
%                       The default is to use form 1 with TW=3 and K=5
%
%	        Fs 	        (sampling frequency) -- optional. Defaults to 1.
%               fpass       (frequency band to be used in the calculation in the form
%                                   [fmin fmax])- optional.
%                                   Default all frequencies between 0 and Fs/2
%	        pad		    (padding factor for the FFT) - optional (can take values -1,0,1,2...).
%                    -1 corresponds to no padding, 0 corresponds to padding
%                    to the next highest power of 2 etc.
%			      	 e.g. For N = 500, if PAD = -1, we do not pad; if PAD = 0, we pad the FFT
%			      	 to 512 points, if pad=1, we pad to 1024 points etc.
%			      	 Defaults to 0.
%	    p		    (P-value to calculate error bars for) - optional.
%                           Defaults to 0.05/N where N is data length.
%       plt         (y/n for plot and no plot respectively) - plots the
%       Fratio at all frequencies if y
%       f0          frequencies at which you want to remove the
%                   lines - if unspecified the program
%                   will compute the significant lines
%       fscanbw     bandwidth centered on ea. f0 to scan for significant
%                   lines (TM)
%
%  Outputs:
%       datafit        (linear superposition of fitted sine waves)
%       Amps           (amplitudes at significant frequencies)
%       freqs          (significant frequencies)
%       Fval           (Fstatistic at all frequencies)
%       sig            (significance level for F distribution p value of p)
data=change_row_to_column(data);
[N,C]=size(data);
if nargin < 2 || isempty(params); params=[]; end;
[tapers,pad,Fs,fpass,err,trialave,params]=getparams(params);
clear pad fpass err trialave;
if nargin < 3 || isempty(p);p=0.05/N;end;
if nargin < 4 || isempty(plt); plt='n'; end;
if nargin < 5; f0=[]; end;
if nargin < 6; fscanbw = []; end

params.tapers=dpsschk(tapers,N,Fs); % calculate the tapers
[Fval,A,f,sig] = ftestc(data,params,p,plt);
if isempty(f0) && isempty(fscanbw)
    fmax=chron_findpeaks(Fval,sig);
    freqs=cell(1,C);
    Amps=cell(1,C);
    datafit=data;
    for ch=1:C;
        fsig=f(fmax(ch).loc);
        freqs{ch}=fsig;
        Amps{ch}=A(fmax(ch).loc,ch);
        Nf=length(fsig);
        %       fprintf('The significant lines for channel %d and the amplitudes are \n',ch);
        %        for nf=1:Nf;
        %            fprintf('%12.8f\n',fsig(nf));
        %            fprintf('%12.8f\n',real(A(fmax(ch).loc(nf),ch)));
        %            fprintf('%12.8f\n',imag(A(fmax(ch).loc(nf),ch)));
        %            fprintf('\n');
        %        end;
        datafit(:,ch)=exp(i*2*pi*(0:N-1)'*fsig/Fs)*A(fmax(ch).loc,ch)+exp(-i*2*pi*(0:N-1)'*fsig/Fs)*conj(A(fmax(ch).loc,ch));
    end;
else

    fsig = [];

    if ~isempty(fscanbw)

        % for each line f0(n), scan f0+-BW/2 for largest significant peak of Fval

        k = 1;
        for n=1:length(f0)
            % extract scan range around f0 ( f0 +- fscanbw/2 )
            [dummy ridx(1)] = min(abs(f-(f0(n)-fscanbw/2)));
            [dummy ridx(2)] = min(abs(f-(f0(n)+fscanbw/2)));

            Fvalscan = Fval(ridx(1):ridx(2));
            Fvalscan(Fvalscan<sig)=0;
            if any(Fvalscan)
                % if there's a significant line, pull the max one
                [dummy rmaxidx] = max(Fvalscan);
                indx(k) = ridx(1)+rmaxidx-1;
                fsig(k) = f(indx(k));
                k = k+1;
            end

        end
    else

        % remove exact lines

        indx = zeros( size(f0) );
        for n=1:length(f0);
            [fsig,indx(n)]=min(abs(f-f0(n)));
        end;
        fsig=f(indx);
    end

    if isempty(fsig)
        % no significant lines
        datafit = zeros(N,C);
        Amps  = cell(1,C);
        freqs = cell(1,C);
    else
        for ch=1:C;
            freqs{ch}=fsig;
            Amps{ch}=A(indx,ch);
            Nf=length(fsig);
            %        fprintf('For channel %d the amplitudes and the Fstatistic at f=%f are \n',ch,f0);
            %        fprintf('Fstatistic = %12.8f Fthreshold = %12.8f\n',Fval(indx),sig);
            %        fprintf('Real part of amplitude = %12.8f\n',real(A(indx,ch)));
            %        fprintf('Imaginary part of amplitude = %12.8f\n',imag(A(indx,ch)));
            datafit(:,ch)=exp(i*2*pi*(0:N-1)'*fsig/Fs)*A(indx,ch)+exp(-i*2*pi*(0:N-1)'*fsig/Fs)*conj(A(indx,ch));
        end;
    end
end;


function [Next,Range] = EnvironmentalSelection(Obj,RefPoint,Range,N)
% The environmental selection of AR-MOEA

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

        %% Selection among feasible solutions
        % Non-dominated sorting
        [FrontNo,MaxFNo] = NDSort(Obj,N);%population 2*N
        Next = FrontNo < MaxFNo;
        % Select the solutions in the last front
        Last   = find(FrontNo==MaxFNo);
        Choose = LastSelection(Obj(Last,:),RefPoint,Range,N-sum(Next));
        Next(Last(Choose)) = true;
        %Population = Population(Next,:);
        % Update the range for normalization
        Range(2,:) = max(Obj,[],1);
        Range(2,Range(2,:)-Range(1,:)<1e-6) = 1;
end

function Remain = LastSelection(PopObj,RefPoint,Range,K)
% Select part of the solutions in the last front

    N  = size(PopObj,1);
    NR = size(RefPoint,1);

    %% Calculate the distance between each solution and point
    Distance    = CalDistance(PopObj-repmat(Range(1,:),N,1),RefPoint);
    Convergence = min(Distance,[],2);

    %% Delete the solution which has the smallest metric contribution one by one
    [dis,rank] = sort(Distance,1);
    Remain     = true(1,N);
    while sum(Remain) > K
        % Calculate the fitness of noncontributing solutions
        Noncontributing = Remain;
        Noncontributing(rank(1,:)) = false;
        METRIC = sum(dis(1,:)) + sum(Convergence(Noncontributing));
        Metric = inf(1,N);
        Metric(Noncontributing) = METRIC - Convergence(Noncontributing);
        % Calculate the fitness of contributing solutions
        for p = find(Remain & ~Noncontributing)
            temp = rank(1,:) == p;
            noncontributing = false(1,N);
            noncontributing(rank(2,temp)) = true;
            noncontributing = noncontributing & Noncontributing;
            Metric(p) = METRIC - sum(dis(1,temp)) + sum(dis(2,temp)) - sum(Convergence(noncontributing));
        end
        % Delete the worst solution and update the variables
        [~,del] = min(Metric);
        temp    = rank ~= del;
        dis     = reshape(dis(temp),sum(Remain)-1,NR);
        rank    = reshape(rank(temp),sum(Remain)-1,NR);
        Remain(del) = false;
    end
end