close all;
clear;
gamma = 1e-2;
kfold = 5;
outNoise        = 255;        % the magnitude of outlier noise magnitude over inlier noise
inNoise         = 255*0.25;
outRatio_list = 0.1;
random_run = 1;
load MITPIT_light_48.mat;
load MITPIT_48.mat;
AllFacePoses = AllFacePoses_48;
AllFacePoses_light = AllFacePoses_light_48;
frad = 48;
fdim = frad*frad;
dim = fdim;
objNum = size(AllFacePoses,1);
% initializing variables
L = kfold;
label = cell(random_run,L);
F = cell(random_run,L);

lsErrMu = zeros(random_run,L);
lsErrStd = zeros(random_run,L);
lsLabel = cell(random_run,L);
lsTestLabel = cell(random_run,L);
lsF = cell(random_run,L);
lsTime = zeros(random_run,L);


tlsErrMu = zeros(random_run,L);
tlsErrStd = zeros(random_run,L);
tlsLabel = cell(random_run,L);
tlsTestLabel = cell(random_run,L);
tlsF = cell(random_run,L);
tlsTime = zeros(random_run,L);

ransacErrMu = zeros(random_run,L);
ransacErrStd = zeros(random_run,L);
ransacLabel = cell(random_run,L);
ransacTestLabel = cell(random_run,L);
ransacF = cell(random_run,L);
ransacTime = zeros(random_run,L);



rpcaErrMu = zeros(random_run,L);
rpcaErrStd = zeros(random_run,L);
rpcaLabel = cell(random_run,L);
rpcaTestLabel = cell(random_run,L);
rpcaF = cell(random_run,L);
rpcaTime = zeros(random_run,L);



torreErrMu = zeros(random_run,L);
torreErrStd = zeros(random_run,L);
torreLabel = cell(random_run,L);
torreTestLabel = cell(random_run,L);
torreF = cell(random_run,L);
torreTime = zeros(random_run,L);


LrsRRErrMu = zeros(random_run,L);
LrsRRErrStd = zeros(random_run,L);
LrsRRLabel = cell(random_run,L);
LrsRRTestLabel = cell(random_run,L);
LrsRRF = cell(random_run,L);
LrsRRTime = zeros(random_run,L);


