function result  = plotResultSGMP(rx, result, filename);
%---------------------------------------------------------
% Plot posEst in ENU and calculate estimation statistics
% Ju-Yong Do,  Feb 3, 2009
%---------------------------------------------------------
% input  : rx, result
% output : result
%=========================================================
%=========================================================
global opt const

%find valid results
if ~isfield(result, 'est'), return; end;
for i=1:min([result.numepoch 5]), result.est(i).valid = 0; end; %ignore first five fixes for caution
igood               = find([result.est.valid]);     %index to valid epoch
numvalid            = length(igood);                %number of successes
if numvalid==0,                                     %no tangible results. 
    result.stat.success = 0;                        %zero success
    return; 
end;                        

%position and clk bias estimates
pos                 = reshape([result.est(igood).pos],3,[])'; %position estimate 
clkbias             = [result.est(igood).clkbias];  %clk bias estimate
tow                 = [result.est(igood).tow];      %corresponding tow
result.stat.avg.xyz = mean(pos,1);                  %average xyz postion
result.stat.avg.lla = wgsxyz2lla(result.stat.avg.xyz);%average lla postion

%convert xyz to ENU (use true position if available)
centerpos           = result.stat.avg.xyz;          %center point at pos plot
if isfield('rx.info','initpos')                     %use given initial pos if available         
    if ~isempty(rx.info.initpos), centerpos = rx.initpos; end;
end
enu                 = (calRotMat(centerpos)*(pos'-centerpos'*ones(1,numvalid)))'; %enu position est
result.stat.avg.enu = mean(enu,1);                  %average enu postion

%=========================================================
%Error statistics
result.stat.success = numvalid/(result.numepoch-5); %ratio of success in posfixes

%standard deviation
stdenu              = sqrt(mean((enu.^2),1));       %std from the initial position
result.stat.std2d   = norm(stdenu(1:2));            %2 dimensional standard deviation
result.stat.std3d   = norm(stdenu);                 %3 dimensional standard deviation

%Circular Error Probable (CEP) for 50%, 67%, 95% (2D)
posErr2D            = sort(sqrt(sum(enu(:,1:2).^2,2)));%2D error from center point
result.stat.cep50   = posErr2D(ceil(numvalid*.50)); %median horizontal error
result.stat.cep67   = posErr2D(ceil(numvalid*.67)); %CEP 67%
result.stat.cep95   = posErr2D(ceil(numvalid*.95)); %CEP 95%
result.stat.max2d   = max(posErr2D);                %worst horizontal error

%Spherical Error Probable for 50%, 67%, 95% (3D)
posErr3D            = sort(sqrt(sum(enu.^2,2)));    %3D error from center point
result.stat.sep50   = posErr3D(ceil(numvalid*.50)); %median horizontal error
result.stat.sep67   = posErr3D(ceil(numvalid*.67)); %CEP 67%
result.stat.sep95   = posErr3D(ceil(numvalid*.95)); %CEP 95%
result.stat.max3d   = max(posErr3D);                %worst position error

if opt.plot.print,
    disp(['[lat long alt] = ', num2str(result.stat.avg.lla,'%-11.5f %-11.5f %-11.1f')]);
    disp(['[success(%) 2Drms cep50 cep67 cep95 max2d] = ',...
        num2str([result.stat.success*100 result.stat.std2d result.stat.cep50 result.stat.cep67 result.stat.cep95 result.stat.max2d],'%-7.1f')]);
    disp(['[success(%) 3Drms sep50 sep67 sep95 max3d] = ',...
        num2str([result.stat.success*100 result.stat.std3d result.stat.sep50 result.stat.sep67 result.stat.sep95 result.stat.max3d],'%-7.1f')]);
end

%save result statistics to a file
fName4save = [opt.outputdir,filename(1:end-4),'_'];
if opt.save.result, save([fName4save,'result'], 'result'); end; 

%=========================================================
%Plot position estimation results
fName4plot = ['"',filename, '" at (', num2str(result.stat.avg.lla(1:2), '%7.1f,'),' ',num2str(result.stat.avg.lla(3), '%6.1f'),'m)']; fName4plot(find(fName4plot=='_'))=' ';
if opt.plot.pos,    
    %1 dimensional plot
    if opt.plot.dim==1 || opt.plot.dim==4 
        gcf=figure(1);
        subplot(411); plot(tow/3600,enu(:,1),'.-');grid on; ylabel('East (m)');title(fName4plot);
        subplot(412); plot(tow/3600,enu(:,2),'.-');grid on; ylabel('North (m)');
        subplot(413); plot(tow/3600,enu(:,3),'.-');grid on; ylabel('Up (m)');
        subplot(414); plot(tow/3600,(clkbias-min(clkbias))/const.speedOfLight*1e6,'.-');grid on; ylabel('Clock (us)');
        xlabel('Time (hour)');         
        if opt.save.plot, saveas(gcf, [fName4save,'pos1D']); saveas(gcf, [fName4save,'pos1D'], 'eps'); end;
    end
    %2 dimensional plot
    if opt.plot.dim==2 || opt.plot.dim==4 
        gcf=figure(2);
        plot(enu(:,1),enu(:,2),'.-'); grid on; xlabel('East (m)'); ylabel('North (m)');title(fName4plot);
        legend([num2str(result.stat.success*100, '%4.1f'),'% success with ',num2str(result.stat.std2d, '%5.1f'), 'm (2drms)']);        
        incirr = 5; outcirr = 15;  %reference circle radius
        rectangle('Position',incirr*[-1 -1 2 2],'Curvature',[1,1],'LineWidth',2,'EdgeColor','g');
        rectangle('Position',outcirr*[-1 -1 2 2],'Curvature',[1,1],'LineWidth',2,'EdgeColor','r');
        if opt.save.plot, saveas(gcf, [fName4save,'pos2D']); saveas(gcf, [fName4save,'pos2D'], 'eps'); end;
    end
    %3 dimensional plot
    if opt.plot.dim==3 || opt.plot.dim==4 
        gcf=figure(3);
        plot3(enu(:,1),enu(:,2),enu(:,3),'.-'); grid on;
        xlabel('East (m)'); ylabel('North (m)'); zlabel('Up (m)');title(fName4plot);                             
    end
end

if opt.plot.stat,
    %positioning error pdf plots
    gcf=figure(4);
    subplot(311); histfit(enu(:,1),1e2); grid on;  ylabel('East'); title(fName4plot); 
    legend(['(\mu, \sigma) = ',num2str([mean(enu(:,1)) std(enu(:,1))], '%6.1f')]);
    subplot(312); histfit(enu(:,2),1e2); grid on;  ylabel('North');
    legend(['(\mu, \sigma) = ',num2str([mean(enu(:,2)) std(enu(:,2))], '%6.1f')]);
    subplot(313); histfit(enu(:,3),1e2); grid on;  ylabel('Up'); xlabel('Position error (m)');     
    legend(['(\mu, \sigma) = ',num2str([mean(enu(:,3)) std(enu(:,3))], '%6.1f')]);    
    if opt.save.plot, saveas(gcf, [fName4save,'poshist']); saveas(gcf, [fName4save,'poshist'], 'eps'); end;
end

return;
%=========================================================
%=========================================================