% apply the ECO tracking for each target (modified based on the original ECO code)
function tracker = ECO_tracking(frame_id, im, bboxes_det, tracker, opt)

tracker.eco.n_frame = tracker.eco.n_frame + 1;
frame = tracker.eco.n_frame;

% variables from ECO_initialize.m
params = tracker.eco.params;
max_train_samples = tracker.eco.max_train_samples;
features = tracker.eco.features;
global_fparams = tracker.eco.global_fparams;
pos = tracker.eco.pos;
target_sz = tracker.eco.target_sz;
currentScaleFactor = tracker.eco.currentScaleFactor;
base_target_sz = tracker.eco.base_target_sz;
img_support_sz = tracker.eco.img_support_sz;
feature_dim = tracker.eco.feature_dim;
num_feature_blocks = tracker.eco.num_feature_blocks;
feature_reg = tracker.eco.feature_reg;
feature_extract_info = tracker.eco.feature_extract_info;
compressed_dim = tracker.eco.compressed_dim;
compressed_dim_cell = tracker.eco.compressed_dim_cell;
filter_sz = tracker.eco.filter_sz;
filter_sz_cell = tracker.eco.filter_sz_cell;
output_sz = tracker.eco.output_sz;
pad_sz = tracker.eco.pad_sz;
kx = tracker.eco.kx;
ky = tracker.eco.ky;
yf = tracker.eco.yf;
cos_window = tracker.eco.cos_window;
interp1_fs = tracker.eco.interp1_fs;
interp2_fs = tracker.eco.interp2_fs;
reg_filter = tracker.eco.reg_filter;
reg_energy = tracker.eco.reg_energy;
nScales = tracker.eco.nScales;
scaleFactors = tracker.eco.scaleFactors;
scale_filter = tracker.eco.scale_filter;
min_scale_factor = tracker.eco.min_scale_factor;
max_scale_factor = tracker.eco.max_scale_factor;
init_CG_opts = tracker.eco.init_CG_opts;
CG_opts = tracker.eco.CG_opts;
rect_position = tracker.eco.rect_position;
prior_weights = tracker.eco.prior_weights;
sample_weights = tracker.eco.sample_weights;
samplesf = tracker.eco.samplesf;
score_matrix = tracker.eco.score_matrix;
latest_ind = tracker.eco.latest_ind;
frames_since_last_train = tracker.eco.frames_since_last_train;
num_training_samples = tracker.eco.num_training_samples;
minimum_sample_weight = tracker.eco.minimum_sample_weight;
res_norms = tracker.eco.res_norms;
is_color_image = tracker.eco.is_color_image;

% variables which are useful when frame == 1
sample_pos = tracker.eco.sample_pos;
sample_scale = tracker.eco.sample_scale;
xl = tracker.eco.xl;
xlf = tracker.eco.xlf;
projection_matrix = tracker.eco.projection_matrix;
shift_samp = tracker.eco.shift_samp;
xlf_proj = tracker.eco.xlf_proj;
hf = tracker.eco.hf;
lf_ind = tracker.eco.lf_ind;
proj_energy = tracker.eco.proj_energy;
sample_energy = tracker.eco.sample_energy;
rhs_samplef = tracker.eco.rhs_samplef;
diag_M = tracker.eco.diag_M;
p = tracker.eco.p;
rho = tracker.eco.rho;
r_old = tracker.eco.r_old;
init_samplef = tracker.eco.init_samplef;
init_samplef_H = tracker.eco.init_samplef_H;
projection_matrix_init = tracker.eco.projection_matrix_init;
init_samplef_proj = tracker.eco.init_samplef_proj;
init_hf = tracker.eco.init_hf;
fyf = tracker.eco.fyf;
res_norms_temp = tracker.eco.res_norms_temp;

% other variables
hf_full = tracker.eco.hf_full;

% narrow the bbox to avoid tracking drift
target_sz(2) = target_sz(2) * 0.5;