for run = 1:random_run
    indices = crossvalind('Kfold', objNum, kfold);
    for k = 1:kfold
        TrainFaceSet = [];
        TrainPoseSet = [];
        TrainCosSinSet = [];
        TestFaceSet = [];
        TestPoseSet = [];
        TestCosSinSet = [];
        % read data
        for posei = 1:objNum
            oneFaceSet = [AllFacePoses_light{posei,1},AllFacePoses{posei,1}];
            onePoseSet = [AllFacePoses_light{posei,2},AllFacePoses{posei,2}];
            oneCosSinSet = [AllFacePoses_light{posei,3},AllFacePoses{posei,3}];
            if indices(posei) ~= k
                TrainFaceSet = [TrainFaceSet,oneFaceSet];
                TrainPoseSet = [TrainPoseSet,onePoseSet];
                TrainCosSinSet = [TrainCosSinSet,oneCosSinSet];
            else
                TestFaceSet = [TestFaceSet,oneFaceSet];
                TestPoseSet = [TestPoseSet,onePoseSet];
                TestCosSinSet = [TestCosSinSet,oneCosSinSet];
            end
        end
        TrainFaceSet = double(TrainFaceSet)/255;
        TestFaceSet = double(TestFaceSet)/255;
        X = TrainFaceSet;
        Y = TrainCosSinSet;
        label{run,k} = TrainPoseSet;
        F{run,k} = TrainCosSinSet;
        
        epsilon = inNoise;
        dist_norm = inNoise;
        fix = dim; const = 1; % denote which element of regressor is 1
        
        %% run OLS
        tic;
        Xhat = [X;ones(1,size(X,2))];
        f_ls = Y * Xhat'*inv(Xhat*Xhat'+ gamma*eye(fdim+1));
        lsTime(run,k) = toc;
        
        % decide label and calculate metric
        lsLabel{run,k} = f_ls*[X;ones(1,size(X,2))];
        lsTestLabel{run,k} = f_ls*[TestFaceSet;ones(1,size(TestFaceSet,2))];
        
        figure(1),subplot(2,1,1),plot(lsLabel{run,k}(2,:),lsLabel{run,k}(1,:),'b.' );
        hold on;
        plot(TrainCosSinSet(2,:),TrainCosSinSet(1,:),'r+' );
        xlim([-1.5 1.5]);
        ylim([-0.5 1.5]);
        xlabel('cos(¦È)');
        ylabel({'Training data','sin(¦È)'});
        
        subplot(2,1,2),plot(lsTestLabel{run,k}(2,:),lsTestLabel{run,k}(1,:),'b.' );
        hold on;
        plot(TestCosSinSet(2,:),TestCosSinSet(1,:),'r+' );
        xlim([-1.5 1.5]);
        ylim([-0.5 1.5]);
        xlabel('cos(¦È)');
        ylabel({'Testing data','sin(¦È)'});
        
        TestAngle = atan2(lsTestLabel{run,k}(2,:),lsTestLabel{run,k}(1,:))*180/pi;
        DiffP = abs(TestPoseSet - TestAngle);
        GoodDP = DiffP(DiffP < 100);
        lsErrMu(run,k) = mean(GoodDP);
        lsErrStd(run,k) = std(GoodDP);
        
        %% run TLS
        tic;
        meanX = mean(X,2);
        meanY = mean(Y,2);
        zeroX = X - repmat(meanX,[1,size(X,2)]);
        zeroY = Y - repmat(meanY,[1,size(Y,2)]);
        f_tls = tls(zeroX,zeroY) ;
        tlsTime(run,k) = toc;
        
        % decide label and calculate metric
        tlsLabel{run,k} = f_tls*zeroX + repmat(meanY,[1,size(Y,2)]);
        tlsTestLabel{run,k} = f_tls*(TestFaceSet-repmat(meanX,[1,size(TestFaceSet,2)]))+ repmat(meanY,[1,size(TestFaceSet,2)]);
        
        figure(2),subplot(2,1,1),plot(tlsLabel{run,k}(2,:),tlsLabel{run,k}(1,:),'b.' );
        hold on;
        plot(TrainCosSinSet(2,:),TrainCosSinSet(1,:),'r+' );
        xlim([-1.5 1.5]);
        ylim([-0.5 1.5]);
        xlabel('cos(¦È)');
        ylabel({'Training data','sin(¦È)'});
        
        subplot(2,1,2),plot(tlsTestLabel{run,k}(2,:),tlsTestLabel{run,k}(1,:),'b.' );
        hold on;
        plot(TestCosSinSet(2,:),TestCosSinSet(1,:),'r+' );
        xlim([-1.5 1.5]);
        ylim([-0.5 1.5]);
        xlabel('cos(¦È)');
        ylabel({'Testing data','sin(¦È)'});
        
        TestAngle = atan2(tlsTestLabel{run,k}(2,:),tlsTestLabel{run,k}(1,:))*180/pi;
        DiffP = abs(TestPoseSet - TestAngle);
        tlsErrMu(run,k) = mean(DiffP);
        tlsErrStd(run,k) = std(DiffP);
        
       %% run LRS Robust Regression
        tic;
        [f_LRSRR,D,A_LSrra,E, LrsrrErr] = LRSR_RR_A(X,Y,1e0,1e2,1.1,gamma);
        LrsRRTime(run,k) = toc;
        f_lrsrr = f_LRSRR'*const;
        LrsRRF{run,k} = f_lrsrr;
        
        % decide label and calculate metric
        LrsRRLabel{run,k} = f_LRSRR*[D;ones(1,size(D,2))];
        
        [D_test,Z_test,E_test] = LRSR_D(TestFaceSet,D,1e2,1.01);  
        LrsRRTestLabel{run,k} = f_LRSRR*[D_test;ones(1,size(D_test,2))];
        
        figure(3),subplot(2,1,1),plot(LrsRRLabel{run,k}(2,:),LrsRRLabel{run,k}(1,:),'b.' );
        hold on;
        plot(TrainCosSinSet(2,:),TrainCosSinSet(1,:),'r+' );
        xlim([-1.5 1.5]);
        ylim([-0.5 1.5]);
        xlabel('cos(¦È)');
        ylabel({'Training data','sin(¦È)'});
        
        subplot(2,1,2),plot(LrsRRTestLabel{run,k}(2,:),LrsRRTestLabel{run,k}(1,:),'b.' );
        hold on;
        plot(TestCosSinSet(2,:),TestCosSinSet(1,:),'r+' );
        xlim([-1.5 1.5]);
        ylim([-0.5 1.5]);
        xlabel('cos(¦È)');
        ylabel({'Testing data','sin(¦È)'});
        
        figure(4),subplot(1,3,1),imshow(reshape(TrainFaceSet(:,7),[frad,frad]),[]);
        hold on;
        subplot(1,3,2),imshow(reshape(D(:,7),[frad,frad]),[]);
        subplot(1,3,3),imshow(reshape(abs(E(:,7)),[frad,frad]),[]);
        
        figure(5),subplot(1,3,1),imshow(reshape(TestFaceSet(:,7),[frad,frad]),[]);
        hold on;
        subplot(1,3,2),imshow(reshape(D_test(:,7),[frad,frad]),[]);
        subplot(1,3,3),imshow(reshape(abs(E_test(:,7)),[frad,frad]),[]);
        
        TestAngle = atan2(LrsRRTestLabel{run,k}(2,:),LrsRRTestLabel{run,k}(1,:))*180/pi;
        DiffP = abs(TestPoseSet - TestAngle);
        GoodDP = DiffP(DiffP < 100);
        LrsRRErrMu(run,k) = mean(GoodDP);
        LrsRRErrStd(run,k) = std(GoodDP);
        
       %% run Torre Robust Regression
        tic;          
        [f_torre,D,E] = RR_torre_New(X,Y,1e0,1e2,1.01,gamma);
        torreTime(run,k) = toc;
        torreF{run,k} = f_torre;
        
        % decide label and calculate metric
        torreLabel{run,k} = f_torre*[D;ones(1,size(TrainFaceSet,2))];
        
        [D_test,Z_test,E_test] = LRR_D(TestFaceSet,D,1e2,1.01);
        %[D_test,Z_test,E_test] = ladmp_lrrA(TestFaceSet,D,1e2,1.01)
        torreTestLabel{run,k} = f_torre*[D_test;ones(1,size(D_test,2))];
        
        figure(6),subplot(2,1,1),plot(torreLabel{run,k}(2,:),torreLabel{run,k}(1,:),'b.' );
        hold on;
        plot(TrainCosSinSet(2,:),TrainCosSinSet(1,:),'r+' );
        xlim([-1.5 1.5]);
        ylim([-0.5 1.5]);
        xlabel('cos(¦È)');
        ylabel({'Training data','sin(¦È)'});
        
        subplot(2,1,2),plot(torreTestLabel{run,k}(2,:),torreTestLabel{run,k}(1,:),'b.' );
        hold on;
        plot(TestCosSinSet(2,:),TestCosSinSet(1,:),'r+' );
        xlim([-1.5 1.5]);
        ylim([-0.5 1.5]);
        xlabel('cos(¦È)');
        ylabel({'Testing data','sin(¦È)'});
        
        figure(7),subplot(1,3,1),imshow(reshape(TrainFaceSet(:,7),[frad,frad]),[]);
        hold on;
        subplot(1,3,2),imshow(reshape(D(:,7),[frad,frad]),[]);
        subplot(1,3,3),imshow(abs(reshape(E(:,7),[frad,frad])),[]);
        
        figure(8),subplot(1,3,1),imshow(reshape(TestFaceSet(:,7),[frad,frad]),[]);
        hold on;
        subplot(1,3,2),imshow(reshape(D_test(:,7),[frad,frad]),[]);
        subplot(1,3,3),imshow(abs(reshape(E_test(:,7),[frad,frad])),[]);
        
        TestAngle = atan2(torreTestLabel{run,k}(2,:),torreTestLabel{run,k}(1,:))*180/pi;
        DiffP = abs(TestPoseSet - TestAngle);
        GoodDP = DiffP(DiffP < 100);
        torreErrMu(run,k) = mean(GoodDP);
        torreErrStd(run,k) = std(GoodDP);  
        
        %% run cRPCA+LSR
        numPoints = size(TrainFaceSet, 2);
        tau = sqrt(dist_norm^2*numPoints);
        tic;
        [~,D_rpca,E,~,~] = rpca_admm(TrainFaceSet,(0.1:0.1:11),tau,1.01,0);
        Dhat = [D_rpca;ones(1,size(TrainFaceSet,2))];
        f_rpca = Y * Dhat'*inv(Dhat*Dhat'+ gamma*eye(fdim+1));
        rpcaTime(run,k) = toc;
        rpcaF{run,k} = f_rpca/f_rpca(fix)*const;
        
        % decide label and calculate metric
        rpcaLabel{run,k} = f_rpca*[D_rpca;ones(1,size(TrainFaceSet,2))];
        
        %[D_test,Z_test,E_test] = LRR_D(TestFaceSet,D_rpca,1e2,1.01);
        [~,D_test,E_test,~,~] = rpca_admm(TestFaceSet,(0.1:0.1:11),tau,1.01,0);
        rpcaTestLabel{run,k} = f_rpca*[D_test;ones(1,size(D_test,2))];
        
        figure(9),subplot(2,1,1),plot(rpcaLabel{run,k}(2,:),rpcaLabel{run,k}(1,:),'b.' );
        hold on;
        plot(TrainCosSinSet(2,:),TrainCosSinSet(1,:),'r+' );
        xlim([-1.5 1.5]);
        ylim([-0.5 1.5]);
        xlabel('cos(¦È)');
        ylabel({'Training data','sin(¦È)'});
        
        subplot(2,1,2),plot(rpcaTestLabel{run,k}(2,:),rpcaTestLabel{run,k}(1,:),'b.' );
        hold on;
        plot(TestCosSinSet(2,:),TestCosSinSet(1,:),'r+' );
        xlim([-1.5 1.5]);
        ylim([-0.5 1.5]);
        xlabel('cos(¦È)');
        ylabel({'Testing data','sin(¦È)'});
        
        figure(10),subplot(1,3,1),imshow(reshape(TrainFaceSet(:,7),[frad,frad]),[]);
        hold on;
        subplot(1,3,2),imshow(reshape(D_rpca(:,7),[frad,frad]),[]);
        subplot(1,3,3),imshow(reshape(abs(E(:,7)),[frad,frad]),[]);
        
        figure(11),subplot(1,3,1),imshow(reshape(TestFaceSet(:,7),[frad,frad]),[]);
        hold on;
        subplot(1,3,2),imshow(reshape(D_test(:,7),[frad,frad]),[]);
        subplot(1,3,3),imshow(reshape(abs(E_test(:,7)),[frad,frad]),[]);
        
        TestAngle = atan2(rpcaTestLabel{run,k}(2,:),rpcaTestLabel{run,k}(1,:))*180/pi;
        DiffP = abs(TestPoseSet - TestAngle);
        GoodDP = DiffP(DiffP < 100);
        rpcaErrMu(run,k) = mean(GoodDP);
        rpcaErrStd(run,k) = std(GoodDP);
        
       %% run RANsac
        tic;
        alldata = [X;Y];
        sn = size(alldata,2);
        randn = min(fdim - 1, int32(sn*0.5));
        [m_ransac,f_ransac] = RANSAC(alldata,@LinearXY,randn,@residual_LXY,500,0.01);
        ransacTime(run,k) = toc;
        
        % decide label and calculate metric
        ransacLabel{run,k} = f_ransac*[X;ones(1,size(TrainFaceSet,2))];
        D_ransac = X(:,m_ransac);
        ransacTestLabel{run,k} = f_ransac*[TestFaceSet;ones(1,size(TestFaceSet,2))];
        
        figure(12),subplot(2,1,1),plot(ransacLabel{run,k}(2,:),ransacLabel{run,k}(1,:),'b.' );
        hold on;
        plot(TrainCosSinSet(2,:),TrainCosSinSet(1,:),'r+' );
        xlim([-1.5 1.5]);
        ylim([-0.5 1.5]);
        xlabel('cos(¦È)');
        ylabel({'sin(¦È)','Training data'});
        
        subplot(2,1,2),plot(ransacTestLabel{run,k}(2,:),ransacTestLabel{run,k}(1,:),'b.' );
        hold on;
        plot(TestCosSinSet(2,:),TestCosSinSet(1,:),'r+' );
        xlim([-1.5 1.5]);
        ylim([-0.5 1.5]);
        xlabel('cos(¦È)');
        ylabel({'sin(¦È)','Testing data'});
        
        TestAngle = atan2(ransacTestLabel{run,k}(2,:),ransacTestLabel{run,k}(1,:))*180/pi;
        DiffP = abs(TestPoseSet - TestAngle);
        GoodDP = DiffP(DiffP < 100);
        ransacErrMu(run,k) = mean(GoodDP);
        ransacErrStd(run,k) = std(GoodDP);
       
       
    end
