function [ perf ] = REVS_calc_performance_stats( time, datalog, varargin)
%REVS_calc_performance_stats

start_phase = parse_varargs(varargin,'start_phase', 1,'numeric');

print_results = parse_varargs( varargin, 'verbose', true, 'numeric' );
plot_results = parse_varargs( varargin, 'do_plots', false, 'toggle' );

phase = datalog.drive_cycle_phase;
%speed_mps = lowpass_filter(datalog.vehicle.speed_mps, time, 1);    %
%seemed to cause NaNs as results, need to check on this...
speed_mps = datalog.vehicle.speed_mps;
distance_m = datalog.vehicle.distance_m;

% Phase 1
start_dist_m = distance_m(find(phase == start_phase + 0,1,'first'));
idx_0 =  find(phase == start_phase + 0 & speed_mps <= (0.0 * convert.mph2mps),1,'last');
%idx_0 =  find(phase == start_phase + 0 & (datalog.drive_cycle_speed_mps > 0.0),1,'first');

perf.start_time = time(idx_0);

%idx_0 = find(datalog.drive_cycle_phase == start_phase + 0 & (datalog.drive_cycle_speed_mps <= 0.01 * convert.mph2mps), 1,'last');
idx_30 = find(phase ==  start_phase + 0 & speed_mps >= (30.0 * convert.mph2mps),1,'first');
idx_60 = find(phase ==  start_phase + 0 & speed_mps >= (60.0 * convert.mph2mps),1,'first');
idx_qtr_mi = find(phase ==  start_phase + 0 & distance_m - start_dist_m >= (0.25 * convert.mi2mtr),1,'first');
idx_half_mi = find(phase ==  start_phase + 0 & distance_m - start_dist_m >= (0.5 * convert.mi2mtr),1,'first');

idx_30 = idx_30 + [-1,0];
time_30 = interp1(speed_mps(idx_30), time(idx_30), 30 * convert.mph2mps );
perf.zero_to_thirty_time_secs = time_30 - perf.start_time;

idx_60 = idx_60 + [-1,0];
time_60 = interp1(speed_mps(idx_60), time(idx_60), 60 * convert.mph2mps );
perf.zero_to_sixty_time_secs = time_60 - perf.start_time;

if plot_results
    fplothg(time, datalog.vehicle.speed_mps * convert.mps2mph);
    plothg(time, datalog.drive_cycle_speed_mps * convert.mps2mph, 'm.-');
    lineat(60 * convert.mps2mph,'g');
    lineat(30,'m');
    lineat(60,'r');
    vlineat(perf.start_time,'g');
    vlineat(time_30,'m');
    vlineat(time_60,'r');
    xlim([perf.start_time-1, time_60+1]);
    ylim([0 65]);
    title('0-30 and 0-60');
end

idx_qtr_mi = idx_qtr_mi + [-1,0];
time_qtr_mi = interp1(distance_m(idx_qtr_mi), time(idx_qtr_mi), start_dist_m + (0.25 * convert.mi2mtr) );
perf.quarter_mile_time_secs = time_qtr_mi - perf.start_time;
perf.quarter_mile_speed_mps = interp1( time, speed_mps, time_qtr_mi);
perf.quarter_mile_speed_mph = perf.quarter_mile_speed_mps * convert.mps2mph;

idx_half_mi = idx_half_mi + [-1,0];
time_half_mi = interp1(distance_m(idx_half_mi), time(idx_half_mi), start_dist_m + (0.50 * convert.mi2mtr) );
perf.half_mile_time_secs = time_half_mi - perf.start_time;
perf.half_mile_speed_mps = datalog.vehicle.speed_mps(idx_half_mi(2));
perf.half_mile_speed_mph = perf.half_mile_speed_mps * convert.mps2mph;

accel_mps2 = derivative(datalog.vehicle.speed_mps, 0) ./ derivative(time, 0);

