function drive_cycle = REVS_create_drive_cycle( cycle_name, cycle_speed_mps, cycle_time, varargin)
% function drive_cycle = REVS_create_drive_cycle( cycle_name, cycle_speed_mps, cycle_time, varargin)
%
% startup_neutral                 = parse_varargs( varargin, 'startup_neutral', 0, 'numeric'); 
% startup_drive                   = parse_varargs( varargin, 'startup_drive', 0, 'numeric'); 
% show_plot                       = parse_varargs( varargin, 'do_plots', false, 'toggle'); 
% drive_cycle.grade_dist_m        = parse_varargs( varargin, 'grade_dist_m', [0 integrate(cycle_speed_mps, cycle_time,1)], 'numeric');
% drive_cycle.grade_pct           = parse_varargs( varargin, 'grade_pct', [0 0], 'numeric');
% drive_cycle.phase               = parse_varargs( varargin, 'cycle_phase', [1 1], 'numeric');
% drive_cycle.phase_time          = parse_varargs( varargin, 'cycle_phase_start_time', [0 cycle_time(end)], 'numeric');
% drive_cycle.phase_name          = parse_varargs( varargin, 'cycle_phase_name', {'phase_1'});
% drive_cycle.ignition            = parse_varargs( varargin, 'cycle_phase_start_time', [1 1], 'numeric');
% drive_cycle.ignition_time       = parse_varargs( varargin, 'cycle_phase_name', [0 cycle_time(end)], 'numeric');
% drive_cycle.sample_start_enable = parse_varargs( varargin, 'sample_start_enable', false, 'toggle');
% save_cycle                      = parse_varargs( varargin, 'save', false, 'toggle');
% smoothing                       = parse_varargs( varargin, 'smoothing', 0, 'numeric');
% lowpass_filter_Hz               = parse_varargs( varargin, 'low_pass_cutoff_Hz', 0, 'numeric');

if (length(cycle_speed_mps) == 1) && (length(cycle_time) == 1)
    cycle_speed_mps = [cycle_speed_mps, cycle_speed_mps];
    cycle_time = [0 cycle_time];
end

startup_neutral             = parse_varargs( varargin, 'startup_neutral', 0, 'numeric'); 
startup_drive               = parse_varargs( varargin, 'startup_drive', 0, 'numeric'); 
show_plot                   = parse_varargs( varargin, 'do_plots', false, 'toggle'); 

if isscalar(cycle_speed_mps) && isempty(cycle_time)
    drive_cycle.grade_dist_m    = parse_varargs( varargin, 'grade_dist_m', [], 'numeric');
    if ~isempty(drive_cycle.grade_dist_m)
        cycle_time = [0; drive_cycle.grade_dist_m(end)/cycle_speed_mps];
        cycle_speed_mps = [cycle_speed_mps; cycle_speed_mps];
    else
        error('grade_dist_m required for constant speed trace without time vector');
    end
else
    drive_cycle.grade_dist_m    = parse_varargs( varargin, 'grade_dist_m', [0 trapz(cycle_time, cycle_speed_mps)], 'numeric');
end

drive_cycle.grade_pct           = parse_varargs( varargin, 'grade_pct', [0 0], 'numeric');
if length(drive_cycle.grade_pct) == 1
    drive_cycle.grade_pct = [drive_cycle.grade_pct drive_cycle.grade_pct];
end
drive_cycle.phase               = parse_varargs( varargin, 'cycle_phase', [1 1], 'numeric');
drive_cycle.phase_time          = parse_varargs( varargin, 'cycle_phase_start_time', [0 cycle_time(end)], 'numeric');
drive_cycle.phase_name          = parse_varargs( varargin, 'cycle_phase_name', {'phase_1'});
drive_cycle.ignition            = parse_varargs( varargin, 'cycle_phase_start_time', [1 1], 'numeric');
drive_cycle.ignition_time       = parse_varargs( varargin, 'cycle_phase_name', [0 cycle_time(end)], 'numeric');
drive_cycle.sample_start_enable = parse_varargs( varargin, 'sample_start_enable', false, 'toggle');
save_cycle                      = parse_varargs( varargin, 'save', false, 'toggle');
smoothing                       = parse_varargs( varargin, 'smoothing', 0, 'numeric');
lowpass_filter_Hz               = parse_varargs( varargin, 'low_pass_cutoff_Hz', 0, 'numeric');

