% Displays AINS navigation solution

function plot_veh_nav_soln(action,path,kernel,start_time);

persistent plot_handle axis_handle current_plot_num num_plots
persistent figure_caption
persistent h_listbox h_fig_listbox
persistent new_plot_num

thismfile=mfilename;

switch action
    
    case 'start'
        
        h_fig_listbox = -1;

        %%%%%%%%%%%%%%%%%%
        % Load data
        
        % Instead of having a vector of structures as in C++
        % the elements of the structure are array of values
        % MATLAB is very clumsy with arrays of structures
        
        prefix_rt = 'vnav';  % real-time
        prefix_pp = 'pnav';  % post-processing
        
        suffix = '.out';
        
        filename_rt = [path prefix_rt kernel suffix];
        filename_pp = [path prefix_pp kernel suffix];
        
        filename = filename_rt;
        if test_file(filename,1) == 0 
          if test_file(filename_pp,1) ~= 0
            filename = filename_pp;
          else
            fprintf('No valid %s or %s file not found\n',filename_rt, filename_pp);
            return
          end
        end
        
        if test_file(filename_rt,1) ~= 0 && test_file(filename_pp,1) ~= 0
          fprintf('Detected valid %s and %s file\n',filename_rt, filename_pp);
          fprintf('Do not know which one to select.\n');
          return
        end

        % data structure has 1(time) + 16  doubles per record
        data = load_data(filename,16);              % External function
        
        if isscalar(data)
          return
        end
        
        %%%%%%%%%%%%%%%%%%
        % Get figure and Y-axis label captions
        
        figure_caption = set_figure_captions;       % Private function below
        y_label_caption = set_y_label_captions;     % Private function below
        
        %%%%%%%%%%%%%%%%%%
        % Bring up new figure window and set its properties
        
        new_plot_figure(0,'IIN Navigation Solution for ',kernel);
        current_plot_num = 1;
        
        %%%%%%%%%%%%%%%%%%
        % Plot data
        
        num_plots = 19;

        % Data record has an element that is not plotted and
        % additional plots are made of derived data so a simple index of
        % data to plot number is not possible

        %1	time			Record time of validity (seconds)           8
        %2	latitude		GPS latitude (degrees)                      8
        %3	longitude		GPS longitude (degrees)                     8
        %4	altitude		GPS ellipsoidal altitude (meters)           8
        %5	velocity[0]		North velocity (meters/second)              8
        %6	velocity[1]		East  velocity (meters/second)              8
        %7	velocity[2]		Down  velocity (meters/second)              8
        %8	attitude[0]		Roll    (degrees)                           8
        %9	attitude[1]		Pitch   (degrees)                           8
        %10	attitude[2]		Heading (degrees)                           8
        %11	wander_angle	Wander angle (degrees)                      8
        %12	acceleration[0]	X Body frame acceleration (meters/second2)  8
        %13	acceleration[1]	Y Body frame acceleration (meters/second2)  8
        %14	acceleration[2]	Z Body frame acceleration (meters/second2)  8
        %15	angular_rate[0]	X Body frame angular rate (degrees/second)  8
        %16	angular_rate[1]	Y Body frame angular rate (degrees/second)  8
        %17	angular_rate[3]	Z Body frame angular rate (degrees/second)  8

        speed = sqrt(data(:,5).^2 + data(:,6).^2 + data(:,7).^2);
        total_accel = sqrt(data(:,12).^2 + data(:,13).^2 + data(:,14).^2);
        total_angular_rate = sqrt(data(:,15).^2 + data(:,16).^2 + data(:,17).^2);

        % Due to plot axis labelling limitation and the number of significant digits in GPS time
        % subtract the start time and plot seconds from start - start_time is given in x-axis label

        time = data(:,1);
        if (start_time < 0.0)
            start_time = time(1);
        end;

        for i=1:num_plots
            axis_handle(i) = axes;        
            switch i
                case 1
                    plot_handle(i) = plot(data(:,3),data(:,2),'k');
                case 2
                    plot_handle(i) = plot(time-start_time,data(:,2),'k');
                case 3
                    plot_handle(i) = plot(time-start_time,data(:,3),'k');
                case 4
                    plot_handle(i) = plot(time-start_time,data(:,4),'k');
                    
                case 5
                    plot_handle(i) = plot(time-start_time,data(:,5),'k');
                case 6
                    plot_handle(i) = plot(time-start_time,data(:,6),'k');
                case 7
                    plot_handle(i) = plot(time-start_time,data(:,7),'k');
                case 8
                    plot_handle(i) = plot(time-start_time,speed,'k');
                
                case 9
                    plot_handle(i) = plot(time-start_time,data(:,8),'k');
                case 10
                    plot_handle(i) = plot(time-start_time,data(:,9),'k');
                case 11
                    plot_handle(i) = plot(time-start_time,data(:,10),'k');

                case 12
                    plot_handle(i) = plot(time-start_time,data(:,12),'k');
                case 13
                    plot_handle(i) = plot(time-start_time,data(:,13),'k');
                case 14
                    plot_handle(i) = plot(time-start_time,data(:,14),'k');
                case 15
                    plot_handle(i) = plot(time-start_time,total_accel,'k');
                    
                case 16
                    plot_handle(i) = plot(time-start_time,data(:,15),'k');
                case 17
                    plot_handle(i) = plot(time-start_time,data(:,16),'k');
                case 18
                    plot_handle(i) = plot(time-start_time,data(:,17),'k');
                case 19
                    plot_handle(i) = plot(time-start_time,total_angular_rate ,'k');
            end
            
            if i > 1
                set(axis_handle(i),'visible','off')
                set(plot_handle(i),'visible','off')
            end
            title(char(figure_caption(i)));
            ylabel(y_label_caption(i));
            if i==1
                x_label_caption = sprintf('Longitude (degrees)');
                xlabel(x_label_caption);
            else
                x_label_caption = sprintf('Time in seconds starting at %i',start_time);
                xlabel(x_label_caption);
                axis('tight')
            end
            
        end % for i=1:num_plots
        
        axes(axis_handle(current_plot_num));
        
        %create buttons
        
        hleft = uicontrol('style','push','pos',[10 50 20 20],'TooltipString','Cycle left',...
            'callback',[ thismfile '(''left'');' ],'fontname','symbol','string',char(172));
        hright = uicontrol('style','push','pos',[10 30 20 20],'TooltipString','Cycle right',...
            'callback',[ thismfile '(''right'');' ],'fontname','symbol','string',char(174));
        hlist = uicontrol('style','push','pos',[10 10 40 20],'TooltipString','Plot selection list',...
            'callback',[ thismfile '(''list'');' ],'string','List');

        % only need the one zoom -need to be after buttons for Matlab 7
        zoom on

    case 'left'

        current_plot_num = current_plot_num - 1;
        if current_plot_num < 1
            current_plot_num = num_plots;   % Wrap to last plot
        end
        
        for i=1:num_plots
            set(axis_handle(i),'visible','off')
            set(plot_handle(i),'visible','off')
        end
        set(axis_handle(current_plot_num),'visible','on')
        set(plot_handle(current_plot_num),'visible','on')
        axes(axis_handle(current_plot_num))

    case 'right'

        current_plot_num = current_plot_num + 1;
        if current_plot_num > num_plots
            current_plot_num = 1;           % Wrap to first plot
        end
        
        for i=1:num_plots
            set(axis_handle(i),'visible','off')
            set(plot_handle(i),'visible','off')
        end
        set(axis_handle(current_plot_num),'visible','on')
        set(plot_handle(current_plot_num),'visible','on')
        axes(axis_handle(current_plot_num))

    case 'list'

        if ishandle(h_fig_listbox) == 0
            h_fig_listbox = figure('name',['Plot Selection List'],'NumberTitle', 'off','Menu','none');     % ' char(figure_caption(i))]);
            set(h_fig_listbox,'units','characters');
    
            apos=get(h_fig_listbox,'pos');
            %set vertical position
            top = apos(2) + apos(4);  % bottom+ height
            new_height = num_plots + 7;
            new_bottom = top - new_height;
            apos(2) = new_bottom;
            apos(4) = new_height;
            %set horiz position
            new_left = apos(1) + 0.25*apos(3); % left +25% of width
            new_width = 40;
            apos(1) = new_left;
            apos(3) = new_width;
            set(h_fig_listbox,'pos',apos);
    
            h_listbox = uicontrol('style','listbox','units','normalized','pos',[.1 .05 .8 .8],'TooltipString','List box',...
              'callback',[ thismfile '(''listbox'');' ]);
            set(h_listbox,'value',current_plot_num);
            set(h_listbox,'string',figure_caption);
            
            h_OK = uicontrol('style','push','units','normalized','pos',[.15 .87 .25 .10],'TooltipString','OK',...
                'callback',[ thismfile '(''ListBox_OK'');' ],'string','OK');
            h_Cancel = uicontrol('style','push','units','normalized','pos',[.55 .87 .25 .10],'TooltipString','Cancel',...
                'callback',[ thismfile '(''ListBox_Cancel'');' ],'string','Cancel');
            new_plot_num = 0;
        end

    case 'listbox'

        selectiontype = get(gcbf,'SelectionType');
        if selectiontype(1)=='o'
            new_plot_num = get(h_listbox,'value');
            %update_plot(new_plot_num)
            current_plot_num = update_plot(new_plot_num,plot_handle, axis_handle , num_plots);

            close(h_fig_listbox);
            h_fig_listbox = -1;
        end    
        
        if selectiontype(1)=='n'
            new_plot_num = get(h_listbox,'value');
        end    

    case 'ListBox_OK'

        current_plot_num = update_plot(new_plot_num,plot_handle, axis_handle , num_plots);
        close(h_fig_listbox);
        h_fig_listbox = -1;
           
    case 'ListBox_Cancel'

        close(h_fig_listbox);
        h_fig_listbox = -1;
        
