function figh = nav_trajset(cfg)
% Displays a trajectory set for the navigation problem
%   FIGH = NAV_TRAJSET(CFG)
% (ad-hoc function)
% Params:
%   CFG         - configuration, see commented defaults
% Returns:
%   FIGH        - a handle to the created trajectories plot

% data file, set of initial points
CFG.datafile = 'navdata_2h_eps1e2_serial';
CFG.p0 = [];
% save configuration
CFG.plottarget = 'screen';     % 'latex', 'beamer', 'screen', ''
% Save implemented for: policy
% CFG.savedir = 'D:\Work\tex\presentations\2007-03-14_benelux-07\img\';
CFG.savedir = 'D:\Work\tex\papers\alamas07\img\';
                        % path for saving figure
CFG.savefig = 'trajset';    % filename for saving figure
CFG.comparetoct = 0;        % compare 2h policy with constant policy
CFG.showreturn = 1;

% process config
if ischar(cfg), cfg = str2cfg(cfg, fieldnames(CFG)); end;
cfg = checkparams(cfg, CFG);
cfg.grayscale = grayscalefromconfig(cfg);
% custom default set of points 
% -- differing for when there is varying friction, and when not
if isempty(cfg.p0),
    if strfind(cfg.datafile, '2h'),
        p0 = [-5 -5; 5 -5; -5 5; 5 5; 5 -3]';
    else
        p0 = [-5 -5; -4 5; 1 -2; 5 0]';
    end;
end;
% cfg           % feedback on config
    

% Plot a set of trajectories starting from representative positions

% replay config
rcfg.replay = 1;
rcfg.datafile = cfg.datafile;
rcfg.x0 = [NaN; NaN; 0; 0];
rcfg.tend = 25;
rcfg.silent = 1;

% plot config
pcfg.trajectory = 1;
pcfg.addtraj = 0;
pcfg.datasource = rcfg.datafile;
pcfg.plottarget = cfg.plottarget;
pcfg.showreturn = cfg.showreturn;
pcfg.savefig = cfg.savefig;
pcfg.savedir = cfg.savedir;
pcfg.centers = 0;
if cfg.grayscale,
    trajcolors = {0 * [1 1 1], .3 * [1 1 1], .5 * [1 1 1], .7 * [1 1 1]};
    trajstyles = {'-', '-', '-', '-'};
else
    trajcolors = {'r', 'g', 'b', 'k', 'm'};
    trajstyles = {'-', '-', '-', '-'};
end;


for i = 1:size(p0,2),
    rcfg.x0(1:2) = p0(:, i);
    pcfg.hist = fuzzyqi(rcfg);
    pcfg.trajcolor = trajcolors{mod(i-1, length(trajcolors)) + 1};
    pcfg.trajstyle = trajstyles{mod(i-1, length(trajstyles)) + 1};
    % if first create figure, otherwise add    
	pcfg.addtraj = nav_plot(pcfg);
end;

% code to compare the first 2h trajectory with the a trajectory generated by
% the constant-damping policy on the varying-damping landscape
if cfg.comparetoct,
    rcfg.x0(1:2) = p0(:, 1);
    rcfg.datafile = 'ctpolicy_2hmodel';
    pcfg.hist = fuzzyqi(rcfg);
    pcfg.datasource = 'ctpolicy_2hmodel';
    pcfg.trajcolor = [.5 .5 .5];
    pcfg.trajstyle = ':';
    % if first create figure, otherwise add    
	pcfg.addtraj = nav_plot(pcfg);    
end;

% retain figure handle
figh = pcfg.addtraj;