function combined_cycle = REVS_combine_drive_cycles( cycle_list, varargin)
% function combined_cycle = REVS_combine_drive_cycles( cycle_list, varargin)
%
% transition_accel  = parse_varargs( varargin, 'transition_accel',.1,'numeric');
% startup_neutral   = parse_varargs( varargin, 'startup_neutral',0,'numeric');
% startup_drive     = parse_varargs( varargin, 'startup_drive',0,'numeric');
% end_zero_speed    = parse_varargs( varargin, 'end_stopped', false, 'toggle');
% show_plot         = parse_varargs( varargin, 'do_plots',false,'toggle');
% smoothing         = parse_varargs( varargin, 'smoothing', 0, 'numeric');
% cycle_name        = parse_varargs( varargin, 'cycle_name', '', 'string');


validate_arg( cycle_list, 'cell' );

%Get varargs
transition_accel    = parse_varargs( varargin, 'transition_accel',.1,'numeric');
startup_neutral     = parse_varargs( varargin, 'startup_neutral',0,'numeric');
startup_drive       = parse_varargs( varargin, 'startup_drive',0,'numeric');
end_zero_speed      = parse_varargs( varargin, 'end_stopped', false, 'toggle');
show_plot           = parse_varargs( varargin, 'do_plots',false,'toggle');
smoothing           = parse_varargs( varargin, 'smoothing', 0, 'numeric');
save_cycle          = parse_varargs( varargin, 'save', false, 'toggle');
cycle_name          = parse_varargs( varargin, 'cycle_name', '', 'string');

%Set initial member variabes for combined_cycle class
combined_cycle.name = cycle_name;
combined_cycle.grade_dist_m = [];
combined_cycle.grade_pct = [];
combined_cycle.phase_name = {};

if startup_neutral + startup_drive > 0
    
    combined_cycle.cycle_speed_mps = [0;0];
    combined_cycle.cycle_time = [0;startup_neutral + startup_drive];
    
    combined_cycle.in_gear = [0;startup_drive>0];
    combined_cycle.in_gear_time = [0; startup_neutral];
    
    combined_cycle.ignition = [1];
    combined_cycle.ignition_time = [0];
    
    combined_cycle.phase = [0];
    combined_cycle.phase_time = [0];
    
    next_start_time = startup_neutral + startup_drive + 1;
    
else
    
    combined_cycle.cycle_speed_mps = [];
    combined_cycle.cycle_time = [];
    
    combined_cycle.in_gear = [];
    combined_cycle.in_gear_time = [];
    
    combined_cycle.ignition = [];
    combined_cycle.ignition_time = [];
    
    combined_cycle.phase = [];
    combined_cycle.phase_time = [];
    
    next_start_time = 0;
    
end

next_start_dist = 0;
prev_cycle_speed_mps = 0;
prev_cycle_end_time = 0;