% load image
if size(im,3) > 1 && is_color_image == false
    im = im(:,:,1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Target localization step
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Do not estimate translation and scaling on the first frame, since we
% just want to initialize the tracker there

if frame > 1
    old_pos = inf(size(pos));
    iter = 1;

    %translation search
    while iter <= params.refinement_iterations && any(old_pos ~= pos)
        % Extract features at multiple resolutions
        sample_pos = round(pos);
        det_sample_pos = sample_pos;
        sample_scale = currentScaleFactor*scaleFactors;
        xt = extract_features(im, sample_pos, sample_scale, features, global_fparams, feature_extract_info);

        % Project sample
        xt_proj = project_sample(xt, projection_matrix);

        % Do windowing of features
        xt_proj = cellfun(@(feat_map, cos_window) bsxfun(@times, feat_map, cos_window), xt_proj, cos_window, 'uniformoutput', false);

        % Compute the fourier series
        xtf_proj = cellfun(@cfft2, xt_proj, 'uniformoutput', false);

        % Interpolate features to the continuous domain
        xtf_proj = interpolate_dft(xtf_proj, interp1_fs, interp2_fs);

        % Compute convolution for each feature block in the Fourier domain
        scores_fs_feat = cellfun(@(hf, xf, pad_sz) padarray(sum(bsxfun(@times, hf, xf), 3), pad_sz), hf_full, xtf_proj, pad_sz, 'uniformoutput', false);

        % Also sum over all feature blocks.
        % Gives the fourier coefficients of the convolution response.
        scores_fs = permute(sum(cell2mat(scores_fs_feat), 3), [1 2 4 3]);

        % Optimize the continuous score function with Newton's method.
        [trans_row, trans_col, scale_ind] = optimize_scores(scores_fs, params.newton_iterations);

        % Compute the translation vector in pixel-coordinates and round
        % to the closest integer pixel.
        translation_vec = [trans_row, trans_col] .* (img_support_sz./output_sz) * currentScaleFactor * scaleFactors(scale_ind);
        scale_change_factor = scaleFactors(scale_ind);

        % update position
        old_pos = pos;
        pos = sample_pos + translation_vec;

        if params.clamp_position
            pos = max([1 1], min([size(im,1) size(im,2)], pos));
        end

        % Do scale tracking with the scale filter
        if nScales > 0 && params.use_scale_filter
            scale_change_factor = scale_filter_track(im, pos, base_target_sz, currentScaleFactor, scale_filter, params);
        end

        % Update the scale
        currentScaleFactor = currentScaleFactor * scale_change_factor;

        % Adjust to make sure we are not to large or to small
        if currentScaleFactor < min_scale_factor
            currentScaleFactor = min_scale_factor;
        elseif currentScaleFactor > max_scale_factor
            currentScaleFactor = max_scale_factor;
        end

        iter = iter + 1;
    end
end

pre_pos = tracker.eco.pos;
pre_target_sz = tracker.eco.target_sz;
pos_diff = double(sqrt((pos(1)-pre_pos(1)).^2 + (pos(2)-pre_pos(2)).^2));
if pos_diff > 1.2 * double(pre_target_sz(2))
    pos = pre_pos;
    is_drift = 1;
else
    is_drift = 0;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Model update step
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Extract sample and init projection matrix
if frame == 1
    % Extract image region for training sample
    sample_pos = round(pos);
    sample_scale = currentScaleFactor;
    xl = extract_features(im, sample_pos, currentScaleFactor, features, global_fparams, feature_extract_info);

    % Do windowing of features
    xlw = cellfun(@(feat_map, cos_window) bsxfun(@times, feat_map, cos_window), xl, cos_window, 'uniformoutput', false);

    % Compute the fourier series
    xlf = cellfun(@cfft2, xlw, 'uniformoutput', false);

    % Interpolate features to the continuous domain
    xlf = interpolate_dft(xlf, interp1_fs, interp2_fs);

    % New sample to be added
    xlf = compact_fourier_coeff(xlf);

    % Initialize projection matrix
    xl1 = cellfun(@(x) reshape(x, [], size(x,3)), xl, 'uniformoutput', false);
    xl1 = cellfun(@(x) bsxfun(@minus, x, mean(x, 1)), xl1, 'uniformoutput', false);

    if strcmpi(params.proj_init_method, 'pca')
        [projection_matrix, ~, ~] = cellfun(@(x) svd(x' * x), xl1, 'uniformoutput', false);
        projection_matrix = cellfun(@(P, dim) single(P(:,1:dim)), projection_matrix, compressed_dim_cell, 'uniformoutput', false);
    elseif strcmpi(params.proj_init_method, 'rand_uni')
        projection_matrix = cellfun(@(x, dim) single(randn(size(x,2), dim)), xl1, compressed_dim_cell, 'uniformoutput', false);
        projection_matrix = cellfun(@(P) bsxfun(@rdivide, P, sqrt(sum(P.^2,1))), projection_matrix, 'uniformoutput', false);
    elseif strcmpi(params.proj_init_method, 'none')
        projection_matrix = [];
    else
        error('Unknown initialization method for the projection matrix: %s', params.proj_init_method);
    end
    clear xl1 xlw

    % Shift sample
    shift_samp = 2*pi * (pos - sample_pos) ./ (sample_scale * img_support_sz);
    xlf = shift_sample(xlf, shift_samp, kx, ky);

    % Project sample
    xlf_proj = project_sample(xlf, projection_matrix);
elseif params.learning_rate > 0
    if ~params.use_detection_sample
        % Extract image region for training sample
        sample_pos = round(pos);
        sample_scale = currentScaleFactor;
        xl = extract_features(im, sample_pos, currentScaleFactor, features, global_fparams, feature_extract_info);

        % Project sample
        xl_proj = project_sample(xl, projection_matrix);

        % Do windowing of features
        xl_proj = cellfun(@(feat_map, cos_window) bsxfun(@times, feat_map, cos_window), xl_proj, cos_window, 'uniformoutput', false);

        % Compute the fourier series
        xlf1_proj = cellfun(@cfft2, xl_proj, 'uniformoutput', false);

        % Interpolate features to the continuous domain
        xlf1_proj = interpolate_dft(xlf1_proj, interp1_fs, interp2_fs);

        % New sample to be added
        xlf_proj = compact_fourier_coeff(xlf1_proj);
    else
        % Use the sample that was used for detection
        sample_scale = sample_scale(scale_ind);
        xlf_proj = cellfun(@(xf) xf(:,1:(size(xf,2)+1)/2,:,scale_ind), xtf_proj, 'uniformoutput', false);
    end

    % Shift the sample so that the target is centered
    shift_samp = 2*pi * (pos - sample_pos) ./ (sample_scale * img_support_sz);
    xlf_proj = shift_sample(xlf_proj, shift_samp, kx, ky);
end

xlf_proj_perm = cellfun(@(xf) permute(xf, [4 3 1 2]), xlf_proj, 'uniformoutput', false);

if params.use_sample_merge
    % Find the distances with existing samples
    dist_vector = find_cluster_distances(samplesf, xlf_proj_perm, num_feature_blocks, num_training_samples, max_train_samples, params);

    [merged_sample, new_cluster, merged_cluster_id, new_cluster_id, score_matrix, prior_weights,num_training_samples] = ...
        merge_clusters(samplesf, xlf_proj_perm, dist_vector, score_matrix, prior_weights,...
                       num_training_samples,num_feature_blocks,max_train_samples,minimum_sample_weight,params);
else
    % Do the traditional adding of a training sample and weight update
    % of C-COT
    [prior_weights, replace_ind] = update_prior_weights(prior_weights, sample_weights, latest_ind, frame, params);
    latest_ind = replace_ind;

    merged_cluster_id = 0;
    new_cluster = xlf_proj_perm;
    new_cluster_id = replace_ind;
end

if frame > 1 && params.learning_rate > 0 || frame == 1 && ~params.update_projection_matrix
    % Insert the new training sample
    for k = 1:num_feature_blocks
        if merged_cluster_id > 0
            samplesf{k}(merged_cluster_id,:,:,:) = merged_sample{k};
        end

        if new_cluster_id > 0
            samplesf{k}(new_cluster_id,:,:,:) = new_cluster{k};
        end
    end
end

sample_weights = prior_weights;

train_tracker = (frame < params.skip_after_frame) || (frames_since_last_train >= params.train_gap);

if train_tracker && is_drift == 0
    % Used for preconditioning
    new_sample_energy = cellfun(@(xlf) abs(xlf .* conj(xlf)), xlf_proj, 'uniformoutput', false);

    if frame == 1
        if params.update_projection_matrix
            hf = cell(2,1,num_feature_blocks);
            lf_ind = cellfun(@(sz) sz(1) * (sz(2)-1)/2 + 1, filter_sz_cell, 'uniformoutput', false);
            proj_energy = cellfun(@(P, yf) 2*sum(abs(yf(:)).^2) / sum(feature_dim) * ones(size(P), 'single'), projection_matrix, yf, 'uniformoutput', false);
        else
            hf = cell(1,1,num_feature_blocks);
        end
        % Initialize the filter
        for k = 1:num_feature_blocks
            hf{1,1,k} = complex(zeros([filter_sz(k,1) (filter_sz(k,2)+1)/2 compressed_dim(k)], 'single'));
        end

        % Initialize Conjugate Gradient parameters
        CG_opts.maxit = params.init_CG_iter; % Number of initial iterations if projection matrix is not updated
        init_CG_opts.maxit = ceil(params.init_CG_iter / params.init_GN_iter);
        sample_energy = new_sample_energy;
        rhs_samplef = cell(size(hf));
        diag_M = cell(size(hf));
        p = []; rho = []; r_old = [];
    else
        CG_opts.maxit = params.CG_iter;

        if params.CG_forgetting_rate == inf || params.learning_rate >= 1
            % CG will be reset
            p = []; rho = []; r_old = [];
        else
            rho = rho / (1-params.learning_rate)^params.CG_forgetting_rate;
        end
        % Update the approximate average sample energy using the learning
        % rate. This is only used to construct the preconditioner.
        sample_energy = cellfun(@(se, nse) (1 - params.learning_rate) * se + params.learning_rate * nse, sample_energy, new_sample_energy, 'uniformoutput', false);
    end

    % Do training
    if frame == 1 && params.update_projection_matrix
        % Initial Gauss-Newton optimization of the filter and
        % projection matrix.

        % Construct stuff for the proj matrix part
        init_samplef = cellfun(@(x) permute(x, [4 3 1 2]), xlf, 'uniformoutput', false);
        init_samplef_H = cellfun(@(X) conj(reshape(X, size(X,2), [])), init_samplef, 'uniformoutput', false);

        % Construct preconditioner
        diag_M(1,1,:) = cellfun(@(m, reg_energy) (1-params.precond_reg_param) * bsxfun(@plus, params.precond_data_param * m, (1-params.precond_data_param) * mean(m,3)) + params.precond_reg_param*reg_energy, sample_energy, reg_energy, 'uniformoutput',false);
        diag_M(2,1,:) = cellfun(@(m) params.precond_proj_param * (m + params.projection_reg), proj_energy, 'uniformoutput',false);

        projection_matrix_init = projection_matrix;

        for iter = 1:params.init_GN_iter
            % Project sample with new matrix
            init_samplef_proj = cellfun(@(x,P) mtimesx(x, P, 'speed'), init_samplef, projection_matrix, 'uniformoutput', false);
            init_hf = cellfun(@(x) permute(x, [3 4 1 2]), hf(1,1,:), 'uniformoutput', false);

            % Construct the right hand side vector for the filter part
            rhs_samplef(1,1,:) = cellfun(@(xf, yf) bsxfun(@times, conj(permute(xf, [3 4 2 1])), yf), init_samplef_proj, yf, 'uniformoutput', false);

            % Construct the right hand side vector for the projection matrix part
            fyf = cellfun(@(f, yf) reshape(bsxfun(@times, conj(f), yf), [], size(f,3)), hf(1,1,:), yf, 'uniformoutput', false);
            rhs_samplef(2,1,:) = cellfun(@(P, XH, fyf, fi) (2*real(XH * fyf - XH(:,fi:end) * fyf(fi:end,:)) - params.projection_reg * P), ...
                projection_matrix, init_samplef_H, fyf, lf_ind, 'uniformoutput', false);

            % Initialize the projection matrix increment to zero
            hf(2,1,:) = cellfun(@(P) zeros(size(P), 'single'), projection_matrix, 'uniformoutput', false);

            % do conjugate gradient
            [hf, ~, ~, ~, res_norms_temp] = pcg_ccot(...
                @(x) lhs_operation_joint(x, init_samplef_proj, reg_filter, feature_reg, init_samplef, init_samplef_H, init_hf, params.projection_reg),...
                rhs_samplef, init_CG_opts, ...
                @(x) diag_precond(x, diag_M), ...
                [], hf);

            % Make the filter symmetric (avoid roundoff errors)
            hf(1,1,:) = symmetrize_filter(hf(1,1,:));

            % Add to the projection matrix
            projection_matrix = cellfun(@plus, projection_matrix, hf(2,1,:), 'uniformoutput', false);

            res_norms = [res_norms; res_norms_temp];
        end

        % Extract filter
        hf = hf(1,1,:);

        % Re-project and insert training sample
        xlf_proj = project_sample(xlf, projection_matrix);
        for k = 1:num_feature_blocks
            samplesf{k}(1,:,:,:) = permute(xlf_proj{k}, [4 3 1 2]);
        end

        if debug
            norm_proj_mat_init = sqrt(sum(cellfun(@(P) norm(P(:))^2, projection_matrix_init)));
            norm_proj_mat = sqrt(sum(cellfun(@(P) norm(P(:))^2, projection_matrix)));
            norm_proj_mat_change = sqrt(sum(cellfun(@(P,P2) norm(P(:) - P2(:))^2, projection_matrix_init, projection_matrix)));
            fprintf('Norm init: %f, Norm final: %f, Matrix change: %f\n', norm_proj_mat_init, norm_proj_mat, norm_proj_mat_change / norm_proj_mat_init);
        end
    else
        % Construct the right hand side vector
        rhs_samplef = cellfun(@(xf) permute(mtimesx(sample_weights, 'T', xf, 'speed'), [3 4 2 1]), samplesf, 'uniformoutput', false);
        rhs_samplef = cellfun(@(xf, yf) bsxfun(@times, conj(xf), yf), rhs_samplef, yf, 'uniformoutput', false);

        % Construct preconditioner
        diag_M = cellfun(@(m, reg_energy) (1-params.precond_reg_param) * bsxfun(@plus, params.precond_data_param * m, (1-params.precond_data_param) * mean(m,3)) + params.precond_reg_param*reg_energy, sample_energy, reg_energy, 'uniformoutput',false);

        % do conjugate gradient
        [hf, ~, ~, ~, res_norms, p, rho, r_old] = pcg_ccot(...
            @(x) lhs_operation(x, samplesf, reg_filter, sample_weights, feature_reg),...
            rhs_samplef, CG_opts, ...
            @(x) diag_precond(x, diag_M), ...
            [], hf, p, rho, r_old);
    end

    % Reconstruct the full Fourier series
    hf_full = full_fourier_coeff(hf);

    frames_since_last_train = 0;
else
    frames_since_last_train = frames_since_last_train+1;
end

% Update the scale filter
if nScales > 0 && params.use_scale_filter
    scale_filter = scale_filter_update(im, pos, base_target_sz, currentScaleFactor, scale_filter, params);
end

% Update the target size (only used for computing output box)
target_sz = base_target_sz * currentScaleFactor;

% restore the size of bbox (narrowed before to avoid tracking drift)
target_sz(2) = target_sz(2) * 2.0;

%save position and calculate FPS
rect_position(frame,:) = round([pos([2,1]) - (target_sz([2,1]) - 1)/2, target_sz([2,1])]);

sampled_scores_display = fftshift(sample_fs(scores_fs(:,:,scale_ind), 10*output_sz));

pre_pos = tracker.eco.pos;
pre_target_sz = tracker.eco.target_sz;
bbox.x = double(pre_pos(2) - (pre_target_sz(2) - 1)/2);
bbox.y = double(pre_pos(1) - (pre_target_sz(1) - 1)/2);
bbox.w = pre_target_sz(2);
bbox.h = pre_target_sz(1);

if isempty(bboxes_det.fr) == 0
    o = calc_overlap(bbox, 1, bboxes_det, 1:numel(bboxes_det.fr));
    [overlap_box, index] = max(o);
    pre_area = bbox.w * bbox.h;
    cur_area = bboxes_det.w(index) * bboxes_det.h(index);
    if overlap_box > opt.det_overlap_thre && max(bbox.h, bboxes_det.h(index))/min(bbox.h, bboxes_det.h(index)) <= 1.3
        pos_det = single([bboxes_det.y(index) + (bboxes_det.h(index) - 1)/2, bboxes_det.x(index) + (bboxes_det.w(index) - 1)/2]);
        target_sz_det = [bboxes_det.h(index), bboxes_det.w(index)];
        pos = single(overlap_box * pos_det + (1 - overlap_box) * pos);
        target_sz = overlap_box * target_sz_det + (1 - overlap_box) * target_sz;
    end
end

% variables from ECO_initialize.m
tracker.eco.params = params;
tracker.eco.max_train_samples = max_train_samples;
tracker.eco.features = features;
tracker.eco.global_fparams = global_fparams;
tracker.eco.pos = pos;
tracker.eco.target_sz = target_sz;
tracker.eco.currentScaleFactor = currentScaleFactor;
tracker.eco.base_target_sz = base_target_sz;
tracker.eco.img_support_sz = img_support_sz;
tracker.eco.feature_dim = feature_dim;
tracker.eco.num_feature_blocks = num_feature_blocks;
tracker.eco.feature_reg = feature_reg;
tracker.eco.feature_extract_info = feature_extract_info;
tracker.eco.compressed_dim = compressed_dim;
tracker.eco.compressed_dim_cell = compressed_dim_cell;
tracker.eco.filter_sz = filter_sz;
tracker.eco.filter_sz_cell = filter_sz_cell;
tracker.eco.output_sz = output_sz;
tracker.eco.pad_sz = pad_sz;
tracker.eco.kx = kx;
tracker.eco.ky = ky;
tracker.eco.yf = yf;
tracker.eco.cos_window = cos_window;
tracker.eco.interp1_fs = interp1_fs;
tracker.eco.interp2_fs = interp2_fs;
tracker.eco.reg_filter = reg_filter;
tracker.eco.reg_energy = reg_energy;
tracker.eco.nScales = nScales;
tracker.eco.scaleFactors = scaleFactors;
tracker.eco.scale_filter = scale_filter;
tracker.eco.min_scale_factor = min_scale_factor;
tracker.eco.max_scale_factor = max_scale_factor;
tracker.eco.init_CG_opts = tracker.eco.init_CG_opts;
tracker.eco.CG_opts = CG_opts;
tracker.eco.rect_position = rect_position;
tracker.eco.prior_weights = prior_weights;
tracker.eco.sample_weights = sample_weights;
tracker.eco.samplesf = samplesf;
tracker.eco.score_matrix = score_matrix;
tracker.eco.latest_ind = latest_ind;
tracker.eco.frames_since_last_train = frames_since_last_train;
tracker.eco.num_training_samples = num_training_samples;
tracker.eco.minimum_sample_weight = minimum_sample_weight;
tracker.eco.res_norms = res_norms;
tracker.eco.is_color_image = is_color_image;

% variables which are useful when frame == 1
tracker.eco.sample_pos = sample_pos;
tracker.eco.sample_scale = sample_scale;
tracker.eco.xl = xl;
tracker.eco.xlf = xlf;
tracker.eco.projection_matrix = projection_matrix;
tracker.eco.shift_samp = shift_samp;
tracker.eco.xlf_proj = xlf_proj;
tracker.eco.hf = hf;
tracker.eco.lf_ind = lf_ind;
tracker.eco.proj_energy = proj_energy;
tracker.eco.sample_energy = sample_energy;
tracker.eco.rhs_samplef = rhs_samplef;
tracker.eco.diag_M = diag_M;
tracker.eco.p = p;
tracker.eco.rho = rho;
tracker.eco.r_old = r_old;
tracker.eco.init_samplef = init_samplef;
tracker.eco.init_samplef_H = init_samplef_H;
tracker.eco.projection_matrix_init = projection_matrix_init;
tracker.eco.init_samplef_proj = init_samplef_proj;
tracker.eco.init_hf = init_hf;
tracker.eco.fyf = fyf;
tracker.eco.res_norms_temp = res_norms_temp;

% other variables
tracker.eco.hf_full = hf_full;

tracker.eco.bb = double([pos([2,1]) - (target_sz([2,1]) - 1)/2, pos([2,1]) + (target_sz([2,1]) - 1)/2]);
tracker.eco.score = max(max(fftshift(sample_fs(scores_fs(:,:,scale_ind), 10*output_sz))));

if tracker.eco.score > opt.tracking_score_thre
    tracker.eco.is_confident = 1;
else
    tracker.eco.is_confident = 0;
end


%% MEEG time-lock searchlight
%
% This example shows MVPA analyses performed on MEEG data.
%
% The input dataset involved a paradigm where a participant saw
% images of six object categories.
%
%
% The code presented here can be adapted for other MEEG analyses, but
% there please note:
% * the current examples do not perform baseline corrections or signal
%   normalizations, which may reduce discriminatory power.
%
% Note: running this code requires FieldTrip.
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

%% get timelock data in CoSMoMVPA format

% set configuration
config=cosmo_config();
data_path=fullfile(config.tutorial_data_path,'meg_obj6');

% show dataset information
readme_fn=fullfile(data_path,'README');
cosmo_type(readme_fn);

% reset citation list
cosmo_check_external('-tic');

% load preprocessed data
data_fn=fullfile(data_path,'meg_obj6_s00.mat');
data_tl=load(data_fn);

% Show data_tl
% >@@>
cosmo_disp(data_tl);
% <@@<

%%

% convert to cosmomvpa struct, using cosmo_meeg_dataset
% >@@>
ds=cosmo_meeg_dataset(data_tl);
% <@@<

% show the dataset
% >@@>
cosmo_disp(ds);
% <@@<

%%

% set the targets in ds.sa.targets (trial condition)
% Hint: use the first column from ds.sa.trialinfo
% >@@>
ds.sa.targets=ds.sa.trialinfo(:,1); % 6 categories
% <@@<

% set the chunks in ds.sa.chunks (independent measurements)
% all trials are here considered to be independent, so the chunks
% must all have a different value
% >@@>
nsamples=size(ds.samples,1);
ds.sa.chunks=(1:nsamples)';
% <@@<

% in addition give a label to each trial
index2label={'body','car','face','flower','insect','scene'};
ds.sa.labels=cellfun(@(x)index2label(x),num2cell(ds.sa.targets));

% just to check everything is ok
cosmo_check_dataset(ds);

%% Count number of channels, time points and trials
% >@@>
fprintf('There are %d channels, %d time points and %d trials\n',...
        numel(unique(ds.fa.chan)),numel(unique(ds.fa.time)),...
        size(ds.samples,1));
% <@@<
%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Part I: compute difference between faces and scenes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% For each time point and sensor; then visualize the results
% for the magnetometer (meg_axial) sensors.

% slice 'ds' twice to get 'ds_face' and 'ds_scene', each with only trials
% from the face and scene categories

% >@@>
ds_face=cosmo_slice(ds,cosmo_match(ds.sa.labels,'face'));
ds_scene=cosmo_slice(ds,cosmo_match(ds.sa.labels,'scene'));
% <@@<

% prepare dataset for output
ds_faceVSscene=cosmo_slice(ds_face,1);
ds_faceVSscene.sa=struct(); % destroy sample attributes

% Compute difference between average of faces versus average of scenes;
% store the result in the samples field of ds_faceVSscene
% >@@>
ds_faceVSscene.samples=mean(ds_face.samples)-mean(ds_scene.samples);
% <@@<

% Convert ds_faceVSscene to a fieldtrip structure and convert
ft_faceVSscene=cosmo_map2meeg(ds_faceVSscene);
%%
% Use FieldTrip to visualize the face versus house contrast
chantype='meg_axial';
layout=cosmo_meeg_find_layout(ds_faceVSscene,'chantype',chantype);

figure();
cfg=struct();
cfg.interactive='yes';
cfg.zlim=[-1 1];
cfg.layout=layout;

% show figure with plots for each sensor
ft_multiplotER(cfg, ft_faceVSscene);
%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Part 2: run searchlight over time
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% set MVPA parameters
fprintf('The input has feature dimensions %s\n', ...
                cosmo_strjoin(ds.a.fdim.labels,', '));

% only select relevant time period and sensors
sensor_posterior_axial={'MEG1631', 'MEG1641', 'MEG1731', 'MEG1841', ...
                        'MEG1911', 'MEG1921', 'MEG1941', 'MEG2231', ...
                        'MEG2311', 'MEG2321', 'MEG2341', 'MEG2431', ...
                        'MEG2441', 'MEG2511', 'MEG2531'};

% define the mask
msk=cosmo_dim_match(ds,'time',@(t) t>=-.1 & t<=.4,...
                        'chan',sensor_posterior_axial);

% first slice the dataset, then use cosmo_dim_prune to avoid using
% non-selected data. Assign the result to ds_sel
%%%% >>> Your code here <<< %%%%
ds_sel_orig=cosmo_slice(ds,msk,2);
cosmo_disp(ds_sel_orig);

ft_orig=cosmo_map2meeg(ds_sel_orig);
%%
ds_sel=cosmo_dim_prune(ds_sel_orig);
cosmo_disp(ds_sel);
ft_sel=cosmo_map2meeg(ds_sel);

%%

% define the neighborhood for time with a time radius of 2 time points,
% and assign to time_nbrhood.
% Hint: use cosmo_interval_neighborhood,

time_nbrhood=cosmo_interval_neighborhood(ds_sel,'time','radius',2);
cosmo_disp(time_nbrhood)

%%

%%%% >>> Your code here <<< %%%%

% Define the measure to be cosmo_crossvalidation_measure,
% and assign to measure
measure=@cosmo_crossvalidation_measure;

%%%% >>> Your code here <<< %%%%

%nsamples=numel(ds_sel.samples);
%rp=cosmo_randperm(nsamples);
%ds_sel.sa.targets=ds_sel.sa.targets(rp);

partitions=cosmo_independent_samples_partitioner(ds_sel,...
                                    'fold_count',5,...
                                    'test_ratio',0.2);

cosmo_disp(partitions)
%%
% Define the partitioning scheme using
% cosmo_independent_samples_partitioner, and assign to partitions.
% Use 'fold_count',5 to use 5 folds,
% and use 'test_ratio',.2 to use 20% of the data for testing (and 80% for
% training) in each fold.

%%%% >>> Your code here <<< %%%%

% Use the LDA classifier and the partitions just defined,
measure_args=struct();
measure_args.partitions=partitions;
measure_args.classifier=@cosmo_classify_lda;

ds_sl=cosmo_searchlight(ds_sel,time_nbrhood,measure,measure_args);

plot(ds_sl.a.fdim.values{1},ds_sl.samples)
xlabel('time');
ylabel('classification accuracy');

%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Part III: channel-time searchlight
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% select limited time period
msk=cosmo_dim_match(ds,'time',@(t) t>=-.1 & t<=.4);

% first slice the dataset using the mask and assign to 'ds_sel'
ds_sel=cosmo_slice(ds,msk,2);

% Now use cosmo_dim_prune to avoid attempts to using
% non-selected data in the searchlight
ds_sel=cosmo_dim_prune(ds_sel);

% Set the seachlight parameters
chan_count=10; % 10 center channels in each searchlight
time_radius=2; % 2*2+1=5 time bins
chan_type='meg_combined_from_planar';

% define the neighborhood for each dimensions
% First, set 'chan_nbrhood' using cosmo_meeg_chan_neighborhood,
% and use the 'chantype' and 'count' parameters to set the channel type
% and the number of sensors in each searchlight.
chan_nbrhood=cosmo_meeg_chan_neighborhood(ds_sel,'chantype',chan_type,...
                                    'count',chan_count);

cosmo_disp(chan_nbrhood);
%%

%%%% >>> Your code here <<< %%%%

% Second, set 'time_nbrhood' using cosmo_interval_neighborhood,
% using the 'time' dimension

time_nbrhood=cosmo_interval_neighborhood(ds_sel,'time','radius',2);

%%%% >>> Your code here <<< %%%%

% cross neighborhoods for chan-time searchlight
% Hint: use cosmo_cross_neighborhood, and use chan_nbrhood and time_nbrhood
% (in that order) in a cell as the second argument
nbrhood=cosmo_cross_neighborhood(ds_sel,{chan_nbrhood,time_nbrhood});

%%%% >>> Your code here <<< %%%%

% print how many neighbors features have on average
nbrhood_nfeatures=cellfun(@numel,nbrhood.neighbors);
fprintf('Features have on average %.1f +/- %.1f neighbors\n', ...
            mean(nbrhood_nfeatures), std(nbrhood_nfeatures));

%%

% set the 'measure' variable to a function handle to the
% split-half correlation measure
measure=@cosmo_correlation_measure;

% Define the partitioning scheme using
% cosmo_independent_samples_partitioner.
% Use 'fold_count',1 to use 1 folds,
% and use 'test_ratio',.5 to use 50% of the data for testing (and 50% for
% training) in the single fold.
partitions=cosmo_independent_samples_partitioner(ds_sel,'fold_count',1,...
                                            'test_ratio',0.5);

%%%% >>> Your code here <<< %%%%
measure_args=struct();
measure_args.partitions=partitions;

%% run searchlight
% run the searchlight using the parameters above, and assign the result
% to a varibale 'ds_sl'

ds_sl=cosmo_searchlight(ds_sel,nbrhood,measure,measure_args);

%%%% >>> Your code here <<< %%%%

%% visualize timeseries results

% deduce layout from output
layout=cosmo_meeg_find_layout(ds_sl);
fprintf('The output uses layout %s\n', layout.name);

% map ds_sl to a FieldTrip structure. Assign the result to 'sl_ft'
sl_ft=cosmo_map2meeg(ds_sl);

%%%% >>> Your code here <<< %%%%

figure();
cfg = [];
cfg.interactive = 'yes';
cfg.zlim=[-1 1];
cfg.layout       = layout;

% show figure with fisher-transformed correlations for each sensor
ft_multiplotER(cfg, sl_ft);

%% visualize topology results
% show figure with topology for 100 before to 400ms after stimulus onset
% in bins of 50 ms
figure();
cfg.xlim=-0.1:0.05:0.4;
ft_topoplotER(cfg, sl_ft);

%% Show citation information
cosmo_check_external('-cite');


function [OMNISol,bilevelMILPProblem] = OMNI(model, selectedRxnList, options, constrOpt, measOpt, prevSolutions, verbFlag)
%
%% ***********************NOT WORKING**************************************
%

%function [OMNISol,bilevelMILPProblem] = OMNI(model,selectedRxnList,options,constrOpt,prevSolutions,verbFlag,solutionFileNameTmp)

%OMNI Run OMNI in the most general form
%
% OMNI(model,selectedRxnList,options,constrOpt,prevSolutions,verbFlag,solutionFileName)
%
%INPUTS
% model                 Structure containing all necessary variables to
%                       describe a stoichiometric model
%   rxns                  Rxns in the model
%   mets                  Metabolites in the model
%   S                     Stoichiometric matrix (sparse)
%   b                     RHS of Sv = b (usually zeros)
%   c                     Objective coefficients
%   lb                    Lower bounds for fluxes
%   ub                    Upper bounds for fluxes
%   rev                    Reversibility of fluxes
% selectedRxnList       List of reactions that can be knocked-out in OMNI
% options               OMNI options
%   numDel                # of bottlenecks
%   numDelSense           Direction of # of bottleneck constraint (G/E/L)
%   vMax                  Max flux
%   solveOMNI             Solve problem within Matlab
%   createGams            Create GAMS input file
%   gamsFile              GAMS input file name
% constrOpt             Explicitly constrained reaction options
%   rxnList               Reaction list
%   values                Values for constrained reactions
%   sense                 Constraint senses for constrained reactions
%                         (G/E/L)
% measOpt               Measured flux options
%   rxnSel                Names of measured reactions
%   values                Flux values of measured reactions
%   weights               Weights for measured fluxes
%
%OPTIONAL INPUTS
% prevSolutions         Previous solutions
% verbFlag              Verbose flag
% solutionFileName      File name for storing temporary solutions
%
%OUTPUTS
% OMNISol               OMNI solution structure
% bilevelMILPProblem    bi-level MILP problem structure used
%
% Markus Herrgard 3/28/05

% Set these for MILP callbacks
global MILPproblemType;
global selectedRxnIndIrrev;
%global rxnList;
global irrev2rev;
%global solutionFileName;
%global biomassRxnID;
%global OMNIKOrxnList;
%global OMNIObjective;
%global OMNIGrowth;
%global solID;

if (nargin < 5)
    prevSolutions = [];
end
if (nargin < 6)
    verbFlag = false;
end
% if (nargin < 7)
%     solutionFileName = 'OMNISolutions.mat';
% else
%     solutionFileName = solutionFileNameTmp;
% end

% Convert to irreversible rxns
[modelIrrev,matchRev,rev2irrev,irrev2rev] = convertToIrreversible(model);

% Create the index of the previous KO's suggested by OMNI to avoid obtaining the same
% solution again
selPrevSolIrrev = [];
for i = 1:size(prevSolutions,2)
    prevSolRxnList = model.rxns(prevSolutions(:,i)==1);
    selPrevSol = ismember(model.rxns,prevSolRxnList);
    selPrevSolIrrev(:,i) = selPrevSol(irrev2rev);
end

[nMets,nRxns] = size(modelIrrev.S);

% Create matchings for reversible reactions in the set selected for KOs
% This is to ensure that both directions of the reaction are knocked out
selSelectedRxn = ismember(model.rxns,selectedRxnList);
selSelectedRxnIrrev = selSelectedRxn(irrev2rev);
selectedRxnIndIrrev = find(selSelectedRxnIrrev);
cnt = 0;
%prevRxnID = -10;
nSelected = length(selectedRxnIndIrrev);
selRxnCnt = 1;
while selRxnCnt <= nSelected
    rxnID = selectedRxnIndIrrev(selRxnCnt);
    if (matchRev(rxnID)>0)
        cnt = cnt + 1;
        selectedRxnMatch(cnt,1) = selRxnCnt;
        selectedRxnMatch(cnt,2) = selRxnCnt+1;
        selRxnCnt = selRxnCnt + 1;
    end
    selRxnCnt = selRxnCnt + 1;
end

% Set inner constraints for the LP
constrOptIrrev = setConstraintsIrrevModel(constrOpt,model,modelIrrev,rev2irrev);
% constrOptIrrev = model;
% constrOptIrrev = [];

% Set objectives for linear and integer parts
cLinear = zeros(nRxns,1);
cInteger = zeros(sum(selSelectedRxnIrrev),1);

% Set the correct objective coefficient (not necessary for OMNI)
% targetRxnID = find(ismember(model.rxns,options.targetRxn));
% targetRxnIDirrev = rev2irrev{targetRxnID}(1);
% cLinear(targetRxnIDirrev) = 1;

% Set measured reaction in objective
sel_meas_rxn = measOpt.rxnSel';
b_meas_rxn = measOpt.values';
wt_meas_rxn = measOpt.weights';
n_m = length(sel_meas_rxn);

% Create selection vector in the decoupled representation
% This is to ensure that the objective function for measured reversible
% reactions is constructed correctly
sel_m = zeros(nRxns,1);
ord_ir = [];
b_meas_tmp = [];
wt_meas_tmp = [];
for i = 1:n_m
    rxn_name = sel_meas_rxn{i};
    rxn_id = find(strcmp(model.rxns,rxn_name));
    if (~isempty(rxn_id)) % Protect against measured fluxes that are not part of the model
        b_meas_tmp = [b_meas_tmp;b_meas_rxn(i)];
        wt_meas_tmp = [wt_meas_tmp;wt_meas_rxn(i)];
        % Reversible rxns
        if (model.rev(rxn_id))
            rxn_id_ir = rev2irrev{rxn_id}(1);
            sel_m(rxn_id_ir) = 1;
            sel_m(rxn_id_ir+1) = -1;
        else
            % Irrev rxns
            rxn_id_ir = rev2irrev{rxn_id};
            sel_m(rxn_id_ir) = 1;
        end
        % Figure out ordering in decoupled representation
        ord_ir = [ord_ir rxn_id_ir];
    end
end
% Get ordering indices
[tmp,ord_ind] = sort(ord_ir);
% Reorder or create weights
if (sum(wt_meas_rxn) == 0)
    measOpts.weights = ones(n_m,1);
else
    measOpts.weights = wt_meas_tmp(ord_ind);
end
% Reorder measured flux values
measOpts.values = b_meas_tmp(ord_ind);

measOpts.rxnSel = sel_m;

% Create the constraint matrices for the bilevel MILP
bilevelMILPProblem = createBilevelMILPproblem(modelIrrev,cLinear,cInteger,selSelectedRxnIrrev,...
    selectedRxnMatch,constrOptIrrev,measOpts,options,selPrevSolIrrev);

% Initial guess (random)
%bilevelMILPProblem.x0 = round(rand(length(bilevelMILPProblem.c),1));
if isfield(options,'initSolution')
    if (length(options.initSolution) > options.numDel | ~all(ismember(options.initSolution,selectedRxnList)))
        warning('Initial solution not valid - starting from a random initial solution')
        bilevelMILPProblem.x0 = [];
    else
        % Set initial integer solution
        selInitRxn = ismember(model.rxns,options.initSolution);
        selInitRxnIrrev = selInitRxn(irrev2rev);
        initRxnIndIrrev = find(selInitRxnIrrev);
        initIntegerSol = ~ismember(selectedRxnIndIrrev,initRxnIndIrrev);
        selInteger = bilevelMILPProblem.vartype == 'B';
        [nConstr,nVar] = size(bilevelMILPProblem.A);
        bilevelMILPProblem.x0 = nan(nVar,1);
        bilevelMILPProblem.x0(selInteger) = initIntegerSol;

%         LPproblem.b = bilevelMILPProblem.b - bilevelMILPProblem.A(:,selInteger)*initIntegerSol;
%         LPproblem.A = bilevelMILPProblem.A(:,bilevelMILPProblem.vartype == 'C');
%         LPproblem.c = bilevelMILPProblem.c(bilevelMILPProblem.vartype == 'C');
%         LPproblem.lb = bilevelMILPProblem.lb(bilevelMILPProblem.vartype == 'C');
%         LPproblem.ub = bilevelMILPProblem.ub(bilevelMILPProblem.vartype == 'C');
%         LPproblem.osense = -1;
%         LPproblem.csense = bilevelMILPProblem.csense;
%         LPsol = solveCobraLP(LPproblem);
%
%         bilevelMILPProblem.x0(~selInteger) = LPsol.full;
    end
else
    bilevelMILPProblem.x0 = [];
end

% Minimize
bilevelMILPProblem.osense = 1;

if (verbFlag)
    [nConstr,nVar] = size(bilevelMILPProblem.A);
    nInt = length(bilevelMILPProblem.intSolInd);
    fprintf('MILP problem with %d constraints %d integer variables and %d continuous variables\n',...
        nConstr,nInt,nVar);
end

bilevelMILPProblem.model = modelIrrev;

% Set these for CPLEX callbacks
MILPproblemType = 'OMNI';
% rxnList = model.rxns;
% biomassRxnID = find(modelIrrev.c==1);
% solID = 0;
% OMNIObjective = [];
% OMNIGrowth = [];
% OMNIKOrxnList = {};

% Solve problem
if (options.solveOMNI)
    OMNISol = solveCobraMILP(bilevelMILPProblem,'printLevel',0);
    if OMNISol.stat~=0
        if (~isempty(OMNISol.cont))
            OMNISol.fluxes = convertIrrevFluxDistribution(OMNISol.cont(1:length(matchRev)),matchRev);
        end
        if (~isempty(OMNISol.int))
            % Figure out the KO reactions
            OMNIRxnInd = selectedRxnIndIrrev(OMNISol.int < 1e-4);
            OMNISol.kos = model.rxns(unique(irrev2rev(OMNIRxnInd)));

%             %sanity check
%             modelTemp = changeRxnBounds(model,OMNISol.kos,0,'b');
%             solTemp = optimizeCbModel(modelTemp);
%             if abs(solTemp.f - OMNISol.obj) > 1e-4
%                 [OMNISol,bilevelMILPProblem] = OMNI(model, selectedRxnList, options, constrOpt, measOpt, prevSolutions, verbFlag);
%                 previous_solutions(:,end+1) = zeros(length(model.rxns),1);
%             end
        end
    else
        OMNISol.fluxes=[];
        OMNISol.kos={};
    end
else
    OMNISol.rxnList = {};
    OMNISol.fluxes = [];
end


function gradx = singletrack_gradx_f(in1,in2,in3)
%SINGLETRACK_GRADX_F
%    GRADX = SINGLETRACK_GRADX_F(IN1,IN2,IN3)

%    This function was generated by the Symbolic Math Toolbox version 8.3.
%    09-Jan-2020 11:52:33

I_z = in3(2,:);
M = in3(1,:);
T = in2(2,:);
V_vx = in1(4,:);
V_vy = in1(5,:);
c_f = in3(8,:);
c_r = in3(9,:);
delta = in2(1,:);
deltamax = in3(5,:);
l_f = in3(3,:);
l_r = in3(4,:);
maxbrakeWForce = in3(6,:);
psi_dot = in1(6,:);
vpsi = in1(3,:);
t2 = cos(vpsi);
t3 = sin(vpsi);
t4 = l_f.*psi_dot;
t5 = l_r.*psi_dot;
t6 = V_vx.^2;
t7 = 1.0./I_z;
t8 = 1.0./M;
t9 = T.*5.0e+1;
t10 = delta.*5.0e+1;
t11 = deltamax.*5.0e+1;
t14 = V_vx.*1.0e+2;
t12 = -t5;
t13 = -t9;
t15 = -t10;
t16 = -t11;
t17 = V_vy+t4;
t18 = tanh(t14);
t19 = t17.^2;
t20 = V_vy+t12;
t21 = t13+5.0e+1;
t22 = t18.^2;
t23 = t13-5.0e+1;
t27 = t11+t15;
t31 = t15+t16;
t24 = t20.^2;
t25 = exp(t21);
t26 = exp(t23);
t28 = exp(t27);
t29 = t6+t19;
t32 = exp(t31);
t34 = t22.*1.0e+2;
t30 = t25+1.0;
t33 = t26+1.0;
t35 = t6+t24;
t36 = t28+1.0;
t38 = t32+1.0;
t39 = 1.0./t29;
t44 = t34-1.0e+2;
t37 = 1.0./t30;
t40 = 1.0./t33;
t41 = 1.0./t36;
t42 = 1.0./t35;
t43 = 1.0./t38;
t45 = t37.*5.0e+1;
t46 = t37-1.0;
t47 = t40.*5.0e+1;
t49 = deltamax.*t41;
t50 = t41-1.0;
t52 = V_vx.*c_r.*l_r.*t42;
t53 = t43-1.0;
t48 = -t45;
t51 = -t47;
t54 = deltamax.*t53;
t55 = T.*t40.*t46;
t57 = t9.*t40.*t46;
t58 = delta.*t43.*t50;
t56 = -t55;
t59 = -t58;
t61 = t48+t51+t57+5.0e+1;
t60 = t40+t46+t56;
t62 = exp(t61);
t64 = t49+t54+t59;
t63 = t62+1.0;
t65 = sin(t64);
t67 = cos(t64);
t66 = 1.0./t63;
t69 = V_vx.*c_f.*l_f.*t39.*t67;
t68 = t66-1.0;
gradx = reshape([0.0,0.0,-V_vx.*t3-V_vy.*t2,t2,-t3,0.0,0.0,0.0,0.0,V_vx.*t2-V_vy.*t3,t3,t2,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,t8.*(-c_f.*t17.*t39.*t65+(maxbrakeWForce.*t44.*t60.*t68)./2.0+(maxbrakeWForce.*t44.*t60.*t67.*t68)./2.0),t8.*(psi_dot+V_vx.*c_f.*t39.*t65),t8.*(V_vy+V_vx.*c_f.*l_f.*t39.*t65),0.0,0.0,0.0,0.0,t8.*(c_r.*t20.*t42+c_f.*t17.*t39.*t67+(maxbrakeWForce.*t44.*t60.*t65.*t68)./2.0),-t8.*(psi_dot+V_vx.*c_r.*t42+V_vx.*c_f.*t39.*t67),-t8.*(V_vy-t52+t69),0.0,0.0,0.0,0.0,t7.*(-c_r.*l_r.*t20.*t42+c_f.*l_f.*t17.*t39.*t67+(l_f.*maxbrakeWForce.*t44.*t60.*t65.*t68)./2.0),t7.*(t52-t69),-t7.*(l_f.*t69+l_r.*t52),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[7,7]);


function [K] = ku1u0(x, y, xp, yp, hyp, ubarp, vbarp, dt, i)

logsigma = hyp(1);
logthetax = hyp(2);
logthetay = hyp(3);

a1 = hyp(4);
a2 = hyp(5);

n_x = size(x,1);
n_y = size(y,1);
n_xp = size(xp,1);
n_yp = size(yp,1);

x = repmat(x,1,n_xp);
y = repmat(y,1,n_yp);
xp = repmat(xp',n_x,1);
yp = repmat(yp',n_y,1);

ubarp = repmat(ubarp',n_x,1);
vbarp = repmat(vbarp',n_y,1);

switch i

case 0

K=exp(1).^(logsigma+(-4).*logthetay+(-1/2).*exp(1).^((-1).*logthetax).*(x+ ...
  (-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1).*yp).^2).*(exp( ...
  1).^(2.*logthetay).*(exp(1).^logthetay+(-1).*(y+(-1).*yp).^2)+a1.*dt.* ...
  exp(1).^((-1).*logthetax+logthetay).*(exp(1).^(2.*logthetay).*ubarp.*(x+ ...
  (-1).*xp)+3.*exp(1).^(logthetax+logthetay).*vbarp.*(y+(-1).*yp)+(-1).* ...
  exp(1).^logthetay.*ubarp.*(x+(-1).*xp).*(y+(-1).*yp).^2+(-1).*exp(1) ...
  .^logthetax.*vbarp.*(y+(-1).*yp).^3)+dt.*exp(1).^(a2+(-2).*logthetax).*( ...
  3.*exp(1).^(2.*(logthetax+logthetay))+exp(1).^(logthetax+3.*logthetay)+( ...
  -1).*exp(1).^(3.*logthetay).*(x+(-1).*xp).^2+(-6).*exp(1).^(2.* ...
  logthetax+logthetay).*(y+(-1).*yp).^2+(-1).*exp(1).^(logthetax+2.* ...
  logthetay).*(y+(-1).*yp).^2+exp(1).^(2.*logthetay).*(x+(-1).*xp).^2.*(y+ ...
  (-1).*yp).^2+exp(1).^(2.*logthetax).*(y+(-1).*yp).^4));

case 1 % logsigma

K=exp(1).^(logsigma+(-4).*logthetay+(-1/2).*exp(1).^((-1).*logthetax).*(x+ ...
  (-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1).*yp).^2).*(exp( ...
  1).^(2.*logthetay).*(exp(1).^logthetay+(-1).*(y+(-1).*yp).^2)+a1.*dt.* ...
  exp(1).^((-1).*logthetax+logthetay).*(exp(1).^(2.*logthetay).*ubarp.*(x+ ...
  (-1).*xp)+3.*exp(1).^(logthetax+logthetay).*vbarp.*(y+(-1).*yp)+(-1).* ...
  exp(1).^logthetay.*ubarp.*(x+(-1).*xp).*(y+(-1).*yp).^2+(-1).*exp(1) ...
  .^logthetax.*vbarp.*(y+(-1).*yp).^3)+dt.*exp(1).^(a2+(-2).*logthetax).*( ...
  3.*exp(1).^(2.*(logthetax+logthetay))+exp(1).^(logthetax+3.*logthetay)+( ...
  -1).*exp(1).^(3.*logthetay).*(x+(-1).*xp).^2+(-6).*exp(1).^(2.* ...
  logthetax+logthetay).*(y+(-1).*yp).^2+(-1).*exp(1).^(logthetax+2.* ...
  logthetay).*(y+(-1).*yp).^2+exp(1).^(2.*logthetay).*(x+(-1).*xp).^2.*(y+ ...
  (-1).*yp).^2+exp(1).^(2.*logthetax).*(y+(-1).*yp).^4));

case 2 % logthetax

K=(1/2).*exp(1).^(logsigma+(-4).*logthetay+(-1/2).*exp(1).^((-1).* ...
  logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1).* ...
  yp).^2).*(exp(1).^((-1).*logthetax).*(x+(-1).*xp).^2.*(exp(1).^(2.* ...
  logthetay).*(exp(1).^logthetay+(-1).*(y+(-1).*yp).^2)+a1.*dt.*exp(1).^(( ...
  -1).*logthetax+logthetay).*(exp(1).^(2.*logthetay).*ubarp.*(x+(-1).*xp)+ ...
  3.*exp(1).^(logthetax+logthetay).*vbarp.*(y+(-1).*yp)+(-1).*exp(1) ...
  .^logthetay.*ubarp.*(x+(-1).*xp).*(y+(-1).*yp).^2+(-1).*exp(1) ...
  .^logthetax.*vbarp.*(y+(-1).*yp).^3)+dt.*exp(1).^(a2+(-2).*logthetax).*( ...
  3.*exp(1).^(2.*(logthetax+logthetay))+exp(1).^(logthetax+3.*logthetay)+( ...
  -1).*exp(1).^(3.*logthetay).*(x+(-1).*xp).^2+(-6).*exp(1).^(2.* ...
  logthetax+logthetay).*(y+(-1).*yp).^2+(-1).*exp(1).^(logthetax+2.* ...
  logthetay).*(y+(-1).*yp).^2+exp(1).^(2.*logthetay).*(x+(-1).*xp).^2.*(y+ ...
  (-1).*yp).^2+exp(1).^(2.*logthetax).*(y+(-1).*yp).^4))+(-2).*dt.*exp(1) ...
  .^((-2).*logthetax+2.*logthetay).*(exp(1).^(a2+logthetax)+a1.*exp(1) ...
  .^logthetax.*ubarp.*(x+(-1).*xp)+(-2).*exp(1).^a2.*(x+(-1).*xp).^2).*( ...
  exp(1).^logthetay+(-1).*(y+(-1).*yp).^2));

case 3 % logthetay

K=exp(1).^(logsigma+(-4).*logthetay+(-1/2).*exp(1).^((-1).*logthetax).*(x+ ...
  (-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1).*yp).^2).*(exp( ...
  1).^(3.*logthetay)+2.*exp(1).^(2.*logthetay).*(exp(1).^logthetay+(-1).*( ...
  y+(-1).*yp).^2)+(exp(1).^(2.*logthetay).*(exp(1).^logthetay+(-1).*(y+( ...
  -1).*yp).^2)+a1.*dt.*exp(1).^((-1).*logthetax+logthetay).*(exp(1).^(2.* ...
  logthetay).*ubarp.*(x+(-1).*xp)+3.*exp(1).^(logthetax+logthetay).* ...
  vbarp.*(y+(-1).*yp)+(-1).*exp(1).^logthetay.*ubarp.*(x+(-1).*xp).*(y+( ...
  -1).*yp).^2+(-1).*exp(1).^logthetax.*vbarp.*(y+(-1).*yp).^3)+dt.*exp(1) ...
  .^(a2+(-2).*logthetax).*(3.*exp(1).^(2.*(logthetax+logthetay))+exp(1).^( ...
  logthetax+3.*logthetay)+(-1).*exp(1).^(3.*logthetay).*(x+(-1).*xp).^2+( ...
  -6).*exp(1).^(2.*logthetax+logthetay).*(y+(-1).*yp).^2+(-1).*exp(1).^( ...
  logthetax+2.*logthetay).*(y+(-1).*yp).^2+exp(1).^(2.*logthetay).*(x+(-1) ...
  .*xp).^2.*(y+(-1).*yp).^2+exp(1).^(2.*logthetax).*(y+(-1).*yp).^4)).*(( ...
  -4)+(1/2).*exp(1).^((-1).*logthetay).*(y+(-1).*yp).^2)+a1.*dt.*exp(1).^( ...
  (-1).*logthetax+2.*logthetay).*(2.*exp(1).^logthetay.*ubarp.*(x+(-1).* ...
  xp)+3.*exp(1).^logthetax.*vbarp.*(y+(-1).*yp)+(-1).*ubarp.*(x+(-1).*xp) ...
  .*(y+(-1).*yp).^2)+dt.*exp(1).^(a2+(-2).*logthetax+logthetay).*(6.*exp( ...
  1).^(2.*logthetax+logthetay)+3.*exp(1).^(logthetax+2.*logthetay)+(-3).* ...
  exp(1).^(2.*logthetay).*(x+(-1).*xp).^2+(-6).*exp(1).^(2.*logthetax).*( ...
  y+(-1).*yp).^2+(-2).*exp(1).^(logthetax+logthetay).*(y+(-1).*yp).^2+2.* ...
  exp(1).^logthetay.*(x+(-1).*xp).^2.*(y+(-1).*yp).^2)+a1.*dt.*exp(1).^(( ...
  -1).*logthetax+logthetay).*(exp(1).^(2.*logthetay).*ubarp.*(x+(-1).*xp)+ ...
  3.*exp(1).^(logthetax+logthetay).*vbarp.*(y+(-1).*yp)+(-1).*exp(1) ...
  .^logthetay.*ubarp.*(x+(-1).*xp).*(y+(-1).*yp).^2+(-1).*exp(1) ...
  .^logthetax.*vbarp.*(y+(-1).*yp).^3));

case 4 % a1

K=dt.*exp(1).^(logsigma+(-1).*logthetax+(-3).*logthetay+(-1/2).*exp(1).^(( ...
  -1).*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+( ...
  -1).*yp).^2).*(exp(1).^(2.*logthetay).*ubarp.*(x+(-1).*xp)+3.*exp(1).^( ...
  logthetax+logthetay).*vbarp.*(y+(-1).*yp)+(-1).*exp(1).^logthetay.* ...
  ubarp.*(x+(-1).*xp).*(y+(-1).*yp).^2+(-1).*exp(1).^logthetax.*vbarp.*(y+ ...
  (-1).*yp).^3);

case 5 % a2

K=dt.*exp(1).^(a2+logsigma+(-2).*logthetax+(-4).*logthetay+(-1/2).*exp(1) ...
  .^((-1).*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).* ...
  (y+(-1).*yp).^2).*(3.*exp(1).^(2.*(logthetax+logthetay))+exp(1).^( ...
  logthetax+3.*logthetay)+(-1).*exp(1).^(3.*logthetay).*(x+(-1).*xp).^2+( ...
  -6).*exp(1).^(2.*logthetax+logthetay).*(y+(-1).*yp).^2+(-1).*exp(1).^( ...
  logthetax+2.*logthetay).*(y+(-1).*yp).^2+exp(1).^(2.*logthetay).*(x+(-1) ...
  .*xp).^2.*(y+(-1).*yp).^2+exp(1).^(2.*logthetax).*(y+(-1).*yp).^4);

otherwise

        K = zeros(n_x, n_xp);
end

end