% function [ fig, horiz_passToPass_error ] = passToPass( dut_filename, truth_filename, dt )
%
% INPUT ARGUMENTS
% dut_filename :: 'dut' refers to device-under-test. Must be a T0X file,
%                  where X is 2 or 4.
% truth_filename :: Must be a T0X file, where X is 2 or 4. Or, a txt/TXT
%                   file from POSPAC.
% dt :: either a row vector or a column vector of timeshifts (units of
%       seconds)
%
% OUTPUT ARGUMENTS
% fig :: Figure handle
% horiz_passToPass_error :: The horizontal pass-to-pass error, i.e., the
%     repeatability of the horizontal errors. MxN matrix, where M is the
%     number of common epochs and N is the number of timeshifts plus 1
%     (i.e., N=numel(dt)+1). NOTE: this matrix contains NaNs.
% 
% SUMMARY
% The purpose of this script is to determine the pass-to-pass repeatability
% of the device-under-test. The device-under-test is compared to the truth.
% The pass-to-pass repeatability is a measure of how similar the errors are
% over time. The script generates one plot of the horizontal repeatability,
% with one line per timeshift.
function [ fig, horiz_passToPass_error, per_95 ] = passToPassRegression( dut_data, truth_data, poly, dt )

    %% COMPUTING -- repeatability
    % Timeshift and difference the data
    % horiz_passToPass_error is an MxN matrix, where M is the number of
    %     epochs and N=numel(dt)+1. The first column is the time and the
    %     subsequent columns correspond to the timeshifts.
    horiz_passToPass_error = timeShiftAndDifference( dt, dut_data, truth_data, poly );


    %% PLOTTING -- repeatability
    [fig, per_95] = plotRepeatability( dt, horiz_passToPass_error );

end



% dt is a row or column vector of timeshifts (s) to apply to the data.
% t_denu_in is Nx4 matrix, where N is the number of epochs. The first
% column contains the time, the second, third, and fourth columns contain
% the east, north, and up error (dut versus truth), respectively.
function [ horiz_passToPass_error ] = timeShiftAndDifference( dt, ...
                                                              dut_data, ...
                                                              truth_data, ...
                                                              poly)

    if ( ~isvector(dt) )
        fprintf( 'WARNING: dt must be 1xN or Nx1, where N >= 1\n' )
    end
 

    % find data inside the polygon that defines the field, we don't care
    % about the turns so 
    in = inpolygon(dut_data(:,3), dut_data(:,2), poly(:,1), poly(:,2));
    dut_data = dut_data(in,:);


    % Find the common epochs between dut and truth
    [c, ia, ib] = intersect( dut_data(:,1), ...
                             truth_data(:,1) );

    % Instantiate the dut_t_denu matrix
    dut_t_denu = NaN( numel(c), 4 );

    % Set the common epochs and compute dENU from dut and truth LLH, using
    % truth as reference
    dut_t_denu(:,1) = c;
    dut_t_denu(:,2:4) = LLH2ENU( dut_data(ia,2).*pi/180,   dut_data(ia,3).*pi/180,   dut_data(ia,4), ...
                                 truth_data(ib,2).*pi/180, truth_data(ib,3).*pi/180, truth_data(ib,4) );
    
    %%%%%%%%% -- Debug start
    debug = 0;
    if(debug)
      figure
      plot(dut_data(:,3),dut_data(:,2),'rx');
      plot_google_map('maptype','satellite');
      title('DUT Data inside polygon');

      figure
      plot( dut_t_denu(:,1),sqrt(dut_t_denu(:,2).^2 +  dut_t_denu(:,3).^2 ));
      xlabel('GPS Time [Seconds]');
      ylabel('2D Error [m]');
    end
    %%%%%%%%% -- Debug end
    
    
    % Instantiate the output
    horiz_passToPass_error = NaN( numel(c), numel(dt) + 1 );
    
    % Set the common epochs
    horiz_passToPass_error(:,1) = c;

    % Timeshift and difference the data, then calculate the pass-to-pass
    % error.
    for i = 1:numel(dt)
        [c, ia, ib] = intersect( dut_t_denu(:,1), dut_t_denu(:,1) + dt(i) );
        denu = dut_t_denu(ia,2:4) - dut_t_denu(ib,2:4);
        horiz_passToPass_error(ia,i+1) = sqrt( denu(:,1).^2 + denu(:,2).^2 );
    end

end



% dt is a row or column vector of timeshifts (s) that were applied to the
% data. t_denu is an Nx(4*M) matrix, where N is the number of epochs and M
% is the number of timeshifts that were applied to the data.
function [ fig, per_95 ] = plotRepeatability( dt, t_denu )
  colorArray(1,1:3)  = [0.0   0.0  1.0 ];
  colorArray(2,1:3)  = [1.0   0.0  0.0 ];
  colorArray(3,1:3)  = [1.0   0.75  0.0 ];
  colorArray(4,1:3)  = [0.0   1.0  0.0 ];
  colorArray(5,1:3)  = [0.0   1.0  1.0 ];
  colorArray(6,1:3)  = [1.0   0.0  1.0 ];
  colorArray(7,1:3)  = [0.0   0.0  0.0 ];
  colorArray(8,1:3)  = [1.0   1.0  0.0 ];
  colorArray(9,1:3)  = [1.0   0.0  0.6 ];
  colorArray(10,1:3) = [0.0   0.5  0.0 ];

  % New figure, hold on because we will probably plot multiple lines
  fig = figure('visible','off');
  hold on;

  % Plot one line per timeshift
  for i = 1:numel(dt)
    plot( t_denu(:,1), t_denu(:,i+1), '.-', 'color', colorArray(i,:));
      
    % Remove NaNs
    good = isfinite(t_denu(:,i+1));
    all  = t_denu(good,i+1);

    fid = fopen(['Err-',num2str(dt(i)),'-Ag.txt'], 'w' );
    for j=1:length(all)
      fprintf(fid,'%.3f\n',all(j));
    end
    fclose(fid);

    data = sort(all);
    len = length(data);

    % Find the 95 percentile
    per_95(i) = data(floor(len*0.95));
  end

  % Format the plot
  hold off;
  xlabel( 'Time, [s]' );
  ylabel( 'dHorizontal [m]' );
  title( 'Horizontal Repeatability' );
  legend( num2str( dt(:) ) );

end