end %switch

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function current_plot_num = update_plot(new_plot_num,plot_handle, axis_handle , num_plots)

    if (new_plot_num > 0) & (new_plot_num <= num_plots)
    
        current_plot_num = new_plot_num;
        for i=1:num_plots
            set(axis_handle(i),'visible','off')
            set(plot_handle(i),'visible','off')
        end
        set(axis_handle(current_plot_num),'visible','on')
        set(plot_handle(current_plot_num),'visible','on')
        axes(axis_handle(current_plot_num))
        
    end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function y_label_caption = set_y_label_captions

%use cells to handle variable length strings
y_label_caption(1) = {'Latitude (degrees)'};  %x label is longitude
y_label_caption(2) = {'Latitude (degrees)'};
y_label_caption(3) = {'Longitude (degrees)'};
y_label_caption(4) = {'Altitude (meters)'};

y_label_caption(5) = {'North velocity (meters/second)'};                        
y_label_caption(6) = {'East velocity (meters/second)'};                        
y_label_caption(7) = {'Down velocity (meters/second)'};    
y_label_caption(8) = {'Speed (meters/second)'};    
                   
y_label_caption(9) = {'Roll (degrees)'};     
y_label_caption(10) = {'Pitch (degrees)'};     
y_label_caption(11) = {'Heading (degrees)'};