end

%% statistic envaluation
% our algorithm
lsErrMuK = mean(lsErrMu,2);
lsErrMuAll = mean(lsErrMuK);
lsErrStdK = mean(lsErrStd,2);
lsErrStdAll = mean(lsErrStdK);


tlsErrMuK = mean(tlsErrMu,2);
tlsErrMuAll = mean(tlsErrMuK);
tlsErrStdK = mean(tlsErrStd,2);
tlsErrStdAll = mean(tlsErrStdK);

ransacErrMuK = mean(ransacErrMu,2);
ransacErrMuAll = mean(ransacErrMuK);
ransacErrStdK = mean(ransacErrStd,2);
ransacErrStdAll = mean(ransacErrStdK);

rpcaErrMuK = mean(rpcaErrMu,2);
rpcaErrMuAll = mean(rpcaErrMuK);
rpcaErrStdK = mean(rpcaErrStd,2);
rpcaErrStdAll = mean(rpcaErrStdK);



torreErrMuK = mean(torreErrMu,2);
torreErrMuAll = mean(torreErrMuK);
torreErrStdK = mean(torreErrStd,2);
torreErrStdAll = mean(torreErrStdK);


LrsRRErrMuK = mean(LrsRRErrMu,2);
LrsRRErrMuAll = mean(LrsRRErrMuK);
LrsRRErrStdK = mean(LrsRRErrStd,2);
LrsRRErrStdAll = mean(LrsRRErrStdK);

