function M = CGDA(X, Y, d, lamda)

% X:  training samples
% Y:  training labels
% d:  reduced dimension d * N

[~, N] = size(X);

%% compute the block-wise graph (W) based on the labels

W = zeros(N, N);
num_labels = length(unique(Y));
L = [];

for i = 1 : num_labels
    ind = (find(Y == i));
    l = length(ind);
    L = [L, l];
    x = X(:, ind);
    w = (x' * x + lamda * eye(size(x' * x))) \ (x' * x);
    if i == 1
        W(1 : l, 1 : l) = w;
    else
        W(sum(L(1 : i-1)) +1 : sum(L(1 : i)), sum(L(1 : i-1)) +1 : sum(L(1 : i))) = w;
    end
end

W = W - diag(diag(W));
W = max(W, W');

%% embedding
M = CGDA_LPP(X, d, W); % M:  learned mapping

end


function [est_map,cost,iter] = regionpixel(data,arr,col,map,te,c,d,A1,A2,A3,niter,cal)
% data = source image
% arr, col = pixel position
% map = initial field map (=0) at (arr,col)th pixel
% te = time
% c,d = cos(F_fw*te), sin(F_fw*te)
% A1 = according to Appendix A
% A2 = inv(A1'*A1)*A1'
% A3 = [1 exp(j*F_fw*te)]
% max = maximum for delta_field map
% niter = iteration number

%% initialize

iter = 0;
nsets = size(data,3); % number of te  = 3
delta_map = 11;
if cal == 1
    % cost function at (arr,col)th pixel
    cost = zeros(2,niter*2); % cost(1,:) = field map, cost(2,:) = cost value
end
% source images at (arr,col)th pixel
orig_s = reshape(data(arr,col,:),nsets,1);

%% Iteration code

% while (abs(delta_map)>1 && iter<niter && abs(delta_map)<230)
while (abs(delta_map)>1 && iter<niter)

    iter = iter+1; % number of iteration

    %% Apendix A

    s =  orig_s.*(exp(-j*2*pi*map*te).'); % shat (m is freq)
    shat = [real(s);imag(s)];
    % shat = [s1_R s2_R s3_R s1_I s2_I s3_I]
    % A2 = inv(A1'*A1)*A1'
    rowhat = A2*shat; % rowhat vector = [water_R water_I fat_R fat_I]

   if cal == 1
        %% cost function of Appendix A
        water = rowhat(1) + j*rowhat(2);
        fat = rowhat(3) + j*rowhat(4);
        cost(1,2*iter-1) = map;
        cost(2,2*iter-1) = norm(orig_s - A3*[water;fat].*(exp(j*2*pi*map*te).'));
        % cost(1,:) = field map values
        % cost(2,:) = cost function valuses
   end
    %% Apendix B

    % c_1n = 1, d_1n = 0 for all t_n, c_2n = c, d_2n = d
    f1 = -rowhat(1) -rowhat(3)*c +rowhat(4)*d;
    f2 = -rowhat(2) -rowhat(3)*d -rowhat(4)*c;

    shathat = shat + [f1;f2]; % according to  appendix B

    g = 2*pi*[te';te'].*[f2;-f1];

    B = [g A1];
    y = inv(B'*B)*B'*shathat; % y(1)is delta_fieldmap

    % update the fieldmap = prefieldmap + delta_field map
    delta_map = y(1);
    if abs(delta_map) > 30
        map = map + delta_map/abs(delta_map)*30;
    else
        map = map + delta_map;
    end

    if cal == 1
        % cost function of Appendix B
        cost(1,2*(iter)) = map;
        cost(2,2*(iter)) = norm(orig_s - A3*[water;fat].*(exp(j*2*pi*map*te).'));
    end
end
if cal == 1
    est_map = cost(1,iter*2-1);
else
    est_map = map;
    cost = 0;
end


% PCA on Fish data
tic
[coeff,score,latent,tsquared,explained,mu] = pca(M');
toc
code_dir = GetCurrentCodeDir();
save(fullfile(code_dir,'\saved mats\Fish3_full100_PCA.mat'),'coeff','score','latent','tsquared','explained','mu');

cIX_full = cIX;
gIX_full = gIX;

%%
cIX = cIX_full;
gIX = gIX_full;

%%
nPCs = 200;
figure;
im = coeff; % % rows reordered with optimal-leaf-order from dendrogram
% im = im.*repmat(explained',size(coeff,1),1); % PC's weighted by percent-explained
imagesc(im(:,1:nPCs));
% axis equal;
% set(gca,'YTick',1:size(coeff,1),'YTickLabel',ylabels);
xlim([0.5,nPCs+0.5])
xlabel('PC''s')

%% screen for cells that have high PC scores

coeff_crp = coeff(:,1:nPCs); % cropped
cellscore = abs(sum(coeff_crp,2));
[A,IX] = sort(cellscore,'descend');

percCell_PC = 0.1;
numCell = size(M,1);
nCell_PC = round(numCell*percCell_PC);
cIX = cIX(IX(1:nCell_PC));
gIX = ceil((1:length(cIX))'/length(cIX)*min(20,length(cIX)));


function [x,VAR]=least_square(v,H,P)

%normal vector
N = (H'*P*H);

%cumpute unknown parameter
x = (N^-1)*H'*P*v;

%convariance of parameter
VAR = N^-1;

return


%  Figure 6.65      Feedback Control of Dynamic Systems, 5e
%                   Franklin, Powell, Emami
%

clear all
close all;

k=10;
num=k;
den=[1 1 0];
w=logspace(-3,1,100);
[m,p]=bode(num,den,w);
num=conv(num,[10 1]);
den=conv(den,[100 1]);
[mc,pc]=bode(num,den,w);
subplot(2,1,1)
w2=[.001 10];
m2=[1 1];
loglog(w,m,w,mc,w2,m2,'r');
grid;
%xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.65 Frequency response of lag-compensation design: magnitude.');
subplot(2,1,2)
semilogx(w,p,w,pc);
grid;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
title('Fig. 6.65 Frequency response of lag compensation design: phase.');
%return;
%numcl=[1 0.1];
%dencl=[1 1.01 1.01 0.1];
%t=0:.01:20;
%y=step(numcl,dencl,t);
%plot(t,y);
%grid;
%xlabel('sec');
%ylabel('y');


function [P_st, s] = flicker_sim(u, fs, f_line)
% flicker_sim - Flickermeter Simulator according IEC 61000-4-15
%
% [P_st, s, cpf] = flicker_sim(u, fs, f_line)
%
% This function implements a flickermeter according [1] and [2].
% Requires MATLAB with Signal Procesing Toolbox installed or Octave.
% For more information refer to [3].
%
% Inputs:
%   u:      vector of voltage samples
%   fs:     sampling frequency of u in Hz (should be >= 2000)
%   f_line: line frequency in Hz (must be 50 or 60 Hz)
%
% Outputs:
%   P_st:   short term flicker
%   s:      instantaneous flicker severity
%===============================================================================
% References:
% [1] IEC 61000-4-15, Electromagnetic compatibility (EMC), Testing and
%     measurement techniques, Flickermeter, Edition 1.1, 2003-02
% [2] Wilhelm Mombauer: "Messung von Spannungsschwankungen und Flickern mit
%     dem IEC-Flickermeter", ISBN 3-8007-2525-8, VDE-Verlag
% [3] http://www.solcept.ch/en/embedded-tools/flickersim.html
%===============================================================================

%% Configuration

SHOW_TIME_SIGNALS           = 0; % enable to plot internal signals of flickermeter
SHOW_CUMULATIVE_PROBABILITY = 0; % enable to plot the statistical evaluation of the instantaneous flicker severity
SHOW_FILTER_RESPONSES       = 0; % enable to plot the filter responses of all internal filter stages (for model verification)

IS_OCTAVE = exist('OCTAVE_VERSION') ~= 0;

%% Check inputs

if (nargin ~= 3)
  error('Invalid number of arguments');
end

if (~isvector(u))
  error('First input argument must be a vector');
end
% convert to row vector if needed
u = reshape(u, 1, length(u));

if ((f_line ~= 50) && (f_line ~= 60))
  error('Line frequency must be 50 or 60 Hz');
end

if (fs < 2000)
  warning('Sampling frequency should be >= 2000 Hz');
end

%% Block 1: Input voltage adaptor

% remove DC component
u = u - mean(u);

% normalize input (scale with peak-amplitude value)
u_rms = sqrt(mean(u.^2));
u = u / (u_rms * sqrt(2));

%% Block 2: Quadratic demodulator

u_0 = u .^ 2;

%% Block 3: Bandpass and weighting filter

% bandpass filter

HIGHPASS_ORDER  = 1;
HIGHPASS_CUTOFF = 0.05;

LOWPASS_ORDER = 6;
if (f_line == 50)
  LOWPASS_CUTOFF = 35;
end
if (f_line == 60)
  LOWPASS_CUTOFF = 42;
end

% subtract DC component to limit filter transients at start of simulation
u_0_ac = u_0 - mean(u_0);

[b_hp, a_hp] = butter(HIGHPASS_ORDER, HIGHPASS_CUTOFF / (fs / 2), 'high');
u_hp = filter(b_hp, a_hp, u_0_ac);

% smooth start of signal to avoid filter transient at start of simulation
smooth_limit = min(round(fs / 10), length(u_hp));
u_hp(1 : smooth_limit) = u_hp(1 : smooth_limit) .* linspace(0, 1, smooth_limit);

[b_bw, a_bw] = butter(LOWPASS_ORDER, LOWPASS_CUTOFF / (fs / 2), 'low');
u_bw = filter(b_bw, a_bw, u_hp);

% weighting filter

if (f_line == 50)
  K = 1.74802;
  LAMBDA = 2 * pi * 4.05981;
  OMEGA1 = 2 * pi * 9.15494;
  OMEGA2 = 2 * pi * 2.27979;
  OMEGA3 = 2 * pi * 1.22535;
  OMEGA4 = 2 * pi * 21.9;
end
if (f_line == 60)
  K = 1.6357;
  LAMBDA = 2 * pi * 4.167375;
  OMEGA1 = 2 * pi * 9.077169;
  OMEGA2 = 2 * pi * 2.939902;
  OMEGA3 = 2 * pi * 1.394468;
  OMEGA4 = 2 * pi * 17.31512;
end

num1 = [K * OMEGA1, 0];
den1 = [1, 2 * LAMBDA, OMEGA1.^2];
num2 = [1 / OMEGA2, 1];
den2 = [1 / (OMEGA3 * OMEGA4), 1 / OMEGA3 + 1 / OMEGA4, 1];
if (IS_OCTAVE)
  [b_w, a_w] = bilinear(conv(num1, num2), conv(den1, den2), 1 / fs);
else
  [b_w, a_w] = bilinear(conv(num1, num2), conv(den1, den2), fs);
end

u_w = filter(b_w, a_w, u_bw);

%% Block 4: Squaring and smoothing

LOWPASS_2_ORDER  = 1;
LOWPASS_2_CUTOFF = 1 / (2 * pi * 300e-3);  % time constant 300 msec
SCALING_FACTOR   = 1238400;  % scaling of output to perceptibility scale  (according [2])

u_q = u_w .^ 2;

[b_lp, a_lp] = butter(LOWPASS_2_ORDER, LOWPASS_2_CUTOFF / (fs / 2), 'low');
s = SCALING_FACTOR * filter(b_lp, a_lp, u_q);

%% Block 5: Statistical evaluation

NUMOF_CLASSES = 10000;

[bin_cnt, cpf.magnitude] = hist(s, NUMOF_CLASSES);
cpf.cum_probability = 100 * (1 - cumsum(bin_cnt) / sum(bin_cnt));

p_50s = mean([get_percentile(cpf, 30), get_percentile(cpf, 50), get_percentile(cpf, 80)]);
p_10s = mean([get_percentile(cpf, 6), get_percentile(cpf, 8), ...
  get_percentile(cpf, 10), get_percentile(cpf, 13), get_percentile(cpf, 17)]);
p_3s = mean([get_percentile(cpf, 2.2), get_percentile(cpf, 3), get_percentile(cpf, 4)]);
p_1s = mean([get_percentile(cpf, 0.7), get_percentile(cpf, 1), get_percentile(cpf, 1.5)]);
p_0_1 = get_percentile(cpf, 0.1);

P_st = sqrt(0.0314 * p_0_1 + 0.0525 * p_1s + 0.0657 * p_3s + ...
  0.28 * p_10s + 0.08 * p_50s);

%% Optional graphical output

% time signals
if (SHOW_TIME_SIGNALS)
  t = 0 : 1 / fs : (length(u) - 1) / fs;
  filter_len = round(10 / 1000 * fs);
  u_0_m = filter(ones(1, filter_len) / filter_len * 2, 1, u_0);

  figure
  clf
  subplot(2, 2, 1)
  hold on
  plot(t, u, 'b');
  plot(t, u_0, 'm');
  plot(t, u_hp, 'r');
  plot(t, u_0_m, 'c');
  hold off
  legend('u', 'u_0', 'u_h_p');
  grid on
  subplot(2, 2, 2)
  hold on
  plot(t, u_bw, 'b');
  plot(t, u_w, 'm');
  legend('u_b_w', 'u_w');
  hold off
  grid on
  subplot(2, 2, 3)
  plot(t, u_q, 'b');
  legend('u_q');
  grid on
  subplot(2, 2, 4)
  plot(t, s, 'b');
  legend('s');
  grid on
end

% cumulative probability function
if (SHOW_CUMULATIVE_PROBABILITY)
  figure
  clf
  plot(cpf.magnitude, cpf.cum_probability);
  grid
end

% frequency responses of filters
if (SHOW_FILTER_RESPONSES)
  [h_hp, f] = freqz(b_hp, a_hp, 4096, fs);
  [h_bw, f] = freqz(b_bw, a_bw, 4096, fs);
  [h_w, f] = freqz(b_w, a_w, 4096, fs);
  [h_lp, f] = freqz(b_lp, a_lp, 4096, fs);

  figure
  clf
  subplot(2, 1, 1)
  hold on
  plot(f, abs(h_hp), 'b')
  plot(f, abs(h_bw), 'r')
  plot(f, abs(h_w), 'g')
  plot(f, abs(h_lp), 'm')
  hold off
  grid
  axis([0, 35, 0, 1]);

  subplot(2, 1, 2)
  hold on
  plot(f, 180 / pi * unwrap(angle(h_hp)), 'b')
  plot(f, 180 / pi * unwrap(angle(h_bw)), 'r')
  plot(f, 180 / pi * unwrap(angle(h_w)), 'g')
  plot(f, 180 / pi * unwrap(angle(h_lp)), 'm')
  hold off
  grid
  axis([0, 35, -200, 300]);
end

end  % end of function flicker_sim

%% Subfunction: get_percentile
function val = get_percentile(cpf, limit)
  [dummy, idx] = min(abs(cpf.cum_probability - limit));
  val = cpf.magnitude(idx);
end


 function pot = potential_func(type, delta, param)
%function pot = potential_func(type, delta, param)
%|
%| Define roughness penalty potential functions (using function handles).
%|
%| The penalty will have the form
%|	R(x) = sum_k w_k * potential_k([Cx]_k, delta_k)
%| where w_k is provided elsewhere, not here!
%|
%| in
%|	type		quad, huber, hyper2, hyper3, cauchy, lange1, lange3, ...
%|			recommended: 'hyper3'
%|	delta		scalar or image-sized array;
%|			"cutoff" parameter for edge-preserving regularization
%|	param		optional additional parameter(s) for some choices
%| out
%|	pot.delta	local copy
%|	anonymous functions:
%|	pot.potk(pot, C*x)	potential function value
%|	pot.wpot(pot, C*x)	potential 'weights' (aka half-quad. curvatures)
%|	pot.dpot(pot, C*x)	potential derivative
%|
% Copyright 2004-5-18, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if streq(type, 'test') potential_func_test, return, end

pot.delta = [];
pot.param = [];
if isvar('delta')
	pot.delta = delta(:);
	clear delta
end
if isvar('param')
	pot.param = param(:);
	clear param
end

% trick: huber2 is just a huber with delta / 2
% so that weighting function drops to 1/2 at delta, like hyper3 etc.
if streq(type, 'huber2')
	type = 'huber';
	pot.delta = pot.delta / 2;
end

% trick: hyper3 is just a hyperbola with delta scaled by sqrt(3)
% to approximately "match" the properties of 'cauchy' (old erroneous 'hyper')
if streq(type, 'hyper3')
	type = 'hyper2';
	pot.delta = pot.delta / sqrt(3);
end

switch type

%
% quadratic potential function
%
case 'quad'
	potk = '(abs(t).^2)/2';
	wpot = 'ones(size(t))';
	dpot = 't';

%
% huber potential function
%
case 'huber'
	potk = 'huber_pot(t, pot.delta)';
	wpot = 'huber_wpot(t, pot.delta)';
	dpot = 'huber_dpot(t, pot.delta)';

%
% cauchy penalty: d^2 / 2 * log(1 + (t/d)^2)
% Not convex!
%
case 'cauchy'
	potk = 'pot.delta.^2 / 2 .* log(1 + abs(t ./ pot.delta).^2)';
	wpot = '1 ./ (1 + abs(t ./ pot.delta).^2)';
	dpot = 't ./ (1 + abs(t ./ pot.delta).^2)';

%
% Geman&McClure penalty: d^2 / 2 * (t/d)^2 / (1 + (t/d)^2)
% Not convex!
%
case 'geman&mcclure'
	potk = 'pot.delta.^2 / 2 .* (t/pot.delta).^2 ./ (1 + abs(t ./ pot.delta).^2)';
	wpot = '1 ./ (1 + abs(t ./ pot.delta).^2).^2';
	dpot = 't ./ (1 + abs(t ./ pot.delta).^2).^2';

%
% hyperbola penalty: d^2 * [ sqrt(1 + (t/d)^2) - 1 ]
%
case 'hyper2'
	potk = 'pot.delta.^2 .* (sqrt(1 + abs(t ./ pot.delta).^2) - 1)';
	wpot = '1 ./ sqrt(1 + abs(t ./ pot.delta).^2)';
	dpot = 't ./ sqrt(1 + abs(t ./ pot.delta).^2)';

case 'hyper'
	error 'use "cauchy" or "hyper3" not "hyper" now'

%
% Lange1 penalty
%
case 'lange1'
	potk = 't.^2 / 2 ./ (1+abs(t./pot.delta))';
	wpot = '(1 + abs(t ./ pot.delta) / 2) ./ (1 + abs(t ./ pot.delta)).^2';
	dpot = ['t .* (' wpot ')'];

%
% Lange3 penalty
%
case 'lange3'
	potk = 'pot.delta.^2 .* (abs(t./pot.delta) - log(1+abs(t./pot.delta)))';
	wpot = '1 ./ (1 + abs(t ./ pot.delta))';
	dpot = 't ./ (1 + abs(t ./ pot.delta))';

%
% li98cfs
%
case 'li98cfs'
	% f = @(x) atan(x) / x - 0.5; fsolve(f, 2.3)
	pot.delta = pot.delta / 2.3311;
	potk = 'pot.delta.^2 .* ((t ./ pot.delta) .* atan(t ./ pot.delta) - 0.5 * log(1 + (t ./ pot.delta).^2))';
	wpot = 'ir_li98cfs_wpot(t, pot.delta)';
	dpot = ['t .* ' wpot];

%
% qgg2: q-generalized gaussian for p=2, due to Thibault, Sauer, Bouman
% q = "param", same as lange1 when q=1
%
case 'qgg2'
	potk = 't.^2 / 2 ./ (1+abs(t./pot.delta).^(2-pot.param))';
	wpot = ['(1 + abs(t ./ pot.delta).^(2-pot.param) * pot.param / 2) ' ...
		' ./ (1 + abs(t ./ pot.delta).^(2-pot.param)).^2'];
	dpot = ['t .* (' wpot ')'];

%
% genhub : "generalized" Huber, same as huber when p=2 and q=1
% p = param(1), q = param(2)
%
case 'genhub'
	potk = ['0.5 * abs(t) .^ p .* (abs(t) <= d) + ' ...
		'0.5 * (p ./ q .* d .^ (p-q) .* abs(t) .^ q + (1 - p ./ q) .* d .^ p) .* (abs(t) > d)'];
	reps = {'p', 'zp', 'q', 'zq', 'd', 'zd', ...
		'zp', 'pot.param(1)', 'zq', 'pot.param(2)', 'zd', 'pot.delta'};
	potk = strreps(potk, reps{:});
	wpot = ['p / 2 .* (abs(t) .^ (p-2)) .* (abs(t) <= d) + ' ...
		'p / 2 .* (d .^ (p-q)) .* ((abs(t) + (abs(t)==0)) .^ (q-2)) .* (abs(t) > d)']; % trick to avoid 0^-:
	wpot = strreps(wpot, reps{:});
	dpot = ['t .* (' wpot ')'];

%
% stevenson:94:dpr
% p = param(1), q = param(2), same as huber when p=2 and q=1 ???
%
case 'stevenson94dpr'
	potk = ['0.5 * abs(t) .^ p .* (abs(t) <= d) + ' ...
		'0.5 * ( (p .* d .^ (p-1) .* abs(t) - p .* d .^ p + (1 ./ q) .^ (1 ./ (q-1)) ) .^ q + d .^ p - (1 ./ q) .^ (q ./ (q-1)) ) .* (abs(t) > d)'];
	reps = {'p', 'zp', 'q', 'zq', 'd', 'zd', ...
		'zp', 'pot.param(1)', 'zq', 'pot.param(2)', 'zd', 'pot.delta'};
	potk = strreps(potk, reps{:});
	wpot = 'ones(size(t))'; % fix: fake for now
	dpot = ['t .* (' wpot ')'];

otherwise
	try % trick: try new strum version
		p = potential_fun(type, pot.delta, pot.param);
		pot.potk = @(dum, t) p.potk(t);
		pot.wpot = @(dum, t) p.wpot(t);
		pot.dpot = @(dum, t) p.dpot(t);
	catch
		fail('Unknown potential "%s"', type)
	end
end

if ~isfield(pot, 'potk')
	pot.potk = ir_str2func(['@(pot,t) ' potk]);
	pot.wpot = ir_str2func(['@(pot,t) ' wpot]);
	pot.dpot = ir_str2func(['@(pot,t) ' dpot]);
end

% test routine
% examine potential functions after rescaling.
function potential_func_test

delta = 10; tmax = 4;
plist = {'quad', 'huber2', 'hyper3', 'lange1', 'lange3', ...
	'cauchy', 'qgg2', 'gf1'};
%plist = {'quad', 'li98cfs', 'hyper3', 'huber2'}; % show li98cfs roughly hyper3
%plist = {'huber', 'genhub', 'quad'};%, 'stevenson94dpr'};
%plist = {'hyper3', 'qgg2'}; delta = 20; tmax = 10;
%plist = {'huber', 'hyper3', 'qgg2'}; delta = 10; tmax = 8;
%plist = {'qgg2', 'gf1'};
t = tmax * linspace(-delta, delta, 401)';
for ii=1:length(plist)
	type = plist{ii};
	leg{ii} = [plist{ii} ' \delta = ' num2str(delta)];
	if streq(type, 'qgg2')
		param = 1.2;
		leg{ii} = [leg{ii} ' q = ' num2str(param)];
	elseif streq(type, 'gf1')
		param = [0.0558 1.6395];
	elseif streq(type, 'genhub')
		param = [1.9 1.1];
		leg{ii} = [leg{ii} sprintf('p=%g q=%g', param(1), param(2))];
	elseif streq(type, 'stevenson94dpr')
		param = [2 2.01];
		leg{ii} = [leg{ii} sprintf('p=%g q=%g', param(1), param(2))];
	else
		param = [];
	end
	pot = potential_func(type, delta, param);
	pp(:,ii) = pot.potk(pot, t);
	pw(:,ii) = pot.wpot(pot, t);
	pd(:,ii) = pot.dpot(pot, t);
end

if im
	clf
	subplot(411), plot(t, pp), title 'potk'
	axis tight
	axisy([-0.0 2.5] * delta^2)
	legend(leg, 'location', 'north')
	subplot(412), plot(t, pw), title 'wpot'
	axisy(0, 1.1)
	subplot(413), plot(t, pd), title 'dpot'
	axisy([-1 1] * 1.1 * delta)
	% check derivatives
	subplot(414)
	plot(t, pd)
	title 'dpot check'
	hold on
	d = diffc(pp) / (t(2)-t(1));
	plot(t(1:end-1), d(1:end-1,:), '--')
	hold off
	axisy([-1 1] * 1.1 * delta)
end


function pde = elli3DlinIntf2(bm,bp,delta)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 12/15/2020 by GRC

%% PDE Structure
pde = struct('intf',@intf,'f',@f,'fm',@fm,'fp',@fp,'exactu',@exactu,...
    'um',@um,'up',@up,'Dxu',@Dxu,'Dxum',@Dxum,'Dxup',@Dxup,'Dyu',@Dyu,...
    'Dyum',@Dyum,'Dyup',@Dyup,'Dzu',@Dzu,'Dzum',@Dzum,'Dzup',@Dzup,...
    'A',@A,'Am',@Am,'Ap',@Ap,'one',@one,'gD',@gD);

pde.bm = bm;
pde.bp = bp;
%% interface function
    function u = intf(x,y,z)
        u = (x-delta);
    end

%% exact solution
    function u = exactu(x,y,z)
        u = um(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up(x(id),y(id),z(id));
    end
    function u = um(x,y,z)
        r = (1+delta)*sin((x-delta)*pi/(1+delta)).*sin(pi*y).*sin(pi*z);
        u = r/bm;
    end
    function u = up(x,y,z)
        r = (1-delta)*sin((x-delta)*pi/(1-delta)).*sin(pi*y).*sin(pi*z);
        u = r/bp;
    end
%% Boundary Function
    function u = gD(x,y,z)
        u = exactu(x,y,z);
    end
%% Derivative of the exact solution
    function u = Dxu(x,y,z)
        u = Dxum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dxup(x(id),y(id),z(id));
    end
    function u = Dxum(x,y,z)
        r = cos((x-delta)*pi/(1+delta)).*sin(pi*y).*sin(pi*z)*pi;
        u = r/bm;
    end
    function u = Dxup(x,y,z)
        r = cos((x-delta)*pi/(1-delta)).*sin(pi*y).*sin(pi*z)*pi;
        u = r/bp;
    end

    function u = Dyu(x,y,z)
        u = Dyum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dyup(x(id),y(id),z(id));
    end
    function u = Dyum(x,y,z)
        r = (1+delta)*sin((x-delta)*pi/(1+delta)).*cos(pi*y).*sin(pi*z)*pi;
        u = r/bm;
    end
    function u = Dyup(x,y,z)
        r = (1-delta)*sin((x-delta)*pi/(1-delta)).*cos(pi*y).*sin(pi*z)*pi;
        u = r/bp;
    end

    function u = Dzu(x,y,z)
        u = Dzum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dzup(x(id),y(id),z(id));
    end
    function u = Dzum(x,y,z)
        r = (1+delta)*sin((x-delta)*pi/(1+delta)).*sin(pi*y).*cos(pi*z)*pi;
        u = r/bm;
    end
    function u = Dzup(x,y,z)
        r = (1-delta)*sin((x-delta)*pi/(1-delta)).*sin(pi*y).*cos(pi*z)*pi;
        u = r/bp;
    end

%% right hand side function
    function u = f(x,y,z)
        u = fm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp(x(id),y(id),z(id));
    end
    function u = fm(x,y,z)
        r = sin((x-delta)*pi/(1+delta)).*sin(pi*y).*sin(pi*z)*pi^2;
        u = r*(1/(1+delta)+2*(1+delta));
    end
    function u = fp(x,y,z)
        r = sin((x-delta)*pi/(1-delta)).*sin(pi*y).*sin(pi*z)*pi^2;
        u = r*(1/(1-delta)+2*(1-delta));
    end

%% Diffusion coefficient function
    function u = A(x,y,z)
        u = Am(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Ap(x(id),y(id),z(id));
    end
    function u = Am(x,y,z)
        u = bm*ones(size(x));
    end
    function u = Ap(x,y,z)
        u = bp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
end


function [x,err,k] = perform_conjugate_gradient(A,y,options)

% perform_conjugate_gradient - perform conjugate gradient
%
%     [x,err,k] = perform_conjugate_gradient(A,y,options);
%
%   Solves for A*x=y.
%   Works both for vector x,y and matrix x,y (parralel soving of many
%   linear equations).
%
%   Works only for semi-definite matrices.
%
%   A can be a matrix or a callback function y=A(x,options).
%   In this case, you have to set options.ncols as the number of columns of
%   A (if it is a callback).
%
%   err monitos the decreasing of |A*x-y|.
%   k is the total number of iterations.
%
%   You can set:
%       options.x is an initial guess
%       options.epsilon is maximum error
%       options.niter_max is maximum number of error
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
if isfield(options, 'niter_max')
    niter = options.niter_max;
else
    niter = 100;
end

use_callback = 0;
if not(isnumeric(A))
    use_callback = 1;
end
if isfield(options, 'x')
    x = options.x;
else
    if use_callback==0
        x = zeros(size(A,2),1);
    else
        if isfield(options, 'ncols')
            x = zeros(options.ncols, 1);
        else
            error('You have to specify options.ncols');
        end
    end
end
if isfield(options, 'epsilon')
    epsilon = options.epsilon;
else
    epsilon = 1e-5;
end

if norm(y, 'fro') == 0
    normb = epsilon;
else
    normb = epsilon * sum(y(:).^2);
end

if use_callback==0
    r  = y - A*x;
else
    r  = y - feval(A,x,options);
end
r0 = sum(r.^2);
err = [sum(r0)];
for it=1:niter
    if (it==1)
        p = r;
    else
        % search direction
        beta = r0./rprev;
        p = r + repmat(beta, [size(x,1) 1]).*p;
    end
    % auxiliary vector
    if use_callback==0
        w  = A*p;
    else
        w  = feval(A,p,options);
    end
    alpha = repmat( r0 ./ sum(p .* w), [size(x,1) 1] );              % found optimal alpha in line search
    x = x + alpha.*p;                       % new guess
    r = r - alpha.*w;                       % the residual is in fact r=b-A*x
    rprev = r0;                             % save norm of the old residual
    r0 = sum(r.^2);                         % compute norm of new residual
    err(end+1) = sum(r0);
    if err(end)<normb
        return;
    end
end

return;

err = [sqrt(r0)];
while ( sqrt(r0) > normb && k < kmax)
    if( k==1 )
        p = r;
    else
        beta = r0/rprev;
        % search direction
        p = r + beta*p;
    end
    if use_callback==1
        w = feval(A,p,options);
    else
        w = A * p;                          %% auxiliary vector
    end
    alpha = r0 / (p' * w);              %% found optimal alpha in line search
    x = x + alpha*p;                    %% new guess
    r = r - alpha*w;                    %% the residual is in fact r=b-A*x
    rprev = r0;                         %% ... save norm of the old residual
    r0 = ( norm(r, 'fro') )^2;                 %% ... compute norm of new residual
    k = k + 1;
    err(end+1) = sqrt(r0);
end


% [thresh,lower,cost] = onedim2class(x,l,costmatrix,weights,lower)
% inputs:
% x is the N x 1 matrix of N unidimensional data points
% l is the N x 1 matrix of N corresponding labels, where l(i) can
% be either -1 or 1
% thresh is the best threshold such that almost everything on one
% side of the threshold is of one class and almost of everything on
% the other side is the other class.
% optional:
% costmatrix is the 2 x 2 matrix. the cost of misclassifying a point with
% label costmatrix(1,1) is costmatrix(1,2) and the cost of misclassifying a
% point with label costmatrix(2,1) is costmatrix(2,2). by default, this is
% [-1 1 ; 1 1]. set to [] to use the default.
% weights is a vector of length n, where w(n) is the weight of the nth
% sample. set to [] to use default w(n) = 1 for all n.
% lower is a scalar. if lower == 1, then class 1 must be below the
% threshold and class -1 must be above. if lower == -1, then class
% -1 must be below the threshold and class 1 must be above. if
% lower == 0, then the best ordering is chosen
function [thresh,lower,cost] = onedim2class(x,l,costmatrix,weights,lower)

% parse inputs
n = length(x);
if nargin < 3 || isempty(costmatrix),
  costmatrix = [-1 1 ; 1 1];
else
  if numel(costmatrix) ~= 4 || rows(costmatrix) ~= 2,
    error('costmatrix must be 2 x 2');
  end;
  if abs(costmatrix(1,1)) ~= 1 || abs(costmatrix(2,1)) ~= 1,
    error('costmatrix(1,1) and constmatrix(2,1) can either be 1 or -1');
  end;
  if costmatrix(1,1) == costmatrix(2,1),
    error('costmatrix(1,1) == costmatrix(2,1)');
  end;
end;
if nargin < 4 || isempty(weights),
  weights = ones(n,1);
end

if nargin < 5,
  lower = 0;
else
  if abs(lower) ~= 1 && lower ~= 0,
    error('lower can be -1, 0, or 1');
  end;
end;
x = x(:);
l = l(:);
if length(x) ~= length(l),
  error('x and l must both be N x 1 vectors');
end;
classes = unique(l);
if length(classes) ~= 2,
  error('l must contain exactly 2 unique values, -1 and 1');
end;

% sort the data
% Sort the data based on response value
[sortedx,sortedi] = sort(x);
sortedl = l(sortedi);
sortedweights = weights(sortedi);

% collapse entries of x that are identical
isid = [false;diff(sortedx) == 0];
if any(isid),
  [startid,endid] = get_interval_ends(isid);
  for i = 1:length(startid),
    t0 = startid(i)-1;
    t1 = endid(i)-1;
    tmp = sum(sortedweights(t0:t1).*sortedl(t0:t1));
    sortedweights(t0) = abs(tmp);
    sortedl(t0) = sign(tmp);
  end
  sortedx(isid) = [];
  sortedl(isid) = [];
  sortedweights(isid) = [];
end

if lower == 0,
  [thresh_lowerpos,cost_lowerpos] = main(sortedx, sortedl, costmatrix, sortedweights, 1);
  [thresh_lowerneg,cost_lowerneg] = main(sortedx, sortedl, costmatrix, sortedweights, -1);
  if cost_lowerpos < cost_lowerneg,
    thresh = thresh_lowerpos;
    cost = cost_lowerpos;
    lower = 1;
  else
    thresh = thresh_lowerneg;
    cost = cost_lowerneg;
    lower = -1;
  end;
else
  [thresh,cost] = main(sortedx, sortedl, costmatrix, sortedweights,lower);
end;

function [thresh,cost] = main(x, l, costmatrix, weights, lower)

N = length(x);
labels = l;

l(labels==costmatrix(1,1)) = costmatrix(1,1)*costmatrix(1,2).*weights(labels==costmatrix(1,1));
l(labels==costmatrix(2,1)) = costmatrix(2,1)*costmatrix(2,2).*weights(labels==costmatrix(2,1));
suml1 = cumsum([0;l]);
suml2 = [flipud(cumsum(flipud(l)));0];
if lower == -1,
  ncorrect = -suml1 + suml2;
else
  ncorrect = suml1 - suml2;
end
[maxncorrect,besti] = max(ncorrect);

% Choose threshold to be in the middle
if besti == 1,
  thresh = x(1)-eps;
elseif besti == N+1,
  thresh = x(N)+eps;
else
  thresh = mean(x(besti-1:besti));
end;
cost = -maxncorrect;


function [logp, yhat, res] = tapas_unitsq_sgm_mu3(r, infStates, ptrans)
% Calculates the log-probability of response y=1 under the unit-square sigmoid model
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 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/>.

% Initialize returned log-probabilities as NaNs so that NaN is
% returned for all irregualar trials
n = size(infStates,1);
logp = NaN(n,1);
yhat = NaN(n,1);
res  = NaN(n,1);

% Weed irregular trials out from inferred states and responses
mu1hat = infStates(:,1,1);
mu1hat(r.irr) = [];
mu3hat = infStates(:,3,1);
mu3hat(r.irr) = [];
y = r.y(:,1);
y(r.irr) = [];

% Decision temperature is exponential of log-volatility
% (i.e., inverse decision temperature is exponential of negative log-volatility)
ze = exp(-mu3hat);

% Avoid any numerical problems when taking logarithms close to 1
x = mu1hat;
logx = log(x);
log1pxm1 = log1p(x-1);
logx(1-x<1e-4) = log1pxm1(1-x<1e-4);
log1mx = log(1-x);
log1pmx = log1p(-x);
log1mx(x<1e-4) = log1pmx(x<1e-4);

% Calculate log-probabilities for non-irregular trials
reg = ~ismember(1:n,r.irr);
logp(reg) = y.*ze.*(logx -log1mx) +ze.*log1mx -log((1-x).^ze +x.^ze);
yhat(reg) = x;
res(reg) = (y-x)./sqrt(x.*(1-x));

return;


% VL_HOMKERMAP Homogeneous kernel map
%   V = VL_HOMKERMAP(X, N) computes a 2*N+1 dimensional approximated
%   kernel map for the Chi2 kernel. X is an array of data points. Each
%   point is expanded into a vector of dimension 2*N+1 and saved to
%   the output V. The expanded feature vectors are stacked along the
%   first dimension, so that the output array V has the same
%   dimensions of the input array X except for the first one, which is
%   2*N+1 times larger.
%
%   The function accepts the following options:
%
%   KChi2::
%     Compute the map for the Chi2 kernel.
%
%   KINTERS::
%     Compute the map for the intersection kernel.
%
%   KL1::
%     Same as KINTERS, but deprecated as the name is not fully
%     accurate.
%
%   KJS::
%     Compute the map for the JS (Jensen-Shannon) kernel.
%
%   Period:: [automatically tuned]
%     Set the period of the kernel specturm. The approximation is
%     based on periodicizing the kernel specturm. If not specified,
%     the period is automatically set based on the heuristic described
%     in [2].
%
%   Window:: [RECTANGULAR]
%     Set the window used to truncate the spectrum before The window
%     can be either RECTANGULAR or UNIFORM window. See [2] and the API
%     documentation for details.
%
%   Gamma:: [1]
%     Set the homogeneity degree of the kernel. The standard kernels
%     are 1-homogeneous, but sometimes smaller values perform better
%     in applications. See [2] for details.
%
%   Example::
%     The following code results in approximatively the same
%     similarities matrices between points X and Y:
%
%       x = rand(10,1) ;
%       y = rand(10,100) ;
%       psix = vl_homkermap(x, 3) ;
%       psiy = vl_homkermap(y, 3) ;
%       figure(1) ; clf ;
%       ker = vl_alldist(x, y, 'kchi2') ;
%       ker_ = psix' * psiy ;
%       plot([ker ; ker_]') ;
%
%   Note::
%     The homogeneous kernels K(X,Y) are normally defined for
%     non-negative data only. VL_HOMKERMAP defines them for both
%     positive and negative data by using the definition
%     SIGN(X)SIGN(Y)K(ABS(X),ABS(Y)) -- note that other extensions are
%     possible as well (see [2]).
%
%   REFERENCES::
%     [1] A. Vedaldi and A. Zisserman
%     `Efficient Additive Kernels via Explicit Feature Maps',
%     Proc. CVPR, 2010.
%
%     [2] A. Vedaldi and A. Zisserman
%     `Efficient Additive Kernels via Explicit Feature Maps',
%     PAMI, 2011 (submitted).
%
%   See also: VL_HELP().

% Authors: Andrea Vedaldi

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).


function x = rationalBezierCurve(p,w,t,tSpan)
% x = rationalBezierCurve(p,w,t,tSpan)
%
% This function evaluates a rational bezier curve, defined by points p and
% weights w.
%
% INPUTS:
%   p = [nCurve x nPoint] = control points
%   w = [1 x nPoint] = control point weights
%   t = [1 x nTime] = times at which to evaluate curve
%   tSpan = [1 x 2] = tSpan(1) <= t <= tSpan(2)
%
% OUTPUTS:
%   x = [nCurve x nTime] = bezier curve, evaluated at t
%
% NOTES:
%   It is not advisable to use this function for high-order polynomials.
%
% See also: BEZIERCURVE, FITBEZIERCURVE
%

[nCurve, nPoint] = size(p);
nTime = length(t);

t = (t-tSpan(1))/diff(tSpan);

%%% Compute the numerator and denominator:
num = zeros(nCurve,nTime);
den = zeros(nCurve,nTime);
ONE = ones(nCurve,1);
n = nPoint - 1;
for i=0:n
    tt = (t.^i).*(1-t).^(n-i);
    binom = nchoosek(n,i);
    num = num + binom *w(i+1)*p(:,i+1)*tt;
    den = den + binom *w(i+1)*ONE*tt;
end
x = num./den;

end


clear all;
n=400000;
p=1000;
density=0.01;

% generate random data
format compact;
randn('seed',0);
rand('seed',0);
X=sprandn(p,n,density);
nrm=sqrt(sum(X.^2));
X=X/mean(nrm);
%mean_nrm=mean();
%X=X/mean_nrm;

% generate some true model
z=double(sign(full(sprandn(p,1,0.05))));
y=X'*z;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EXPERIMENT 1: Lasso
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('EXPERIMENT FOR LASSO\n');
nrm=sqrt(sum(y.^2));
y=y+0.01*nrm*randn(n,1);    % add noise to the model
nrm=sqrt(sum(y.^2));
y=y*(sqrt(n)/nrm);

clear param;
param.regul='l1';        % many other regularization functions are available
param.loss='square';     % only square and log are available
param.numThreads=-1;    % uses all possible cores
param.normalized=false;  % if the columns of X have unit norm, set to true.
param.averaging_mode=0;  % no averaging, averaging was not really useful in experiments
param.weighting_mode=0;  % weights are in O(1/sqrt(n))  (see help mexStochasticProx)
param.optimized_solver=true;  % best is not to touch this option
param.verbose=false;

% set grid of lambda
max_lambda=max(abs(X*y))/n;
tablambda=max_lambda*(2^(1/4)).^(0:-1:-20);  % order from large to small
param.lambda=tablambda;    % best to order from large to small
tabepochs=[1 2 3 5 10];  % in this script, we compare the results obtained when varying the number of passes over the data.

%% The problem which will be solved is
%%   min_beta  1/(2n) ||y-X' beta||_2^2 + lambda ||beta||_1
fprintf('EXPERIMENT: ALL LAMBDAS IN PARALLEL\n');
% we try different experiments when varying the number of epochs.
% the problems for different lambdas are solve INDEPENDENTLY in parallel
obj=[];
objav=[];
for ii=1:length(tabepochs)
   param.iters=tabepochs(ii)*n;
   fprintf('EXP WITH %d PASS\n',tabepochs(ii));
   nlambdas=length(param.lambda);
   Beta0=zeros(p,nlambdas);
   tic
   [Beta tmp]=mexStochasticProx(y,X,Beta0,param);
   toc
   yR=repmat(y,[1 nlambdas]);
   fprintf('Objective functions: \n');
   obj=[obj; 0.5*sum((yR-X'*Beta).^2)/n+param.lambda.*sum(abs(Beta))];
   obj
   if param.averaging_mode
      objav=[objav; 0.5*sum((yR-X'*tmp).^2)/n+param.lambda.*sum(abs(tmp))];
      objav
   end
   fprintf('Sparsity: \n');
   sum(Beta ~= 0)
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EXPERIMENT 2: L2 logistic regression
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('EXPERIMENT FOR LOGISTIC REGRESSION + l2\n');
y=sign(y);
param.regul='l2';
param.loss='logistic';
obj=[];
objav=[];
for ii=1:length(tabepochs)
   param.iters=tabepochs(ii)*n;   % one pass over the data
   fprintf('EXP WITH %d PASS\n',tabepochs(ii));
   nlambdas=length(param.lambda);
   Beta0=zeros(p,nlambdas);
   tic
   [Beta tmp]=mexStochasticProx(y,X,Beta0,param);
   toc
   yR=repmat(y,[1 nlambdas]);
   fprintf('Objective functions: \n');
   obj=[obj;sum(log(1.0+exp(-yR .* (X'*Beta))))/n+0.5*param.lambda.*sum(abs(Beta.^2))];
   obj
   if param.averaging_mode
      objav=[objav; sum(log(1.0+exp(-yR .* (X'*tmp))))/n+0.5*param.lambda.*sum(abs(tmp))];
      objav
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EXPERIMENT 3: L1 logistic regression
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('EXPERIMENT FOR LOGISTIC REGRESSION + l1\n');
y=sign(y);
param.regul='l1';        % many other regularization functions are available
param.loss='logistic';     % only square and log are available
obj=[];
objav=[];
for ii=1:length(tabepochs)
   param.iters=tabepochs(ii)*n;   % one pass over the data
   fprintf('EXP WITH %d PASS\n',tabepochs(ii));
   nlambdas=length(param.lambda);
   Beta0=zeros(p,nlambdas);
   tic
   [Beta tmp]=mexStochasticProx(y,X,Beta0,param);
   toc
   yR=repmat(y,[1 nlambdas]);
   fprintf('Objective functions: \n');
   obj=[obj; sum(log(1.0+exp(-yR .* (X'*Beta))))/n+param.lambda.*sum(abs(Beta))];
   obj
   if param.averaging_mode
      objav=[objav; sum(log(1.0+exp(-yR .* (X'*tmp))))/n+param.lambda.*sum(abs(tmp))];
      objav
   end
   fprintf('Sparsity: \n');
   sum(Beta ~= 0)
end


function beta = LpAnalysis(H, Ht, A, At, y, p, normfac, err, insweep, decfac, tol)

% Algorithm for solving problems of the form:
% min ||Ax||_p s.t. ||y-Hx||_2 < err

% Inputs
% A - Sparsifying Forward Transform
% At - Sparsifying Backward Transform
% H - Forward Measurement Operator
% Ht - Backward Measurement Operator
% y - collected data
% p - non-convex norm
% normfac - highest singular value of A (default 1)
% err - two norm mismatch (default 1e-6)
% insweep - number of sweeps for internal loop (default 50)
% decfac - lambda decrease factor for outside loop (default 0.5)
% tol - tolerance for convergence (default 1e-4)

% copyright (c) Angshul Majumdar 2010

c = 1.1;

if nargin < 6
    normfac=1;
end
if nargin < 7
    p = 1;
end
if nargin < 8
    err = 1e-6;
end
if nargin < 9
    insweep = 50;
end
if nargin < 10
    decfac = 0.5;
end
if nargin < 11
    tol = 1e-4;
end

alpha = 1.1*normfac;
x_initial = Ht(y);
z_initial = zeros(length(A(x_initial)),1);

x = x_initial; z = z_initial;

lambdaInit = decfac*max(abs(Ht(y))); lambda = lambdaInit;

f_current = norm(y-H(x_initial)) + lambda*norm(A(x),p);

while lambda > lambdaInit*tol
    % debug lambda
    for ins = 1:insweep
        f_previous = f_current;

        b = x + (1/alpha)*(Ht(y-H(x)));
        z = (c*z + A(b-At(z)))./((2*alpha/lambda)*(abs(A(x))).^(2-p)+c);
        x = b - At(z);

        f_current = norm(y-H(x)) + lambda*norm(A(x),p);

        if norm(f_current-f_previous)/norm(f_current + f_previous)<tol
            break;
        end
    end
    if norm(y-H(x))<err
        break;
    end
    lambda = decfac*lambda;
end
beta = x;


function y = ssrfunc(x)

% sun-synchronous repeating ground track
% orbit objective function

% required by ssrepeat.m

% input

%  x = array of current dependent variables

% output

%  y = function value array evaluated at x

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global j2 mu req omega xldot

global ecc xqp

y = zeros(2, 1);

% "unload" current solution

sma = x(1);
inc = x(2);

% determine current values of nonlinear system

mm = sqrt(mu / sma ^ 3);

slr = sma * (1.0 - ecc * ecc);

cinc = cos(inc);

% compute perturbations

raandot = -1.5 * j2 * mm * (req / slr) ^ 2 * cinc;

apdot = 0.75 * j2 * mm * (req / slr) ^ 2 * (5.0 * cinc ^ 2 - 1);

madot = mm + 0.75 * j2 * mm * (req / sma) ^ 2 * (3.0 * cinc ^ 2 - 1) ...
    / (1.0 - ecc * ecc) ^ 1.5;

% sun-synchronous inclination equation

y(1) = cinc + 2.0 * xldot * sma ^ 3.5 * (1 - ecc * ecc) ^ 2 ...
    / (3 * j2 * req * req * sqrt(mu));

% repeating groundtrack equation

y(2) = (1.0 / xqp) / (omega - raandot) - (1.0 / (apdot + madot));


%{
load('dataset/trafficdb/traffic_patches.mat');
[M,m,n,p] = convert_video3d_to_2d(im2double(imgdb{100}));
out = run_algorithm('MC', 'MC_logdet', M, [])
show_results(M.*out.Omega,out.L,out.S,out.O,p,m,n);
%}

mu = 0.1; % 0.1; 0.01;
kappa = 1.2; % 1.2; 1.0;
toler = 1e-10;
maxiter = 1000;
L = MC_LogDet_v3(M,Omega,mu,kappa,toler,maxiter);
S = M - L;


function x = daub14_transform_inverse ( n, y )

%*****************************************************************************80
%
%% DAUB14_TRANSFORM_INVERSE inverts the DAUB14 transform of a vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the vector.
%    N must be a power of 2 and at least 4.
%
%    Input, real Y(N), the transformed vector.
%
%    Output, real X(N), the original vector.
%
  c = [ ...
     7.785205408500917e-02; ...
     3.965393194819173e-01; ...
     7.291320908462351e-01; ...
     4.697822874051931e-01; ...
    -1.439060039285649e-01; ...
    -2.240361849938749e-01; ...
     7.130921926683026e-02; ...
     8.061260915108307e-02; ...
    -3.802993693501441e-02; ...
    -1.657454163066688e-02; ...
     1.255099855609984e-02; ...
     4.295779729213665e-04; ...
    -1.801640704047490e-03; ...
     3.537137999745202e-04 ];
  p = 13;
  x(1:n,1) = y(1:n);
  m = 4;
  q = floor ( ( p - 1 ) / 2 );

  while ( m <= n )

    z(1:m,1) = 0.0;

    j = 1;

    mh = floor ( m / 2 );

    for i = - q + 1 : mh - q

      for k = 0 : 2 : p - 1
        i0 = i4_wrap ( i      + k / 2,     1,      mh );
        i1 = i4_wrap ( i + mh + k / 2,     mh + 1, m  );
        z(j,1)   = z(j,1)   + c(p-k)   * x(i0) + c(k+2) * x(i1);
        z(j+1,1) = z(j+1,1) + c(p-k+1) * x(i0) - c(k+1) * x(i1);
      end

      j = j + 2;

    end

    x(1:m,1) = z(1:m);

    m = m * 2;

  end

  return
end


function varargout = quiver(N, varargin)
% QUIVER   Draw phase plot diagrams, based on ODEs specified with CHEBOPS
%
% Calling sequence:
%   H = QUIVER(N, AXIS, 'OPT1', VAL1, ...)
%
% Here, the inputs are:
%   N    : A chebop, whose N.op arguments specifies a first or second order
%          scalar ODE, or a coupled system of two first order ODEs.
%   AXIS : A 4-vector with elements [XMIN XMAX YMIN YMAX] that specify the
%          rectangular region shown on the phase plot. If none is passed, the
%          default values [-1 1 -1 1] are used.
%
% It is possible to pass the method various option pairs of the form
% 'OPTIONNAME', OPTIONVALUE. The options supported are:
%   'XPTS'      : An integer, specifying the resolution of the x-axis for the
%                 quiver plot. Default value: 10.
%   'YPTS'      : An integer, specifying the resolution of the y-axis for the
%                 quiver plot. Default value: 10.
%   'NORMALIZE' : A Boolean, which determines whether the arrows on the quiver
%                 plot or normalized all to have the same length. Default value:
%                 false.
%   'SCALE':      By default, quiver automatically scales the arrows to fit
%                 within the grid. By passing a SCALE argument S, the arrows are
%                 fitted within the grid and then stretched by S. Use S = 0 to
%                 plot the arrows without the automatic scaling. See the
%                 documentation for the built-in MATLAB QUIVER for more
%                 information.
%   LINESPEC      Specifies options for the quiver plot. The LINESPEC argument
%                 can either be a string supported by the MATLAB PLOT command,
%                 such as 'ro', or a parameter/value pair to specify additional
%                 properties of the quiver lines, such as
%                   quiver(N,[0 2 0 2], 'k', 'linewidth', 1.4).
%
% The optional output is
%   H   : A quivergroup handle.
%
% Note: The CHEBOP QUIVER command works by reformulating higher order problems
% as coupled first order systems, evaluating the resulting first order system at
% grid that should be interpreted as values of u and u', then calling the
% built-ing MATLAB QUIVER method on the results. In the case of first order
% scalar problems, the grid should be interpreted as values of t and u.
%
% Example 1 -- van der Pol equation (second order ODE)
%   N = chebop(0,100);
%   N.op = @(t,u) diff(u, 2) - 3*(1-u^2)*diff(u) + u;
%   quiver(N,[-2.5 2.5 -5.5 5.5], 'r', 'xpts', 40, 'ypts', 40, 'scale', .5, ...
%       'normalize', true, 'linewidth', 1.5)
%   hold on % Plot a particular solution on top of quiver plot
%   N.lbc = [2; 0];
%   u = N\0;
%   plot(u, diff(u),'linewidth',2)
%
% Example 2 -- Lotka-Volterra (first order coupled system)
%   N = chebop(@(t,u,v) [diff(u)-2.*u+u.*v; diff(v)+v-u.*v], [0 4]);
%   quiver(N, [0 2.5 0 4])
%   hold on
%   N.lbc = @(u,v) [u - 0.5; v - 1]; % Initial populations
%   [u, v] = N\0;
%   plot(u, v, 'linewidth', 2)
%   plot(0.5, 1,'m*','markersize',15) % Mark initial condition
%
% Example 3 -- Slopefield for a first order problem
%   N = chebop(@(t,u) diff(u)-sin(t)*u);
%   quiver(N,[-1.2*pi 1.2*pi -1 1])

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

% Set default values:
scale = 1;
normalize = false;
u0 = [];
xpts = 20;
ypts = 20;
axisLims = [-1 1 -1 1];

% The limits for the axes have to appear in the first entry of varargin if
% they're specified. They'll always be a 4-vector.
if ( ~isempty(varargin) && length(varargin{1}) == 4 )
    axisLims = varargin{1};
    % Throw away the axisLims:
    varargin(1) = [];
end

% Cell for linespecs
linespec = {};

% Parse VARARGIN, go through all elements:
while ( ~isempty(varargin) )
    if ( ~ischar(varargin{1}) && ~isnumeric(varargin{2}) )
        error('followpath:inputArgument','Incorrect options input arguments');
    end

    throwAwayLength = 2;
    switch lower(varargin{1})
        case 'normalize'
            normalize = varargin{2};
        case 'xpts'
            xpts = varargin{2};
        case 'ypts'
            ypts = varargin{2};
        case 'scale'
            scale = varargin{2};
        otherwise
            % Must have gotten something to do with linespec
            findNorm = strcmpi(varargin, 'normalize');
            findXpts = strcmpi(varargin, 'xpts');
            findYpts = strcmpi(varargin, 'ypts');
            findScale = strcmpi(varargin, 'scale');
            optionPos = findNorm | findXpts | findYpts | findScale;
            nextOption = find(optionPos, 1, 'first');
            % If nextOption is empty, we only got linespec options left
            if ( isempty(nextOption) )
                nextOption = length(varargin) + 1;
            end
            linespec = [linespec, varargin{1:nextOption - 1}];
            throwAwayLength = nextOption - 1;
    end

    % Throw away option name and argument and move on:
    varargin(1:throwAwayLength) = [];
end

% Extract the x and y limits:
xl = axisLims(1:2);
yl = axisLims(3:4);

% If ylim is empty, we solve the problem to obtain a range for plotting on:
if ( isempty(xl) )
    u0 = N\0;
    xl = 1.1*minandmax(u0);
end

if ( isempty(yl) )
    if ( isempty(u0) )
        u0 = N\0;
    end
    yl = 1.1*minandmax(diff(u0));
end

% Convert the operator in N to first order.
[firstOrderFun, ~, ~, ~, diffOrd] = treeVar.toFirstOrder(N.op, 0, N.domain);

% Vectors for constructing a meshgrid:
y1 = linspace(xl(1), xl(end), xpts);
y2 = linspace(yl(1), yl(end), ypts);

% Get a meshgrid for points of interests in the phase plane.
[x,y] = meshgrid(y1, y2);
u = zeros(size(x));
v = zeros(size(x));

% Phase plane portraits really only make sense for autonomous systems, which
% shouldn't depend on t, hence, we simply take t = 0 for evaluating.
t = 0;

% Check if we got passed a system of too high order:
numEquations = length(strfind(func2str(firstOrderFun),';'));

if ( numEquations > 2 )
    error('CHEBFUN:CHEBOP:quiver:tooHighOrder', ...
        ['The ODE passed to chebop/quiver must either be a scalar, second ' ...
        'order ODE or a system of two first order equations.'])
end

% Are we plotting a slope field (cf. #2238), or a standard phase plane?
if( (length(diffOrd) == 1) && (diffOrd == 1))   % Slope field
    for i = 1:numel(x)
        res = firstOrderFun(x(i), y(i));
        u(i) = 1;
        v(i) = res(1);
    end
else                                            % Phase plane
    for i = 1:numel(x)
        res = firstOrderFun(t,[x(i); y(i)]);
        u(i) = res(1);
        v(i) = res(2);
    end
end

if ( normalize )
    % Make all arrows equal length:
    nrm = sqrt(u.^2 + v.^2);
    u = u./nrm;
    v = v./nrm;
end

h = quiver(x, y, u, v, scale,linespec{:});

% Set x and y limits:
xlim(xl);
ylim(yl);
if ( nargout > 0 )
    varargout{1} = h;
else

end


function Test_fixedrank_2factors()
% function Test_LR()
% Test for fixedrankLRquotientfactory geometry (low rank matrix completion)
%
% Paper link: http://www.icml-2011.org/papers/350_icmlpaper.pdf
%

% This file is part of Manopt: www.manopt.org.
% Original author: Bamdev Mishra, July 11, 2013.
% Contributors:
% Change log:

    clear all; clc; close all;

    % Problem
    m = 20;
    n = 20;
    r = 8;
    A = randn(m, r);
    B = randn(n, r);
    C = A*B';

    % Create the problem structure

%     problem.M = fixedrankfactory_2factors(m, n, r);
    %     problem.M = fixedrankfactory_2factors_preconditioned(m, n, r);
        problem.M = fixedrankfactory_2factors_subspace_projection(m, n, r);

    df = problem.M.dim();
    p = 8*df/(m*n);
    %     mask = rand(m, n) <= p;
    mask = ones(m,n );

    problem.cost = @cost;
    function f = cost(X)
        f = .5*norm(mask.*(X.L*X.R' - C), 'fro')^2;
    end

    problem.grad = @(X) problem.M.egrad2rgrad(X, egrad(X));
    function g = egrad(X)
        P = mask.^2 .* (X.L*X.R' - C);
        g.L = P*X.R;
        g.R = P'*X.L;
    end

    problem.hess = @(X, U) problem.M.ehess2rhess(X, egrad(X), ehess(X, U), U);
    function Ress = ehess(X, eta)
        P = (mask.^2).*( X.L*X.R' - C);
        Pdot = (mask.^2).*(eta.L*X.R' + X.L*eta.R');
        Ress.L = Pdot*X.R + P*eta.R;
        Ress.R = Pdot'*X.L + P'*eta.L;
    end

    % % Check numerically whether gradient and Ressian are correct
    checkgradient(problem);
    drawnow;
    pause;
    checkhessian(problem);
    drawnow;
    pause;

    % Initialization
    X0 = [];

    % Options (not mandatory)
    options.maxiter = inf;
    options.maxinner = 30;
    options.maxtime = 120;
    options.tolgradnorm = 1e-5;
    options.Delta_bar = min(m ,n )*r;
    options.Delta0 = min(m ,n )*r/64;

    % Pick an algorithm to solve the problem
    [Xopt costopt info] = trustregions(problem, X0, options);
    % [Xopt costopt info] = steepestdescent(problem, X0, options);
    % [Xopt costopt info] = conjugategradient(problem, X0, options);

    evs = real(hessianspectrum(problem, Xopt));
    evs = real(evs);
    max(evs)/min(evs)
    stairs(sort(evs));
    title(['Eigenvalues of the Hessian of the cost function ' ...
        'at the solution']);
end


function templates_test ( )

%*****************************************************************************80
%
%% TEMPLATES_TEST tests the TEMPLATES library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 March 2006
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEMPLATES_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the TEMPLATES library.\n' );

  bicg_test ( );
  bicgstab_test ( );
  cg_test ( );
  cgs_test ( );
  cheby_test ( );
  gmres_test ( );
  gs_test ( );
  jacobi_test ( );
  lehmer_test ( );
  mm_to_msm_test ( );
  poisson_test ( );
  qmr_test;
  sor_test ( );
  wathen_test ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEMPLATES_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


% created: Zoya Bylinskii, Aug 2014

% This finds the KL-divergence between two different saliency maps when
% viewed as distributions: it is a non-symmetric measure of the information
% lost when saliencyMap is used to estimate fixationMap.

function score = KLdiv(saliencyMap, fixationMap)
% saliencyMap is the saliency map
% fixationMap is the human fixation map

map1 = im2double(imresize(saliencyMap, size(fixationMap)));
map2 = im2double(fixationMap);

% make sure map1 and map2 sum to 1
if any(map1(:))
    map1 = map1/sum(map1(:));
end

if any(map2(:))
    map2 = map2/sum(map2(:));
end

% compute KL-divergence
score = sum(sum(map2 .* log(eps + map2./(map1+eps))));


function [bestEpsilon, bestF1] = selectThreshold(yval, pval)
    %% SELECTTHRESHOLD Find the best threshold (epsilon) to use for selecting
    %   outliers
    %   [bestEpsilon bestF1] = SELECTTHRESHOLD(yval, pval) finds the best
    %   threshold to use for selecting outliers based on the results from a
    %   validation set (pval) and the ground truth (yval).
    %

    bestEpsilon = 0;
    bestF1 = 0;
    F1 = 0;

    stepsize = (max(pval) - min(pval)) / 1000;
    for epsilon = min(pval) : stepsize : max(pval)
        % ====================== YOUR CODE HERE ======================
        % Instructions: Compute the F1 score of choosing epsilon as the
        %               threshold and place the value in F1. The code at the
        %               end of the loop will compare the F1 score for this
        %               choice of epsilon and set it to be the best epsilon if
        %               it is better than the current choice of epsilon.
        %
        % Note: You can use predictions = (pval < epsilon) to get a binary vector
        %       of 0's and 1's of the outlier predictions
        predictions = (pval < epsilon);  % entry = 1 if anomalous

        % calculate how well we did
        tp = sum(predictions == 1 & yval == 1);
        fp = sum(predictions == 1 & yval == 0);
        fn = sum(predictions == 0 & yval == 1);

        % calculate precision and recall
        precision = tp / (tp + fp);
        recall = tp / (tp + fn);
        F1 = (2 * precision * recall) / (precision + recall);

        % =============================================================

        if F1 > bestF1
           bestF1 = F1;
           bestEpsilon = epsilon;
        end

        %fprintf('tp=%d, fp=%d, fn=%d, precision=%f, recall=%f, best=%f was F1=%f\n', ...
        %    tp, fp, fn, precision, recall, bestEpsilon, bestF1);
    end
end


function linplus_test03 ( )

%*****************************************************************************80
%
%% TEST03 tests R83_CR_FA, R83_CR_SL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST03\n' );
  fprintf ( 1, '  For a real tridiagonal matrix,\n' );
  fprintf ( 1, '  using CYCLIC REDUCTION,\n' );
  fprintf ( 1, '  R83_CR_FA factors;\n' );
  fprintf ( 1, '  R83_CR_SL solves.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
  fprintf ( 1, '  The matrix is NOT symmetric.%d\n' );
  fprintf ( 1, '\n' );
%
%  Set the matrix values.
%
  a(1,1) = 0.0E+00;
  for j = 2 : n
    a(1,j) = j;
  end

  for j = 1 : n
    a(2,j) = 4.0E+00 * j;
  end

  for j = 1 : n - 1
    a(3,j) = j;
  end
  a(3,n) = 0.0E+00;

  r83_print ( n, a, '  The matrix:' );
%
%  Set the desired solution.
%
  x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
  b = r83_mxv ( n, a, x );
  x(1:n) = 0.0E+00;
%
%  Factor the matrix.
%
  a_cr = r83_cr_fa ( n, a );
%
%  Solve the linear system.
%
  x = r83_cr_sl ( n, a_cr, b );

  r8vec_print_some ( n, x, 10, '  Solution:' );

  return
end


% Question No: 4

% Detect the line segments in a binary image using Hough transform

function houghtr(x)
im=imread(x);
f=edge(im,'canny');
figure,imshow(f),title('Image after applying Canny Filter');
[h,theta,rho]=hough1(f,0.5);
imshow(theta,rho,h,[],'notruesize'),axis on,axis normal;
xlabel('\theta'),ylabel('\rho');
[r,c]=houghpeaks1(h,5);
hold on;
plot(theta(c),rho(r),'linestyle','none','marker','s','color','w');
lines=houghlines1(f,theta,rho,r,c);
figure,imshow(f),hold on;
for k=1:length(lines)
    xy=[lines(k).point1;lines(k).point2];
    plot(xy(:,2),xy(:,1),'Linewidth',4,'Color',[.6 .6 .6]);
end
end

function [h,theta,rho]=hough1(f,dtheta,drho)
if nargin<3
    drho=1;
end
if nargin<2
    dtheta=1;
end
f=double(f);
[m,n]=size(f);
theta=linspace(-90,0,ceil(90/dtheta)+1);
theta=[theta -fliplr(theta(2:end-1))];
ntheta=length(theta);
d=sqrt((m-1)^2+(n-1)^2);
q=ceil(d/drho);
nrho=2*q-1;
rho=linspace(-q*drho,q*drho,nrho);
[x,y,val]=find(f);
x=x-1;y=y-1;
h=zeros(nrho,length(theta));
for k=1:ceil(length(val)/1000)
    first=(k-1)*1000+1;
    last=min(first+999,length(x));
    x_matrix=repmat(x(first:last),1,ntheta);
    y_matrix=repmat(y(first:last),1,ntheta);
    val_matrix=repmat(val(first:last),1,ntheta);
    theta_matrix=repmat(theta,size(x_matrix,1),1)*pi/180;
    rho_matrix=x_matrix.*cos(theta_matrix)+y_matrix.*sin(theta_matrix);
    slope=(nrho-1)/(rho(end)-rho(1));
    rho_bin_index=round(slope*(rho_matrix-rho(1))+1);
    theta_bin_index=repmat(1:ntheta,size(x_matrix,1),1);
    h=h+full(sparse(rho_bin_index(:),theta_bin_index(:),val_matrix(:),nrho,ntheta));
end
end

function [r,c,hnew]=houghpeaks1(h,numpeaks,threshold,nhood)
if nargin<4
    nhood=size(h)/50;
    nhood=max(2*ceil(nhood/2)+1,1);
end
if nargin<3
    threshold=0.5*max(h(:));
end
if nargin<2
    numpeaks=1;
end
done=false;
hnew=h;r=[];c=[];
while ~done
    [p,q]=find(hnew==max(hnew(:)));
    p=p(1);q=q(1);
    if hnew(p,q)>=threshold
        r(end+1)=p;
        c(end+1)=q;
        p1=p-(nhood(1)-1)/2;
        p2=p+(nhood(1)-1)/2;
        q1=q-(nhood(2)-1)/2;
        q2=q+(nhood(2)-1)/2;
        [pp,qq]=ndgrid(p1:p2,q1:q2);
        pp=pp(:);qq=qq(:);
        badrho=find((pp<1)|(pp>size(h,1)));
        pp(badrho)=[];
        qq(badrho)=[];
        theta_too_low=find(qq<1);
        qq(theta_too_low)=size(h,2)+qq(theta_too_low);
        pp(theta_too_low)=size(h,1)-pp(theta_too_low)+1;
        theta_too_high=find(qq>size(h,2));
        qq(theta_too_high)=qq(theta_too_high)-size(h,2);
        pp(theta_too_high)=size(h,1)-pp(theta_too_high)+1;
        hnew(sub2ind(size(hnew),pp,qq))=0;
        done=length(r)==numpeaks;
    else
        done=true;
    end
end
end

function [r,c]=houghpixels1(f,theta,rho,rbin,cbin)
[x,y,val]=find(f);
x=x-1;
y=y-1;
theta_c=theta(cbin)*pi/180;
rho_xy=x*cos(theta_c)+y*sin(theta_c);
nrho=length(rho);
slope=(nrho-1)/(rho(end)-rho(1));
rho_bin_index=round(slope*(rho_xy-rho(1))+1);
idx=find(rho_bin_index==rbin);
r=x(idx)+1;
c=y(idx)+1;
end

function lines=houghlines1(f,theta,rho,rr,cc,fillgap,minlength)
if nargin<6
    fillgap=20;
end
if nargin<7
    minlength=40;
end
numlines=0;
lines=struct;
for k=1:length(rr)
    rbin=rr(k);
    cbin=cc(k);
    [r,c]=houghpixels1(f,theta,rho,rbin,cbin);
    if isempty(r)
        continue
    end
    omega=(90-theta(cbin))*pi/180;
    t=[cos(omega) sin(omega);-sin(omega) cos(omega)];
    xy=[r-1 c-1]*t;
    x=sort(xy(:,1));
    diff_x=[diff(x);Inf];
    idx=[0;find(diff_x>fillgap)];
    for p=1:length(idx)-1
        x1=x(idx(p)+1);
        x2=x(idx(p+1));
        linelength=x2-x1;
        if linelength>=minlength
            point1=[x1 rho(rbin)];
            point2=[x2 rho(rbin)];
            tinv=inv(t);
            point1=point1*tinv;
            point2=point2*tinv;
            numlines=numlines+1;
            lines(numlines).point1=point1+1;
            lines(numlines).point2=point2+1;
            lines(numlines).length=linelength;
            lines(numlines).theta=theta(cbin);
            lines(numlines).rho=rho(rbin);
        end
    end
end
end


function [Y,L,S,params] = loadSyntheticProblem(varargin)
% [Y,L,S,params] = loadSyntheticProblem(problemName, referenceSolutionDirectory)
%
% Returns parameters and solution for the following problems
%   (parameters are tuned so that each problem has the
%    the same true solution (L,S) )
% Or, call this with no input arguments to set the path...
%
% "Lag": min   lambdaL*||L||_* + lambdaS*||S||_1 + .5||L+S-Y||_F^2
%
% "Sum": min   ||L||_* + lambdaSum*||S||_1
%        subject to ||L+S-Y||_F <= epsilon
%
% "Max": min   max( ||L||_* , lambdaMax*||S||_1  )
%        subject to ||L+S-Y||_F <= epsilon
%
% "Dual-sum"  min  .5||L+S-Y||_F^2
%         subject to ||L||_* + lambdaSum*||S||_1  <= tauSum
%
% "Max-sum"  min  .5||L+S-Y||_F^2
%         subject to max( ||L||_*, lambdaMax*||S||_1)  <= tauMax
%
% so params has the following fields:
%   .lambdaL
%   .lambdaS
%   .lambdaSum
%   .lambdaMax
%   .tauSum
%   .tauMax
%   .epsilon
%
%   .objective  This is a function of (L,S) and is the objective
%               of the "Lagrangian" formulation
%
%   .L1L2   set to 0, 'rows', or 'sum' for using l1 or l1l2 block norms
%
% Stephen Becker, March 6 2014

% addpath for the proxes and solvers
Mfilename = mfilename('fullpath');
baseDirectory = fileparts(Mfilename);
addpath(genpath(baseDirectory));

if nargin==0
    % we just setup the path, nothing else...
    return;
end

if nargin >= 2
    baseDirectory = varargin{2};
else
    baseDirectory = fullfile(baseDirectory,'referenceSolutions');
end
norm_nuke = @(L) sum(svd(L,'econ'));
vec = @(X) X(:);
problemString = varargin{1};

%%
% -- Common problem strings --
% problemString = 'small_randn_v2';
% problemString = 'medium_randn_v1';
% problemString = 'medium_exponentialNoise_v1';
% problemString = 'large_exponentialNoise_square_v1';
% problemString = 'large_exponentialNoise_rectangular_v1';
% problemString = 'huge_exponentialNoise_rectangular_v1'; % not yet done!
% problemString = 'nsa500_SNR80_problem1_run1';

fprintf('%s\n** Problem %s **\n\n',char('*'*ones(60,1)), problemString );
fileName      = fullfile(baseDirectory,[problemString,'.mat']);
if exist(fileName,'file')
    fprintf('Loading reference solution from file %s\n', fileName);
    load(fileName); % should contain L, S and Y at least, and preferably lambdaL and lambdaS
else
    [L,S,Y] = deal( [] );
end
% Default:
L1L2  = 0;

% For problems from the NSA software package:
if length(problemString)>3 && strcmpi( problemString(1:3), 'nsa' )
    % This sscanf line is broken in versions of Matlab after R2010a
    tmp = sscanf(problemString,'nsa%d_SNR%d_problem%d_run%d');
    m       = tmp(1); % either 500, 1000 or 1500
    SNR     = tmp(2); % either 80 or 45
    problem_type = tmp(3);
    run_counter  = tmp(4);
    % e.g.,  'nsa500_SNR80_problem1_run1'
    n  = m;
%     % Variable parameters
%     m = 500; n = m;
%     problem_type = 1; % between 1 and 4. Controls rank/sparsity
%     SNR          = 80;
%     run_counter  = 1; % between 1 and 10. Only affects seed

%     % Possible sparsity amounts:
%     p_array = ceil( m*n*[ 0.05, 0.1, 0.05, 0.1 ] );
%     r_array = ceil(min(m,n)*[  0.05, 0.05, 0.1, 0.1 ] );

    % Changing the conventions, March 14. They had 4 cases, for p = {.05,.1}
    %   and r = {.05, .1 } (these are sparsity and rank percentages)
    % I don't want to do that many tests since we will display graphically
    %   and not in tables. I also want more values of p and r. So I will
    %   make p and r be linked.
    p_array = ceil( m*n*[ 0.05, 0.1, 0.2, 0.3 ] );
    r_array = ceil(min(m,n)*[  0.05, 0.1, 0.2, 0.3 ] );

    p = p_array(problem_type);
    r = r_array(problem_type);
    pwr = r+p/(m*n)*100^2/3;
    stdev = sqrt(pwr/10^(SNR/10));
    delta = sqrt(min(m,n)+sqrt(8*min(m,n)))*stdev;
    lambdaSum = 1/sqrt( max(m,n) ); % "xi" in NSA
else
    switch problemString
        case 'small_randn_v1'
            my_rng(101);
            m   = 40; n = 42; lambdaL = .5; lambdaS = 1e-1;
            if isempty(Y)
                Y   = randn(m,n);
                printEvery = 500; tol = 1e-12;  maxIts = 3e3;
            end

        case 'small_randn_v2'
            my_rng(102);
            m   = 40; n = 42; lambdaL = .5; lambdaS = 1e-1;
            if isempty(Y)
                Y   = randn(m,n);
                printEvery = 500; tol = 1e-12; maxIts = 3e3;
            end
        case 'medium_randn_v1'
            my_rng(101);
            m   = 400; n = 500; lambdaL = .25; lambdaS = 1e-2;
            if isempty(Y)
                Y   = randn(m,n);
                printEvery = 1; %tol = 1e-4; maxIts = 100;
                tol = 1e-10; maxIts = 1e3;
            end
        case 'medium_exponentialNoise_v1'
            % Converges much faster than 'medium_randn_v1'
            my_rng(101);
            m   = 400; n = 500; lambdaL = .25; lambdaS = 1e-2;
            rk  = round(.5*min(m,n) );
            Q1  = haar_rankR(m,rk,false);
            Q2  = haar_rankR(n,rk,false);
            Y   = Q1*diag(.1+rand(rk,1))*Q2';
            mdn = median(abs(Y(:)));
            Y   = Y + exprnd( .1*mdn, m, n );

            printEvery = 5; tol = 1e-10; maxIts = 1000;

        case 'medium_exponentialNoise_v2'
            % Converges much faster than 'medium_randn_v1'
            % our quasi-Newton scheme does well here
            my_rng(101);
            m   = 400; n = 500; lambdaL = .25; lambdaS = 1e-2;
            rk  = round(.05*min(m,n) );
            Q1  = haar_rankR(m,rk,false);
            Q2  = haar_rankR(n,rk,false);
            Y   = Q1*diag(.1+rand(rk,1))*Q2';
            mdn = median(abs(Y(:)));
            Y   = Y + exprnd( .1*mdn, m, n );

            printEvery = 5; tol = 1e-10; maxIts = 1000;

        case 'medium_exponentialNoise_v3'
            % Converges much faster than 'medium_randn_v1'
            my_rng(101);
            m   = 400; n = 500; lambdaL = .25; lambdaS = 1e-2;
            rk  = round(.1*min(m,n) );
            Q1  = haar_rankR(m,rk,false);
            Q2  = haar_rankR(n,rk,false);
            Y   = Q1*diag(.1+rand(rk,1))*Q2';
            mdn = median(abs(Y(:)));
            Y   = Y + exprnd( .1*mdn, m, n );

            printEvery = 5; tol = 1e-10; maxIts = 1000;

        case 'large_exponentialNoise_square_v1'
            my_rng(201);
            m   = 1e3; n = m; lambdaL = .25; lambdaS = 1e-2;
            rk  = round(.5*min(m,n) );
            Q1  = haar_rankR(m,rk,false);
            Q2  = haar_rankR(n,rk,false);
            Y   = Q1*diag(.1+rand(rk,1))*Q2';
            mdn = median(abs(Y(:)));
            Y   = Y + exprnd( .1*mdn, m, n );

            printEvery = 5; tol = 1e-8; maxIts = 250;

        case 'large_exponentialNoise_rectangular_v1'
            my_rng(201);
            m   = 5e3; n = 200; lambdaL = .25; lambdaS = 5e-3; % OK
            rk  = round(.5*min(m,n) );
            Q1  = haar_rankR(m,rk,false);
            Q2  = haar_rankR(n,rk,false);
            Y   = Q1*diag(.1+rand(rk,1))*Q2';
            mdn = median(abs(Y(:)));
            Y   = Y + exprnd( .1*mdn, m, n );

            printEvery = 5; tol = 1e-8; maxIts = 100;

        case 'huge_exponentialNoise_rectangular_v1';
            disp('Warning: lambda values not yet tuned');
            my_rng(301);
            m   = 5e4; n = 200; lambdaL = .25; lambdaS = 5e-2;
            rk  = round(.5*min(m,n) );
            Q1  = haar_rankR(m,rk,false);
            Q2  = haar_rankR(n,rk,false);
            Y   = Q1*diag(.1+rand(rk,1))*Q2';
            mdn = median(abs(Y(:)));
            Y   = Y + exprnd( .1*mdn, m, n );

            printEvery = 5; tol = 1e-10; maxIts = 100;

        case 'escalator_small';
            % added March 29 2015
            load escalator_data_2015_small % has Y, mat, m, n, nFrames
            m   = m*n; % 64 x 79, 200 frames
            n   = nFrames;
            lambdaL     = 3;
            lambdaS     = 0.06;
            printEvery  = 1;
            tol         = 1e-9;
            maxIts      = 100;
            L0          = Y;
            S0          = 0*Y;

        case 'verySmall_l1'
            load verySmall_l1
            % Used CVX to generate solution

        case 'verySmall_l1l2'
            load verySmall_l1l2
            % Used CVX to generate solution
            L1L2 = 'rows';

        otherwise
            error('bad problem name');
    end
end

% Generate new reference soln if not precomputed
if isempty(L)
    fprintf('Generating reference solution...\n');
    if length(problemString)>3 && strcmpi( problemString(1:3), 'nsa' )
        % We solve Sum problem, which gives us a lot of parameters but not
        % the parameters for the Lagrangian problem unfortunately.

        % I am changing the seeds format
        seed = 601;
        my_rng( seed + run_counter );
        [Y, optimal_L, optimal_S, deltabar]=create_data_noisy_L2(m,n,p,r,stdev);

        tol = 0.0001;
        denoise_flag=0;

        %         [L,S,out] = nsa(Y,stdev,tol,denoise_flag,optimal_L,optimal_S);
        %         [L,S,out] = nsa(Y,-delta,tol,denoise_flag,optimal_L,optimal_S);

        [L,S,out] = nsa(Y,-delta,tol,denoise_flag,[],[],lambdaSum);
        fprintf('Discrepancy with intitial signal L is %.2e\n', ...
            norm(L-optimal_L,'fro')/norm(L,'fro') );
        fprintf('Discrepancy with intitial signal S is %.2e\n', ...
            norm(S-optimal_S,'fro')/norm(S,'fro') );

        lambdaL     = [];
        lambdaS     = [];
        maxIts      = [];
    else
        tic
        opts    = struct('printEvery',printEvery,'FISTA',true,'tol',tol,'maxIts',maxIts);
        %[L,S,errHist] = solver_RPCA_Lagrangian_simple(Y,lambdaL,lambdaS,[],opts);
        opts.quasiNewton  = true; opts.FISTA=false; opts.BB = false;
        opts.SVDstyle   = 1; % for highest accuracy
        if exist('L0','var')
            opts.L0     = L0;
        end
        if exist('S0','var')
            opts.S0     = S0;
        end
        [L,S,errHist] = solver_RPCA_Lagrangian(Y,lambdaL,lambdaS,[],opts);
        toc
    end

    save(fileName,'L','S','Y','lambdaL','lambdaS','tol','maxIts');
end

sigma   = svd(L,'econ');
rk      = sum( sigma > 1e-8 );
normL   = sum(sigma);

params = [];
params.L1L2        = L1L2;
if L1L2
    if strcmpi(L1L2,'rows')
        params.objective    = @(L,S) lambdaL*norm_nuke(L)+lambdaS*sum( sqrt( sum(S.^2,2) ) ) + ...
            .5*norm(vec(L+S-Y))^2;
        normS   = sum( sqrt( sum(S.^2,2) ) );
    elseif strcmpi(L1L2,'cols')
        params.objective    = @(L,S) lambdaL*norm_nuke(L)+lambdaS*sum( sqrt( sum(S.^2,1) ) ) + ...
            .5*norm(vec(L+S-Y))^2;
        normS   = sum( sqrt( sum(S.^2,1) ) );
    else error('bad value for L1L2');
    end
else
    params.objective    = @(L,S) lambdaL*norm_nuke(L)+lambdaS*norm(S(:),1) + ...
        .5*norm(vec(L+S-Y))^2;
    normS   = norm(S(:),1);
end
params.epsilon     = norm( L + S - Y, 'fro' );
params.lambdaL     = lambdaL;
params.lambdaS     = lambdaS;
if exist('lambdaSum','var')
    params.lambdaSum   = lambdaSum;
else
    params.lambdaSum   = lambdaS/lambdaL;
end
params.lambdaMax   = normL/normS;
params.tauSum      = normL + params.lambdaSum*normS;
params.tauMax      = max(normL,params.lambdaMax*normS );

fprintf('\tSize %d x %d\n\tL has nuclear norm %.3f and rank %d of %d possible\n', ...
    m,n,normL, rk, length(sigma) );
fprintf('\tS has l1 (or l1l2) norm %.3f and %.2f%% of its elements are nonzero\n', ...
    normS, 100*sum( abs(S(:)) > 1e-8)/numel(S) );
fprintf('\t||L+S-Y||/||Y|| is %.2e\n', params.epsilon/norm(Y,'fro' ) );

end % end of main function

function Q = haar_rankR(n, r, cmplx)
% Q = HAAR_RANKR( N , R)
%   returns a N x N unitary matrix Q
%   from the Haar measure on the
%   Circular Unitary Ensemble (CUE)
%
% Q = HAAR_RANKR( N, R, COMPLEX )
%   if COMPLEX is false, then returns a real matrix from the
%   Circular Orthogonal Ensemble (COE). By default, COMPLEX = true
%
% For more info, see http://www.ams.org/notices/200705/fea-mezzadri-web.pdf
% "How to Generate Random Matrices fromthe Classical Compact Groups"
% by Francesco Mezzadri (also at http://arxiv.org/abs/math-ph/0609050 )
%
% Other references:
%   "How to generate a random unitary matrix" by Maris Ozols
%
% Stephen Becker, 2/24/11. updated 11/1/12 for rank r version.
%
% To test that the eigenvalues are evenly distributed on the unit
% circle, do this:   a = angle(eig(haar(1000))); hist(a,100);
%
% This calls "randn", so it will change the stream of the prng

if nargin < 3, cmplx = true; end
if nargin < 2 || isempty(r) , r=n; end

if cmplx
    z = (randn(n,r) + 1i*randn(n,r))/sqrt(2.0);
else
    z = randn(n,r);
end
[Q,R] = qr(z,0); % time consuming
d = diag(R);
ph = d./abs(d);
% Q = multiply(q,ph,q) % in python. this is q <-- multiply(q,ph)
%   where we multiply each column of q by an element in ph

%Q = Q.*repmat( ph', n, 1 );
%  use bsxfun for this to make it fast...
Q = bsxfun( @times, Q, ph' );

end % end of haar_rankR

function varargout = my_rng(varargin)
%MY_RNG Control the random number generator used by RAND, RANDI, and RANDN (SRB version)
%   MY_RNG(SD) seeds the random number generator using the non-negative
%   integer SD so that RAND, RANDI, and RANDN produce a predictable
%   sequence of numbers.
% MODIFIED BY STEPHEN BECKER
%   See also RAND, RANDI, RANDN, RandStream, NOW.

%   See Choosing a Random Number Generator for details on these generators.

if exist('rng','file')
    [varargout{1:nargout}] = rng(varargin{:});
    return;
end
% Otherwise, imitate the functionality...

% -- SRB adding this --
error(nargchk(1,1,nargin));
error(nargoutchk(0,0,nargout));
arg1 = varargin{1};
% For R2008a, this doesn't work... (not sure what earliest version is)
if verLessThan('matlab','7.7')
    randn('state',arg1);
    rand('state',arg1);
else
    if verLessThan('matlab','8.2')
        RandStream.setDefaultStream(RandStream('mt19937ar', 'seed', arg1) );
    else
        RandStream.setGlobalStream(RandStream('mt19937ar', 'seed', arg1) );
    end
end
end % end of my_rng


%%***********************************************************
%% lmiexamp1: generate SDP data for the following LMI problem
%%
%%  max  -eta
%%  s.t. B*P + P*B'  <= 0
%%        -P         <= -I
%%         P - eta*I <= 0
%%         P(1,1)     = 1
%%***********************************************************
%% Here is an example on how to use this function to
%% find an optimal P.
%%
%% B = [-1  0  0; 5 -2  0; 1  1 -1];
%% [blk,At,C,b] = lmiexamp1(B);
%% [obj,X,y,Z] = sqlp(blk,At,C,b);
%% P = smat(blk(1,:),y);
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 20 Apr 02
%%***********************************************************

   function [blk,At,C,b] = lmiexamp1(B);

   n = length(B); n2 = n*(n+1)/2;
   I  = speye(n);
   z0 = sparse(n2,1);
   blktmp{1,1} = 's'; blktmp{1,2} = n;
%%
   blk{1,1} = 's'; blk{1,2} = n;
   blk{2,1} = 's'; blk{2,2} = n;
   blk{3,1} = 's'; blk{3,2} = n;
   blk{4,1} = 'u'; blk{4,2} = 1;
%%
   At{1,1} = [lmifun(B,I),     z0];
   At{2,1} = [lmifun(-I/2,I),  z0];
   At{3,1} = [lmifun(I/2,I),   svec(blktmp,-I,1)];
   At{4,1} = sparse([1, zeros(1,n2)]);
%%
   C{1,1} = sparse(n,n);
   C{2,1} = -speye(n);
   C{3,1} = sparse(n,n);
   C{4,1} = 1;
%%
   b = [zeros(n2,1); -1];
%%**********************************************************


function test_suite = test_mirdwt
  disp("mrdwt")
  test_mirdwt_1
  test_mirdwt_2

function test_mirdwt_1
       xin = makesig('Leopold',8);
       h = daubcqf(4,'min');
       Lin = 1;
       [yl,yh,L] = mrdwt(xin,h,Lin);
       [x,L] = mirdwt(yl,yh,h,L);

assertEqual(L,Lin);
assertVectorsAlmostEqual(x, xin,'relative',0.0001);

function test_mirdwt_2
       load ../lena512;
       x = lena512;
       h = daubcqf(6);
       [yl,yh,L] = mrdwt(x,h);
assertEqual(L,9);
       [x_new,L] = mirdwt(yl,yh,h);
assertEqual(L,9);
assertVectorsAlmostEqual(x, x_new,'relative',0.0001);


function [samples, energies, diagn] = metrop2(f, x, opt, gradf, varargin)
%METROP2	Markov Chain Monte Carlo sampling with Metropolis algorithm.
%
%	Description
%	SAMPLES = METROP(F, X, OPT) uses the Metropolis algorithm to
%	sample from the distribution P ~ EXP(-F), where F is the first
%	argument to METROP.   The Markov chain starts at the point X and each
%	candidate state is picked from a Gaussian proposal distribution and
%	accepted or rejected according to the Metropolis criterion.
%
%	SAMPLES = METROP(F, X, OPT, [], P1, P2, ...) allows additional
%	arguments to be passed to F().  The fourth argument is ignored, but
%	is included for compatibility with HMC and the optimizers.
%
%	[SAMPLES, ENERGIES, DIAGN] = METROP(F, X, OPT) also returns a log
%	of the energy values (i.e. negative log probabilities) for the
%	samples in ENERGIES and DIAGN, a structure containing diagnostic
%	information (position and acceptance threshold) for each step of the
%	chain in DIAGN.POS and DIAGN.ACC respectively.  All candidate states
%	(including rejected ones) are stored in DIAGN.POS.
%
%	S = METROP('STATE') returns a state structure that contains the state
%	of the two random number generators RAND and RANDN. These are
%	contained in fields randstate,  randnstate.
%
%	METROP('STATE', S) resets the state to S.  If S is an integer, then
%	it is passed to RAND and RANDN. If S is a structure returned by
%	METROP('STATE') then it resets the generator to exactly the same
%	state.
%
%       See METROP2_OPT for the optional parameters in the OPTIONS
%       structure.
%
%	See also
%	HMC, METROP2_OPT
%

%	Copyright (c) Christopher M Bishop, Ian T Nabney (1996, 1997)
%	Copyright (c) 1998-2000 Aki Vehtari

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

% Global variable to store state of momentum variables: set by set_state
% Used to initialize variable if set
global HMC_MOM
if nargin <= 2
  if ~strcmp(f, 'state')
    error('Unknown argument to metrop2');
  end
  switch nargin
   case 1
    samples = get_state(f);
    return;
   case 2
    set_state(f, x);
    return;
  end
end

% Set empty omptions to default values
opt=metrop2_opt(opt);
% Refrence to structures is much slower, so...
opt_nsamples=opt.nsamples;
opt_display =opt.display;
stddev = opt.stddev;

% Set up string for evaluating potential function.
%f = fcnchk(f, length(varargin));

nparams = length(x);
samples = zeros(opt_nsamples, nparams);		% Matrix of returned samples.
if nargout >= 2
  en_save = 1;
  energies = zeros(opt_nsamples, 1);
else
  en_save = 0;
end
if nargout >= 3
  diagnostics = 1;
  diagn_pos = zeros(opt_nsamples, nparams);
  diagn_acc = zeros(opt_nsamples, 1);
else
  diagnostics = 0;
end

% Main loop.
k = - opt.nomit + 1;
Eold = f(x, varargin{:});	% Evaluate starting energy.
nreject = 0;				% Initialise count of rejected states.
while k <= opt_nsamples

  xold = x;
  % Sample a new point from the proposal distribution
  x = xold + randn(1, nparams)*stddev;

  % Now apply Metropolis algorithm.
  Enew = f(x, varargin{:});	% Evaluate new energy.
  a = exp(Eold - Enew);			% Acceptance threshold.
  if (diagnostics & k > 0)
    diagn_pos(k,:) = x;
    diagn_acc(k,:) = a;
  end
  if (opt_display > 1)
    fprintf(1, 'New position is\n');
    disp(x);
  end

  if a > rand(1)	% Accept the new state.
    Eold = Enew;
    if (opt_display > 0)
      fprintf(1, 'Finished step %4d  Threshold: %g\n', k, a);
    end
  else			% Reject the new state
    if k > 0
      nreject = nreject + 1;
    end
    x = xold;	% Reset position
    if (opt_display > 0)
      fprintf(1, '  Sample rejected %4d.  Threshold: %g\n', k, a);
    end
  end
  if k > 0
    samples(k,:) = x;			% Store sample.
    if en_save
      energies(k) = Eold;		% Store energy.
    end
  end
  k = k + 1;
end

if (opt_display > 0)
  fprintf(1, '\nFraction of samples rejected:  %g\n', ...
          nreject/(opt_nsamples));
end

if diagnostics
  diagn.pos = diagn_pos;
  diagn.acc = diagn_acc;
end

% Return complete state of the sampler.
function state = get_state(f)

state.randstate = rand('state');
state.randnstate = randn('state');
return

% Set state of sampler, either from full state, or with an integer
function set_state(f, x)

if isnumeric(x)
  rand('state', x);
  randn('state', x);
else
  if ~isstruct(x)
    error('Second argument to metrop must be number or state structure');
  end
  if (~isfield(x, 'randstate') | ~isfield(x, 'randnstate'))
    error('Second argument to metrop must contain correct fields')
  end
  rand('state', x.randstate);
  randn('state', x.randnstate);
end
return


function [ CatchAline ] = RunElastic2D_SteelBlock(  )
% SoundSim - 2D Elastic Simulation - For Academic Use Only
% -------------------------------------------------------------------------------------
% Limited Feature Beta Release of a SoundSim (www.SoundSim.com)
% Please Send all Questions, Suggestions, and Comments to kevinrudd@SoundSim.com
% No Error Checking - May become unstable if incorrect parameters are used
% -------------------------------------------------------------------------------------
% Steel Block Example -
% A 0.8 MHz Impulse on a top of a 7cm by 7cm steel block.
% The longitudinal, sheer, and Raleigh waves are visible
% The mode conversions are also visible as these waves
% reflect from the edges of the block

%                                                                                Aprox. Material Values From
%                                                                               Stress Waves in Solids (H. Kolsky) Dover 1963
% Material Properties                                                                   Aluminum  Steel  Copper  Glass
p.cl = 5940;                         % Longitudinal Wave Speed (m/s)                     6320      5940   4560    5800
p.cs = 3220;                         % Sheer Wave Speed (m/s)                            3100      3220   2250    3350
p.den = 7800;                        % Material density (kg/m^3)                         2700      7800   8900    2500
p.platethickness = 0.07;             % block thickness (meters)  (in x2-direction)
p.platelength    = 0.07;             % block length (meters)     (in x1-direction)

% Driving Transducer Properties
% -- These parameters adust the transducer position, size, frequency, and drive function length
% -- of the driving transducer.  The transducer is on the top of the plate.
p.tpos = 0.025;                      % transducer position  (meters) - from left side of the block
p.tthickness = 0.001;                % transducer diameter  (meters) - this is 2D
p.tfreq = 800000;                    % frequency of transducer (Hertz)
p.tpulselength = (.5)*(1/(p.tfreq)); % transducer pulse length (seconds)

% Catch Transducer Properties
% -- These parameters adjust the catch transducer location and size.  Again, this transducer
% -- can only be placed on the top of the block
p.catchtpos = 0.05;                  % catch transducer position  (meters) - from left side of the plate
p.catchtthickness = 0.001;           % catch transducer diameter  (meters) - this is 2D

% Other Params
p.abc = 0;                           % thickness of absorbing boundary layer (in simulation units ds) Only on left and right
p.plotevery = 3;                     % update the plot every <plotevery> timesteps
p.SimulationTime =.000013;           % siulation runtime (seconds)

p.plotmode = 'abs_velocity';         % plot mode ('abs_velocity' = absolute velocity);
                                     %           ('x1_velocity'  = velocity in the x1 position (left and right))
                                     %           ('x2_velocity'  = velocity in the x2 position (up and down))

CatchAline=SoundSim_ElasticEngine2D( p );  % Run the Simulation


function [estimateclasstotal,model,D]=adaboost(mode,X,labels,model,itt,weak_type)
% This function AdaBoost, consist of two parts a simpel weak classifier and
% a boosting part:
% The weak classifier tries to find the best treshold in one of the data
% dimensions to sepparate the data into two classes -1 and 1
% The boosting part calls the clasifier iteratively, after every classification
% step it changes the weights of miss-classified examples. This creates a
% cascade of "weak classifiers" which behaves like a "strong classifier"
%
%
%   input -----------------------------------------------------------------
%
%       o mode   : string,  'train' or 'apply'.
%
%       o X      : (N x D), dataset of N samples of dimension D.
%
%       o labels : (N x 1), class labels {-1,+1}
%
%       o model  : struct of model learned, set ot [] when training.
%
%       o itt    :  number of iterations to perform which is equivalent to
%                   the number of resulting weak classifiers.
%
%       o weak_type : string, weak classifier type:
%
%               - decision_stump : splits space along the dimension
%
%   output ----------------------------------------------------------------
%
%  inputs/outputs:
%    datafeatures : An Array with size number_samples x number_features
%    dataclass : An array with the class off all examples, the class
%                 can be -1 or 1
%    itt : The number of training itterations
%    model : A struct with the cascade of weak-classifiers
%    estimateclass : The by the adaboost model classified data
%

if ~exist('weak_type','var'), weak_type = 'decision_stump'; end

switch(mode)
    case 'train'
        % Train the adaboost model

        % Set the data class labels
        labels    = labels(:);
        model     = struct;

        % Weight of training samples, first every sample is even important
        % (same weight)
        D=ones(length(labels),1)/length(labels);

        % This variable will contain the results of the single weak
        % classifiers weight by their alpha
        predict_label   =   zeros(size(labels));

        % Calculate max min of the data
        boundary=[min(X,[],1) max(X,[],1)];
        % Do all model training itterations
        for t=1:itt
            % Find the best treshold to separate the data in two classes

            if strcmpi(weak_type,'decision_stump')

                [estimateclass,err,h] = WeightedThresholdClassifier(X,labels,D);

            elseif strcmpi(weak_type,'linear')

                [B_hat,mapping] = train_linear_classifier(X,labels,D);
                [estimateclass] = predictor_linear_classifier(X,B_hat,mapping);
                err             = sum(D(:) .* (estimateclass ~= labels));
            end

            % Weak classifier influence on total result is based on the current
            % classification error
            alpha=1/2 * log((1-err)/max(err,eps));

            % Store the model parameters
            model(t).alpha      = alpha;

            if strcmpi(weak_type,'decision_stump')
                model(t).dimension  = h.dimension;
                model(t).threshold  = h.threshold;
                model(t).direction  = h.direction;
                model(t).boundary   = boundary;
            elseif strcmpi(weak_type,'linear')
                model(t).B_hat   = B_hat;
                model(t).mapping = mapping;
            end

            % We update D so that wrongly classified samples will have more weight
            D = D.* exp(-model(t).alpha.*labels.*estimateclass);
            D = D./sum(D);

            % Calculate the current error of the cascade of weak
            % classifiers
            predict_label       =   predict_label + estimateclass * model(t).alpha;
            estimateclasstotal  =   sign(predict_label);

            model(t).error      =   sum(estimateclasstotal~=labels)/length(labels);

            if(model(t).error==0), break; end

        end

    case 'apply'
        % Apply Model on the test data

        if strcmpi(weak_type,'decision_stump')

            % Limit datafeatures to orgininal boundaries
            if(length(model)>1);
                minb=model(1).boundary(1:end/2);
                maxb=model(1).boundary(end/2+1:end);
                X=bsxfun(@min,X,maxb);
                X=bsxfun(@max,X,minb);
            end

            % Add all results of the single weak classifiers weighted by their alpha
            predict_label=zeros(size(X,1),1);
            for t=1:length(model);
                predict_label=predict_label+model(t).alpha*ApplyClassTreshold(model(t), X);
            end
            % If the total sum of all weak classifiers
            % is less than zero it is probablly class -1 otherwise class 1;
        elseif strcmpi(weak_type,'linear')

            predict_label = zeros(size(X,1),1);

            for t=1:length(model);
                estimateclass = predictor_linear_classifier(X,model(t).B_hat,model(t).mapping);
                predict_label = predict_label + model(t).alpha * estimateclass;
            end

        end

        estimateclasstotal = sign(predict_label);

    otherwise
        error('adaboost:inputs','unknown mode');
end


function J = GenToneMap(im)
% ==============================================
%   Compute the tone map 'T'
%
%   Paras:
%   @im        : input image ranging value from 0 to 1.
%

    %% Parameters
    Ub = 225;
    Ua = 105;

    Mud = 90;

    DeltaB = 9;
    DeltaD = 11;

    % groups from dark to light
    % 1st group
    Omega1 = 42;
    Omega2 = 29;
    Omega3 = 29;
    % 2nd group
    Omega1 = 52;
    Omega2 = 37;
    Omega3 = 11;
    % 3rd group
    Omega1 = 76;
    Omega2 = 22;
    Omega3 = 2;

    %% Compute the target histgram
    histgramTarget = zeros(256, 1);
    total = 0;
    for ii = 0 : 255
        if ii < Ua || ii > Ub
            p = 0;
        else
            p = 1 / (Ub - Ua);
        end

        histgramTarget(ii+1, 1) = (...
            Omega1 * 1/DeltaB * exp(-(255-ii)/DeltaB) + ...
            Omega2 * p + ...
            Omega3 * 1/sqrt(2 * pi * DeltaD) * exp(-(ii-Mud)^2/(2*DeltaD^2))) * 0.01;

        total = total + histgramTarget(ii+1, 1);
    end
    histgramTarget(:, 1) = histgramTarget(:, 1)/total;

%     %% Smoothing
%     im = medfilt2(im, [5 5]);

    %% Histgram matching
    J = histeq(im, histgramTarget);

    %% Smoothing
    G = fspecial('average', 10);
    J = imfilter(J, G,'same');
end


function [angleRate] = PTrc2deg(X,rcRate,rcExpo,superrate, C)
% raw RCcommand data to RCrate in deg/s, i.e. "set point"
%  with help from: https://github.com/betaflight/betaflight-configurator/blob/master/src/js/RateCurve.js
%   X is a vector containing a single axis of RCcommand data scaled from -500 to 500,
%   rcRate(0-255),rcExpo(0-100),superrate(0-100)

    expoPower=3;
    rcRateConstant=C;
    angleRate=[];

    rcRate=rcRate/100;
    rcExpo=(rcExpo/100);
    superrate=superrate/100;

    maxRC=500;
    rcCommandf = X / maxRC;% scales from -1 to 1 for the math
    rcCommandfAbs = abs(rcCommandf) / 1;%max(abs(rcCommandf));

    if (rcRate > 2)
        rcRate = rcRate + (rcRate - 2) * 14.54;
    end
    if (rcExpo > 0)
       %disp('rcExpo > 0')
        rcCommandf =  rcCommandf .* power(rcCommandfAbs, expoPower) * rcExpo + rcCommandf * (1-rcExpo);
    end
    if (superrate > 0)
        %disp('superrate > 0')
        rcFactor = 1 ./ (1 - rcCommandfAbs * superrate); % this creates the super expo curve needed to convert RCcommand to RCrate
        angleRate = (rcRateConstant * rcRate * rcCommandf);
        angleRate = angleRate .* rcFactor;
       % disp(['angleRate:' num2str(angleRate) ' rcFactor:' num2str(rcFactor)])
    else
        angleRate = (rcRateConstant * rcRate * rcCommandf);
    end

end


clearvars;
close all;
clc;
rng default;
e1 = [1 0 0]';
e2 = [0 1 0]';
e3 = [0 0 1]';

basis1 = [e1 e2];
basis2 = [e2 e3];

bases = {basis1, basis2};
cluster_sizes = [5 5];
points = spx.data.synthetic.subspaces.uniform_points_on_subspaces(bases, cluster_sizes);
X = points.X

S = size(X, 2);

C = zeros(S, S);
start_time = tic;
fprintf('Processing %d signals\n', S);
for s=1:S
    fprintf('.');
    if (mod(s, 50) == 0)
        fprintf('\n');
    end
    x = X(:, s);
    cvx_begin
    % storage for  l1 solver
    variable z(S, 1);
    minimize norm(z, 1)
    subject to
    x == X*z;
    z(s) == 0;
    cvx_end
    C(:, s)  = z;
end
elapsed_time  = toc(start_time);
fprintf('\n');
spr_stats = spx.cluster.subspace.subspace_preservation_stats(C, cluster_sizes);

labels = spx.cluster.labels_from_cluster_sizes(cluster_sizes)

spr_flags = zeros(1, S);
spr_errors = zeros(1, S);

C = abs(C);

c1 = C(:, 1);

k = labels(1);

non_zero_indices = (c1 >= 1e-3);

non_zero_labels = labels(non_zero_indices);

spr_flags(1) = all(non_zero_labels == k)

w = labels == k;

c1k = c1(w);

spr_errors(1) = 1 - sum(c1k) / sum (c1);


function [pb,theta] = pb2MM2(im,sigma)
% function [pb,theta] = pb2MM2(im,sigma)
%
% Compute probability of boundary using the spatially averaged
% second moment matrix at multiple scales.
%
% See also det2MM.
%
% David R. Martin <dmartin@eecs.berkeley.edu>
% March 2003

if nargin<2, sigma=1; end

switch sigma, % from logistic fits (train2MM2.m)
 case 1, beta = [ -3.2369080e+00 ...
                  -7.9668057e+00 -7.1098318e-01 ...
                  -4.6178498e-01  3.1575066e+01 ];
 case 2, beta = [ -3.5512404e+00 ...
                  -8.0101784e+00  1.3527093e+01 ...
                   1.0531737e+01  1.9292447e+01 ];
 otherwise,
  error(sprintf('no parameters for sigma=%g\n',sigma));
end

h = size(im,1);
w = size(im,2);
[a1,b1,t1] = det2MM(im,sigma);
[a2,b2,t2] = det2MM(im,sigma*2);
a1 = sqrt(a1(:));
b1 = sqrt(b1(:));
a2 = sqrt(a2(:));
b2 = sqrt(b2(:));
x = [ ones(size(a1)) a1 b1 a2 b2 ];
pb = 1 ./ (1 + exp(-x*beta'));
pb = reshape(pb,[h w]);

% average orientations for nonmax suppression
dt = mod(t2-t1,2*pi); % [0,2pi)
dt = (dt<pi).*dt + (dt>=pi).*(dt-2*pi); % [-pi,pi)
pb = nonmax(pb,t1+dt/2);


function heapsort_complexity ( )

%*****************************************************************************80
%
%% HEAPSORT_COMPLEXITY examines the time required to perform a heap sort.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 December 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HEAPSORT_COMPLEXITY\n' );
  fprintf ( 1, '  How does the time for heapsort increase with vector length N?\n' );
%
%  Get some data for N = 1 : 200.
%  Do the loop twice to avoid startup anomalies.
%
  for i = 1 : 2

    data_size = zeros ( 200, 1 );
    data_time = zeros ( 200, 1 );

    for n = 1 : 200
      a = rand ( n, 1 );
      tic;
      a_heap = r8vec_sort_heap_a ( n, a );
      data_size(n) = n;
      data_time(n) = toc;
    end

  end

  figure ( 1 )
  plot ( data_size, data_time, 'r-' )
  grid on
  xlabel ( 'Vector length N' );
  ylabel ( 'Elapsed time T' );
  title ( 'T(N), time to heap sort a vector of length N' );

  filename = 'heapsort_1to200.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot saved as "%s".\n', filename );
%
%  Get some data for N = 1, 2, 4, ..., 2^12.
%  Do the loop twice to avoid startup anomalies.
%
  for i = 1 : 2

    data_size = zeros ( 13, 1 );
    data_time = zeros ( 13, 1 );

    n = 1;

    for nlog = 0 : 12
      a = rand ( n, 1 );
      tic;
      a_heap = r8vec_sort_heap_a ( n, a );
      data_size(nlog+1) = n;
      data_time(nlog+1) = toc;
      n = n * 2;
    end

  end

  figure ( 2 )
  loglog ( data_size, data_time, 'r-o' )
  grid on
  xlabel ( 'Vector length N' );
  ylabel ( 'Elapsed time T' );
  title ( 'T(N), time to heap sort a vector of length N' );

  filename = 'heapsort_powersoftwo.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot saved as "%s".\n', filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HEAPSORT_COMPLEXITY:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end
function a_out = r8vec_heap_d ( n, a )

%*****************************************************************************80
%
%% R8VEC_HEAP_D reorders an R8VEC into an descending heap.
%
%  Discussion:
%
%    A descending heap is an array A with the property that, for every index J,
%    A(J) >= A(2*J) and A(J) >= A(2*J+1), (as long as the indices
%    2*J and 2*J+1 are legal).
%
%  Diagram:
%
%                  A(1)
%                /      \
%            A(2)         A(3)
%          /     \        /  \
%      A(4)       A(5)  A(6) A(7)
%      /  \       /   \
%    A(8) A(9) A(10) A(11)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    A Nijenhuis and H Wilf,
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, the size of the input array.
%
%    Input, real A(N), an unsorted array.
%
%    Output, real A_OUT(N), the array has been reordered into a heap.
%
  a_out(1:n) = a(1:n);
%
%  Only nodes N/2 down to 1 can be "parent" nodes.
%
  for i = floor ( n/2 ) : -1 : 1
%
%  Copy the value out of the parent node.
%  Position IFREE is now "open".
%
    key = a_out(i);
    ifree = i;

    while ( 1 )
%
%  Positions 2*IFREE and 2*IFREE + 1 are the descendants of position
%  IFREE.  (One or both may not exist because they exceed N.)
%
      m = 2 * ifree;
%
%  Does the first position exist?
%
      if ( n < m )
        break;
      end
%
%  Does the second position exist?
%
      if ( m + 1 <= n )
%
%  If both positions exist, take the larger of the two values,
%  and update M if necessary.
%
        if ( a_out(m) < a_out(m+1) )
          m = m + 1;
        end

      end
%
%  If the large descendant is larger than KEY, move it up,
%  and update IFREE, the location of the free position, and
%  consider the descendants of THIS position.
%
      if ( a_out(m) <= key )
        break;
      end

      a_out(ifree) = a_out(m);
      ifree = m;

    end
%
%  Once there is no more shifting to do, KEY moves into the free spot IFREE.
%
    a_out(ifree) = key;

  end

  return
end
function a_sorted = r8vec_sort_heap_a ( n, a )

%*****************************************************************************80
%
%% R8VEC_SORT_HEAP_A ascending sorts an R8VEC using heap sort.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 April 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    A Nijenhuis and H Wilf,
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real A(N), the array to be sorted;
%
%    Output, real A_SORTED(N), the sorted array.
%
  if ( n < 1 )
    a_sorted = [];
    return
  end

  if ( n == 1 )
    a_sorted(1) = a(1);
    return
  end
%
%  1: Put A into descending heap form.
%
  a_sorted = r8vec_heap_d ( n, a );
%
%  2: Sort A.
%
%  The largest object in the heap is in A(1).
%  Move it to position A(N).
%
  temp = a_sorted(1);
  a_sorted(1) = a_sorted(n);
  a_sorted(n) = temp;
%
%  Consider the diminished heap of size N1.
%
  for n1 = n-1 : -1 : 2
%
%  Restore the heap structure of A(1) through A(N1).
%
    a_sorted(1:n1) = r8vec_heap_d ( n1, a_sorted );
%
%  Take the largest object from A(1) and move it to A(N1).
%
    temp = a_sorted(1);
    a_sorted(1) = a_sorted(n1);
    a_sorted(n1) = temp;

  end

  return
end