%% Data generation
A = randn(m,n);
[U,D,V] = svd(A,'econ');
D = diag(1./(1:size(D,1)));
A = U*D*V';

x_true = sort(randn(n,1));

noise = 0.1*randn(m,1);
b = A*x_true + noise;

%% (sub)gradient oracles

ppnorm = @(x) (1/p)*sum(abs(x).^p); % ppnorm is (1/p)*norm(x,p)^p
subgradppnorm = @(x,p) evalsubgradppnorm(x,p);

obj = @(x) ppnorm(A*x - b);
subgrad = @(x) A'*subgradppnorm(A*x - b,p);
stochsubgrad = @(x) evalstochsubgrad(x,A,b,p);

%% CVX to find the ground truth

objcvx = @(x) (1/p)*sum(pow_abs(A*x - b,p));
cvx_begin
cvx_precision best
variable x_cvx(n,1)
minimize objcvx(x_cvx)
subject to;
x_cvx(1:end-1) <= x_cvx(2:end);
cvx_end

%% Projection operators for isotonic regression

proj1 = @(x,gamma) evalIsoProj1(x);
proj2 = @(x,gamma) evalIsoProj2(x);

x = zeros(size(x_true));
maxit = 1e5;
gamma0 = 1;

[xAdapTOS,outAdapTOS] = TOS(proj1,proj2,subgrad,obj,x,maxit,gamma0,'adaptive');
[xNTOS,outNTOS] = TOS(proj1,proj2,subgrad,obj,x,maxit,gamma0,'nonsmooth');
if p == 2, [xTOS,outTOS] = TOS(proj1,proj2,subgrad,obj,x,maxit,gamma0,'smooth'); end

%% Save results

data.x_cvx = x_cvx;
data.cvx_optval = cvx_optval;
data.xAdapTOS = xAdapTOS;
data.outAdapTOS = outAdapTOS;
data.xNTOS = xNTOS;
data.outNTOS = outNTOS;
if p == 2
    data.xTOS = xTOS;
    data.outTOS = outTOS;
end

dirName = ['./runs/isotonic_regression/p=',num2str(p),'/'];
if ~exist(dirName,'dir'), mkdir(dirName); end
saveName = [dirName,datestr(now,30),'_',num2str(randi(1e5)),'.mat'];
save(saveName,'data');

%% Functions
function u = evalstochsubgrad(x,A,b,p)

[m,n] = size(A);
ind = randi(m);
ai = A(ind,:);
bi = b(ind);
u = m*ai'*evalsubgradppnorm(ai*x - bi,p);

end

function u = evalsubgradppnorm(x,p)

y = x;
y(y==0) = 1;    % this is to prevent 1/0 in the below line
% if x_j = 0, then u_j = 0 is a subgrad
u = x.*abs(y).^(p-2);

end

function y = evalIsoProj1(x)

y = reshape(x,2,[]);
[~, ind] = find(y(1,:) > y(2,:));
newvals = 0.5*sum(y(:,ind));
y(:,ind) = [newvals;newvals];
y = reshape(y,[],1);

end

function y = evalIsoProj2(x)

y = x(2:end-1);
y = reshape(y,2,[]);
[~, ind] = find(y(1,:) > y(2,:));
newvals = 0.5*sum(y(:,ind));
y(:,ind) = [newvals;newvals];
y = [x(1);reshape(y,[],1);x(end)];

end