MuRes = [lsErrMu;tlsErrMu;ransacErrMu;rpcaErrMu;torreErrMu;LrsRRErrMu];
StdRes = [lsErrStd;tlsErrStd;ransacErrStd;rpcaErrStd;torreErrStd;LrsRRErrStd];
TimeRes = [lsTime;tlsTime;ransacTime;rpcaTime;torreTime;LrsRRTime];

save FacePoseMuRes.mat  MuRes StdRes TimeRes



%% display statistic result
figure,
errorbar(outRatio_list*100,rrHmeansMean,rrHmeansSem,'r');
hold on;
errorbar(outRatio_list*100,coHmeansMean,coHmeansSem,'r-.');
errorbar(outRatio_list*100,ransacHmeansMean,ransacHmeansSem,'g-.');
errorbar(outRatio_list*100,msacHmeansMean,msacHmeansSem,'k-.');
errorbar(outRatio_list*100,mlesacHmeansMean,mlesacHmeansSem,'b-.');
errorbar(outRatio_list*100,lmedsHmeansMean,lmedsHmeansSem,'m--');
errorbar(outRatio_list*100,mestHmeansMean,mestHmeansSem,'c--');
errorbar(outRatio_list*100,rpcaHmeansMean,rpcaHmeansSem,'b');
errorbar(outRatio_list*100,bprrHmeansMean,bprrHmeansSem,'g');
errorbar(outRatio_list*100,torreHmeansMean,torreHmeansSem,'k');
errorbar(outRatio_list*100,LrsRRHmeansMean,LrsRRHmeansSem,'y');
errorbar(outRatio_list*100,bsrrHmeansMean,bsrrHmeansSem,'m');
legend('proposed','proposed with prior','RANSAC','MSAC','MLESAC','LMeds','M-estimator','cRPCA','BPRR','RR','LRS-RR','BSRR');
title('H-means');
hold off;

