
% Example taken from 
% L. Bako, "Identification of switched linear systems via sparse optimization", 
%  Automatica 47(4):668-677, 2011.
%
%
% You must have klinreg_matlab installed and in the PATH
% to run this test.
%
clear all;
close all;

format compact;

sigmab = 0.08;	% noise standard deviation

n = 3;	% number of modes
p = 4;	% dimension of the data

N=300;	% number of points

% True parameters to be estimated
wtrue = [-0.4, 1.55, 1; 0.25, -0.58, -0.24; -0.15, -2.1, -0.65; 0.08, 0.96, 0.30];

Nexpe = 100;  % number of trials
Wexpe= zeros(Nexpe,n*p);
Wrefexp = zeros(Nexpe,n*p);

NMSEref = zeros(Nexpe,1);
NMSE = zeros(Nexpe,1);

restarts = rstar(0.001,N,n,p,1)
	
for expe=1:Nexpe

	U = randn(N+2,1);
	Y(1:2) = [0;0];
	X = zeros(N+2,p);
	lambdatrue = zeros(N+2,1);
	for i=3:N+2
		X(i,:) = [Y(i-1), Y(i-2), U(i-1), U(i-2)];
		lambdatrue(i) = ceil(100*rand(1,1));
		if lambdatrue(i) > 60
			lambdatrue(i) = 3;
		else 
			if lambdatrue(i) > 30 
				lambdatrue(i) = 2;
			else
				lambdatrue(i) = 1;
			end
		end
		Y(i) = X(i,:) * wtrue(:,lambdatrue(i)) + sigmab*randn(1,1);
	end
	XX = X(3:N+2,:);
	YY  = Y(3:N+2);
	if size(YY,2) > size(YY,1) 
		YY = YY';
	end
	LL = lambdatrue(3:N+2);

	% klinreg
	tic;
	wmax = 100;
	[w,lbls] = klinreg_simple(XX,YY,n,restarts,wmax);
	
	comp_time(expe) = toc;
	
	for j=1:n
		ew = wtrue(:,j)*ones(1,n) - w;
		[ewm wj] = min(sum(ew.*ew));
		Wexpe(expe,(j-1)*p+1:j*p) = w(:,wj)';
	end
	NMSE(expe) = sum((Wexpe(expe,1:4)-wtrue(:,1)').^2)./sum(wtrue(:,1).^2) + sum((Wexpe(expe,5:8)-wtrue(:,2)').^2)./sum(wtrue(:,2).^2) + sum((Wexpe(expe,9:12)-wtrue(:,3)').^2)./sum(wtrue(:,3).^2) ;
	
	% Reference model
	wref = zeros(p,n);
	for j=1:n
		wref(:,j) = XX(LL==j,:) \ YY(LL == j);
	end

	Wrefexp(expe,:) = reshape(wref,1,12);

	NMSEref(expe) = sum((Wrefexp(expe,1:4)-wtrue(:,1)').^2)./sum(wtrue(:,1).^2) + sum((Wrefexp(expe,5:8)-wtrue(:,2)').^2)./sum(wtrue(:,2).^2) + sum((Wrefexp(expe,9:12)-wtrue(:,3)').^2)./sum(wtrue(:,3).^2) ;

	ej = YY*ones(1,n) - XX*wref(:,1:n);
	error_ref(expe) = sum(min(ej.*ej,[],2));


	SNR(expe) = 20*log10(std(Y)/sigmab);
end

meanParams = reshape(mean(Wexpe),4,3)
stdParams = reshape(std(Wexpe),4,3)

meanParamsref = reshape(mean(Wrefexp),4,3)
stdParamsref =  reshape(std(Wrefexp),4,3)

meanTime = mean(comp_time)
stdTime = std(comp_time)

SNRmean = mean(SNR)