y_label_caption(12) = {'X acceleration (meters/second/second)'};       
y_label_caption(13) = {'Y acceleration (meters/second/second)'};       
y_label_caption(14) = {'Z acceleration (meters/second/second)'};       
y_label_caption(15) = {'Total acceleration (meters/second/second)'};       

y_label_caption(16) = {'X angular rate (degrees/second)'};       
y_label_caption(17) = {'Y angular rate (degrees/second)'};       
y_label_caption(18) = {'Z angular rate (degrees/second)'};       
y_label_caption(19) = {'Total angular rate (degrees/second)'};       

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function figure_caption = set_figure_captions

%use cells to handle variable length strings

figure_caption(1) = {'Position Plan View'}; 
figure_caption(2) = {'Latitude'};
figure_caption(3) = {'Longitude'};
figure_caption(4) = {'Altitude'};
figure_caption(5) = {'North velocity'};                        
figure_caption(6) = {'East velocity'};                        
figure_caption(7) = {'Down velocity'};    
figure_caption(8) = {'Speed'};    
figure_caption(9) = {'Roll'};     
figure_caption(10) = {'Pitch'};     
figure_caption(11) = {'Heading'};
figure_caption(12) = {'X acceleration'};       
figure_caption(13) = {'Y acceleration'};       
figure_caption(14) = {'Z acceleration'};       
figure_caption(15) = {'Total acceleration'};       
figure_caption(16) = {'X angular rate'};       
figure_caption(17) = {'Y angular rate'};       
figure_caption(18) = {'Z angular rate'};       
figure_caption(19) = {'Total angular rate'};       