%% calculate and plot regressor estimation error
rrdiff = cellfun(@regressorDiff,F,rrF);
ransacdiff = cellfun(@regressorDiff,F,ransacF);
msacdiff = cellfun(@regressorDiff,F,msacF);
mlesacdiff = cellfun(@regressorDiff,F,mlesacF);
lmedsdiff = cellfun(@regressorDiff,F,lmedsF);
mestdiff = cellfun(@regressorDiff,F,mestF);
rpcadiff = cellfun(@regressorDiff,F,rpcaF);
bprrdiff = cellfun(@regressorDiff,F,bprrF);
bsrrdiff = cellfun(@regressorDiff,F,bsrrF);
torrediff = cellfun(@regressorDiff,F,torreF);
LrsRRdiff = cellfun(@regressorDiff,F,LrsRRF);
codiff = cellfun(@regressorDiff,F,coF);

n = sqrt(random_run);
figure,
errorbar(outRatio_list*100,mean(rrdiff,2),std(rrdiff,0,2)/n,'r');
hold on;
errorbar(outRatio_list*100,mean(codiff,2),std(codiff,0,2)/n,'r-.');
errorbar(outRatio_list*100,mean(ransacdiff,2),std(ransacdiff,0,2)/n,'g-.');
errorbar(outRatio_list*100,mean(msacdiff,2),std(msacdiff,0,2)/n,'k-.');
errorbar(outRatio_list*100,mean(mlesacdiff,2),std(mlesacdiff,0,2)/n,'b-.');
errorbar(outRatio_list*100,mean(lmedsdiff,2),std(lmedsdiff,0,2)/n,'m--');
errorbar(outRatio_list*100,mean(mestdiff,2),std(mestdiff,0,2)/n,'c--');
errorbar(outRatio_list*100,mean(rpcadiff,2),std(rpcadiff,0,2)/n,'b');
errorbar(outRatio_list*100,mean(bprrdiff,2),std(bprrdiff,0,2)/n,'g');
errorbar(outRatio_list*100,mean(torrediff,2),std(torrediff,0,2)/n,'k');
errorbar(outRatio_list*100,mean(LrsRRdiff,2),std(LrsRRdiff,0,2)/n,'y');
errorbar(outRatio_list*100,mean(bsrrdiff,2),std(bsrrdiff,0,2)/n,'m');
legend('proposed','proposed with prior','RANSAC','MSAC','MLESAC','LMeds','M-estimator','cRPCA','BPRR','RR','LRS-RR','BSRR');
title('Regressor Error');
hold off;