for c = 1:length(cycle_list)
    
    %Check for soak
    if  all(cycle_list{c}(1:4) == 'soak')
        
        %Determine length of soak
        soak_length = str2double(cycle_list{c}(5:end));
        
        %Create dummy drive cycle struct from name
        drive_cycle = struct('name',cycle_list{c},'cycle_speed_mps',zeros(soak_length,1), ...
            'cycle_time',0:(soak_length-1),'phase',[0,0],'phase_time',[0,soak_length], ...
            'grade',[0,0],'grade_pct',[0,0],'grade_dist_m',[0,0],'in_gear',[0,0], ...
            'in_gear_time',[0,0],'ignition',[0,0],'ignition_time',[0,0]);
        
    elseif evalin('base',['exist(''' cycle_list{c} ''',''var'')']) == 1
        drive_cycle = evalin('base',cycle_list{c});
    else
        load(cycle_list{c} ,'drive_cycle');
    end
        
    if (c == 1)
        try
            combined_cycle.sample_start_enable = drive_cycle.sample_start_enable;
        catch
            combined_cycle.sample_start_enable = false;
        end
    end
        
    if  ~isempty(combined_cycle.cycle_time) % Nothing at start
        
        if combined_cycle.cycle_time(end) > combined_cycle.in_gear_time(end)
            combined_cycle.in_gear_time = [combined_cycle.in_gear_time; combined_cycle.cycle_time(end)];
            combined_cycle.in_gear = [combined_cycle.in_gear; 1];
        else
            combined_cycle.in_gear(end) = 1;
        end
        
        if combined_cycle.cycle_time(end) > combined_cycle.ignition_time(end)
            combined_cycle.ignition_time = [combined_cycle.ignition_time; combined_cycle.cycle_time(end)];
            combined_cycle.ignition = [combined_cycle.ignition; 1;];
        else
            combined_cycle.ignition(end) = 1;
        end
        
        
        if combined_cycle.cycle_time(end) > combined_cycle.phase_time(end)
            combined_cycle.phase_time = [combined_cycle.phase_time;combined_cycle.cycle_time(end)];
            combined_cycle.phase = [combined_cycle.phase; 0];
        else
            combined_cycle.phase(end) = 0;
        end
        
    end
    
    if drive_cycle.cycle_speed_mps(1) > 0
        % Insert Parabolic Transition
        transition_duration =  abs(drive_cycle.cycle_speed_mps(1) - prev_cycle_speed_mps) / transition_accel ;
        
        transition_time = 1:transition_duration;
        transition_speed = linspace(-1,0,length(transition_time)).^2*(prev_cycle_speed_mps - drive_cycle.cycle_speed_mps(1)) + drive_cycle.cycle_speed_mps(1);
        
        combined_cycle.cycle_speed_mps = [combined_cycle.cycle_speed_mps; transition_speed(:)];
        combined_cycle.cycle_time = [ combined_cycle.cycle_time; prev_cycle_end_time + transition_time(:) ];
        
        next_start_time = combined_cycle.cycle_time(end) + 1;
        next_start_dist = 1 + sum(diff(combined_cycle.cycle_time).*(combined_cycle.cycle_speed_mps(2:end)+combined_cycle.cycle_speed_mps(1:end-1)))/2 + drive_cycle.cycle_speed_mps(1)*1;
        
    elseif ~isempty(combined_cycle.cycle_speed_mps) && combined_cycle.cycle_speed_mps(end) > 0
        % Insert linear transision to zero speed
        transition_duration = abs(drive_cycle.cycle_speed_mps(1) - combined_cycle.cycle_speed_mps(end)) / (2* transition_accel );
        
        combined_cycle.cycle_time(end+1) = combined_cycle.cycle_time(end) + transition_duration ;
        combined_cycle.cycle_speed_mps(end+1) = 0;
        
        next_start_time = combined_cycle.cycle_time(end) + 1;
        next_start_dist = 1 + sum(diff(combined_cycle.cycle_time).*(combined_cycle.cycle_speed_mps(2:end)+combined_cycle.cycle_speed_mps(1:end-1)))/2;
        
        
    elseif ~isempty(combined_cycle.cycle_speed_mps) % Direct Transition
        % Zero to Zero transition 1 sec pause
        transition_duration = 0;
        
        next_start_time = combined_cycle.cycle_time(end) + transition_duration;
        next_start_dist = 0.01 + sum(diff(combined_cycle.cycle_time).*(combined_cycle.cycle_speed_mps(2:end)+combined_cycle.cycle_speed_mps(1:end-1)))/2;
        
        if combined_cycle.phase_time(end) >= combined_cycle.cycle_time(end)
            combined_cycle.phase_time(end) = [];
            combined_cycle.phase(end) = [];
        end
        
        if combined_cycle.ignition_time(end) >= combined_cycle.cycle_time(end)
            combined_cycle.ignition_time(end) = [];
            combined_cycle.ignition(end) = [];
        end
        
        if combined_cycle.in_gear_time(end) >= combined_cycle.cycle_time(end)
            combined_cycle.in_gear_time(end) = [];
            combined_cycle.in_gear(end) = [];
        end
        
        combined_cycle.cycle_time(end) = [];
        combined_cycle.cycle_speed_mps(end) = [];
        
        drive_cycle.cycle_time = drive_cycle.cycle_time;
        drive_cycle.cycle_speed_mps = drive_cycle.cycle_speed_mps;
    end
    
    max_phase = max( [0;combined_cycle.phase] );
    
    if c<2 || next_start_dist >= combined_cycle.grade_dist_m(end)
        % All good
    else
        remove_pts = combined_cycle.grade_dist_m > next_start_dist;
        add_grade = interp1( combined_cycle.grade_dist_m , combined_cycle.grade_pct, next_start_dist - 0.001);
        
        combined_cycle.grade_dist_m  = [ combined_cycle.grade_dist_m(~remove_pts) ; next_start_dist - 0.001];
        combined_cycle.grade_pct = [combined_cycle.grade_pct(~remove_pts) ; add_grade];
        
        warning('Drive cycle %s has a grade distance greater than the integrated vehicle speed clipping data to match speed trace',cycle_list{c-1});
    end
    
    
    % Append Cycle
    combined_cycle.cycle_speed_mps = [ combined_cycle.cycle_speed_mps; drive_cycle.cycle_speed_mps(:)];
    combined_cycle.cycle_time = [ combined_cycle.cycle_time;  next_start_time + drive_cycle.cycle_time(:)];
    
    combined_cycle.grade_dist_m = [ combined_cycle.grade_dist_m; next_start_dist+drive_cycle.grade_dist_m(:) ];
    combined_cycle.grade_pct = [combined_cycle.grade_pct; drive_cycle.grade_pct(:)];
    
    combined_cycle.in_gear_time = [combined_cycle.in_gear_time; next_start_time + drive_cycle.in_gear_time(:)];
    combined_cycle.in_gear = [combined_cycle.in_gear; drive_cycle.in_gear(:)];
    
    combined_cycle.ignition_time = [combined_cycle.ignition_time; next_start_time + drive_cycle.ignition_time(:)];
    combined_cycle.ignition = [combined_cycle.ignition; drive_cycle.ignition(:)];
    
    combined_cycle.phase_time = [combined_cycle.phase_time; next_start_time + drive_cycle.phase_time(:)];
    combined_cycle.phase = [combined_cycle.phase; drive_cycle.phase(:) + (drive_cycle.phase(:)>0) * max_phase ];
    
    if isempty( cycle_name ) % Check if Name was provided
        combined_cycle.name = [combined_cycle.name, drive_cycle.name, ' & '];
    end
    
    
    if isfield( drive_cycle,'phase_name')
        % Use Phase Names
        combined_cycle.phase_name = { combined_cycle.phase_name{:}, drive_cycle.phase_name{:} };
    elseif max( drive_cycle.phase ) > 1
        % Use Cycle Name & Phase #
        phases = unique(drive_cycle.phase(drive_cycle.phase > 0));
        drive_cycle.phase_name = cellstr(strcat( drive_cycle.name, '_', int2str(phases(:))));
        combined_cycle.phase_name = { combined_cycle.phase_name{:}, drive_cycle.phase_name{:} };
    else
        % Use Cycle Name
        combined_cycle.phase_name = { combined_cycle.phase_name{:}, drive_cycle.name };
    end
    
    prev_cycle_speed_mps = combined_cycle.cycle_speed_mps(end);
    prev_cycle_end_time = combined_cycle.cycle_time(end);
    
end



if combined_cycle.cycle_speed_mps( end) > 0 && end_zero_speed
    %Add Transition to zero speed
    transition_duration = abs(0 - combined_cycle.cycle_speed_mps(end)) / (2 * transition_accel );
    
    combined_cycle.cycle_time(end+1) = combined_cycle.cycle_time(end) + transition_duration ;
    combined_cycle.cycle_speed_mps(end+1) = 0;
end

% Close Out Cycle
if combined_cycle.cycle_time(end) > combined_cycle.in_gear_time(end)
    combined_cycle.in_gear_time = [combined_cycle.in_gear_time; combined_cycle.cycle_time(end)];
    combined_cycle.in_gear = [combined_cycle.in_gear; 0];
else
    combined_cycle.in_gear(end) = 0;
end

if combined_cycle.cycle_time(end) > combined_cycle.ignition_time(end)
    combined_cycle.ignition_time = [combined_cycle.ignition_time; combined_cycle.cycle_time(end)];
    combined_cycle.ignition = [combined_cycle.ignition; 0;];
else
    combined_cycle.ignition(end) = 0;
end


if combined_cycle.cycle_time(end) > combined_cycle.phase_time(end)
    combined_cycle.phase_time = [combined_cycle.phase_time;combined_cycle.cycle_time(end)];
    combined_cycle.phase = [combined_cycle.phase; 0];
else
    combined_cycle.phase(end) = 0;
end


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

if isempty( cycle_name ) % Remove Trailing _ of generated name
    combined_cycle.name = combined_cycle.name(1:end-3);
end

if show_plot
    REVS_plot_drive_cycle(combined_cycle);
end

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