%Phase 2
%ref_30 = datalog.vehicle.speed_mps(find(datalog.drive_cycle_phase == start_phase + 1,1,'first')) + (0.001*convert.mph2mps);
ref_30 = 30.00 * convert.mph2mps;
%idx_30 = find(datalog.drive_cycle_phase ==  start_phase + 1 & datalog.vehicle.speed_mps <= ref_30,1,'last');
idx_30 = find(datalog.drive_cycle_phase ==  start_phase + 1 & datalog.drive_cycle_speed_mps >= ref_30 & accel_mps2 >= 0.001,1,'first') - 1;
idx_50 = find(datalog.drive_cycle_phase ==  start_phase + 1 & datalog.vehicle.speed_mps >= (50 * convert.mph2mps),1,'first');

% %idx_30 = idx_30 + [-1,0,1];
% idx_30 = idx_30 + [0,1];
% %time_30 = interp1(speed_mps(idx_30), time(idx_30), ref_30 );
% time_30 = interp1(datalog.drive_cycle_speed_mps(idx_30), time(idx_30), ref_30 );
time_30 = time(idx_30);

idx_50 = idx_50 + [-1,0];
time_50 = interp1(speed_mps(idx_50), time(idx_50), 50 * convert.mph2mps );

perf.thirty_to_fifty_time_secs =  time_50 - time_30;

if plot_results
    fplothg(time, datalog.vehicle.speed_mps * convert.mps2mph);
    plothg(time, datalog.drive_cycle_speed_mps * convert.mps2mph, 'm.-');
    lineat(ref_30 * convert.mps2mph,'g');
    lineat(50,'r');
    vlineat(time_30,'g');
    vlineat(time_50,'r');
    xlim([time_30-1 time_50+1]);
    ylim([25 55]);
    title('30-50');
end

% Phase 3
%ref_50 = datalog.vehicle.speed_mps(find(datalog.drive_cycle_phase == start_phase + 2,1,'first')) + (0.001*convert.mph2mps);
ref_50 = 50.00 * convert.mph2mps;
%idx_50 = find(datalog.drive_cycle_phase ==  start_phase + 2 & datalog.vehicle.speed_mps <= ref_50, 1, 'last');
idx_50 = find(datalog.drive_cycle_phase ==  start_phase + 2 & datalog.drive_cycle_speed_mps >= ref_50 & accel_mps2 >= 0.001,1,'first') - 1;
idx_70 = find(datalog.drive_cycle_phase == start_phase + 2 & datalog.vehicle.speed_mps >= (70 * convert.mph2mps), 1, 'first');

% idx_50 = idx_50 + [0,1];
% %time_50 = interp1(speed_mps(idx_50), time(idx_50), ref_50 );
% time_50 = interp1(datalog.drive_cycle_speed_mps(idx_50), time(idx_50), ref_50 );
time_50 = time(idx_50);

idx_70 = idx_70 + [-1,0];
time_70 = interp1(speed_mps(idx_70), time(idx_70), 70 * convert.mph2mps );

perf.fifty_to_seventy_time_secs =  time_70 - time_50;

if plot_results
    fplothg(time, datalog.vehicle.speed_mps * convert.mps2mph);
    plothg(time, datalog.drive_cycle_speed_mps * convert.mps2mph, 'm.-');
    lineat(ref_50 * convert.mps2mph,'g');
    lineat(70,'r');
    vlineat(time_50,'g');
    vlineat(time_70,'r');
    xlim([time_50-1 time_70+1]);
    ylim([45 75]);
    title('50-70');
end

if print_results
	fprintf('\n** Performance Stats ** \n');
	fprintf(' 0 to 30 time: %f sec \n', perf.zero_to_thirty_time_secs);    
	fprintf(' 0 to 60 time: %f sec \n', perf.zero_to_sixty_time_secs);
	fprintf('30 to 50 time: %f sec \n', perf.thirty_to_fifty_time_secs);
	fprintf('50 to 70 time: %f sec \n', perf.fifty_to_seventy_time_secs);
	fprintf('1/4 mile time: %f sec @ %f mph\n', perf.quarter_mile_time_secs, perf.quarter_mile_speed_mph );
	fprintf('1/2 mile time: %f sec @ %f mph\n', perf.half_mile_time_secs, perf.half_mile_speed_mph );
	fprintf('\n');
end