%% 

drive_cycle.name = cycle_name;

if startup_neutral + startup_drive > 0
	drive_cycle.cycle_speed_mps = [0; 0;                                cycle_speed_mps(:)];
	drive_cycle.cycle_time      = [0; startup_neutral + startup_drive;  cycle_time(:) + startup_neutral + startup_drive + eps];
 
	drive_cycle.in_gear         = [0; startup_drive > 0];
	drive_cycle.in_gear_time    = [0; startup_neutral;];
else
	drive_cycle.cycle_speed_mps = cycle_speed_mps(:);
	drive_cycle.cycle_time      = cycle_time(:);
 
	drive_cycle.in_gear         = [1 1];
	drive_cycle.in_gear_time    = [0 cycle_time(end)];
end

if (lowpass_filter_Hz > 0) || (smoothing > 0)
    drive_cycle.cycle_speed_mps_raw = drive_cycle.cycle_speed_mps;
    drive_cycle.cycle_time_raw = drive_cycle.cycle_time;
end

if lowpass_filter_Hz > 0
    drive_cycle.cycle_speed_mps = lowpass_filter(drive_cycle.cycle_speed_mps, drive_cycle.cycle_time, lowpass_filter_Hz);
end

if smoothing > 0    
    interp_time = (0:smoothing:drive_cycle.cycle_time(end))';
    interp_cycle = interp1( drive_cycle.cycle_time,drive_cycle.cycle_speed_mps, interp_time,'pchip');
    
    remove_pts = diff(interp_cycle) == 0;
    remove_pts = [0;remove_pts] + [remove_pts;0];
    
    drive_cycle.cycle_time = interp_time(remove_pts < 2);
    drive_cycle.cycle_speed_mps = interp_cycle(remove_pts < 2);    
end

%%
if show_plot
    REVS_plot_drive_cycle(drive_cycle);
end

% if show_plot
% 	
% 	figure;
% 	subplot(2,1,1)
% 	hold on
% 
%     if (lowpass_filter_Hz > 0) || (smoothing > 0)
%         plot(drive_cycle.cycle_time_raw, drive_cycle.cycle_speed_mps_raw,'m.-')
%     end
% 
%     plot(drive_cycle.cycle_time, drive_cycle.cycle_speed_mps,'b.-')
% 	plot(drive_cycle.phase_time, drive_cycle.phase,'kx');
% 	stairs(drive_cycle.phase_time, drive_cycle.phase,'k-');
% 	stairs(drive_cycle.ignition_time,drive_cycle.ignition*30,'r.-')
% 	stairs(drive_cycle.in_gear_time,drive_cycle.in_gear*35,'g.-')
%     
% 	subplot(2,1,2)
% 	hold on
% 	
% 	dist = integral( drive_cycle.cycle_speed_mps, drive_cycle.cycle_time, 1);
% 	plot(drive_cycle.grade_dist_m,drive_cycle.grade_pct,'r.-');
% 	plot(dist,drive_cycle.cycle_speed_mps,'b.-');
%     plot(drive_cycle.grade_dist_m,drive_cycle.grade_pct,'g.-');
% 	
% 	phase_dist = interp1( drive_cycle.cycle_time, dist, drive_cycle.phase_time,'linear','extrap');
% 	
% 	stairs(phase_dist, drive_cycle.phase,'k-');
% 	plot(phase_dist, drive_cycle.phase,'kx');
% end

if save_cycle
    uiputfile('*.mat', 'Save drive_cycle', drive_cycle.name);
end
