% 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


function [view,area3d] = measureFlatROIAreaMesh(view, grayThickness)
%
% [view,area3d] = measureFlatROIAreaMesh(view, [grayThickness])
%
% Measures the area on the 3D cortical surface of the current
% flat-map ROI.
%
%    1. Select ROI in the Flat window.
%
%    2. Use the Area Measure pull down in the Analysis pulldown of
% the Flat window.
%
% NOTE: this code must find the flat.mat file associated with the
% unfold, load it, and find the 'unfoldMeshSummary' field. This extra
% information is saved from within newer versions of mrFlatMesh when
% you check 'save extra info'.
%
% The method here is much preferred to that in measureFlatROIArea, which
% tries to create it's own triangulation based only on layer 1 nodes. This
% usually works OK for ROIs that are convex, but can severely over-estimate
% the area when the ROI has concavities. Here we just use the triangles
% from the gray/white interface surface mesh.
%
% See Also: measureFlatROIArea, sumTriangularArea
%
% HISTORY:
% 2003.01.07 RFD (bob@white.stanford.edu) wrote it.
% 2003.01.30 RFD: minor cleaning, tested some alternative vertex inclusion
% criteria.
% 2006.04.10 RFD: addes comments about our preference for this function
% over the older measureFlatROIArea.

global mrSESSION;

if(~exist('grayThickness','var') | isempty(grayThickness))
    grayThickness = 3;
end

% If someone want the area returned, then we assume that we are scripted
% and thus don't use gui stuff.
if(nargout<2),  gui = 1;
else,  gui = 0; end

% Get selpts from current ROI
if view.selectedROI,   ROIcoords = getCurROIcoords(view);
else  error('No current ROI'); end
ROIname = view.ROIs(view.selectedROI).name;

if isempty(ROIcoords), error('ROI is empty!'); end

hemi = ROIcoords(3,1);
if(~all(ROIcoords(3,:) == hemi))
    error('ROI spans both hemispheres!');
end

% load the flat.mat file
if(hemi==1)
    hemiName = 'left';
    unfoldFile = view.leftPath;
else
    hemiName  = 'right';
    unfoldFile = view.rightPath;
end
if(~exist(unfoldFile,'file'))
    % Try to fix broken paths
    warning([unfoldFile ' not found- trying to find it...']);
    anatPath = getAnatomyPath(mrSESSION.subject);
    indx = findstr(mrSESSION.subject,unfoldFile);
    if(~isempty(indx))
        unfoldFile = fullfile(fileparts(anatPath), unfoldFile(indx(1):end));
        unfoldFile = strrep(unfoldFile, '\', filesep);
        unfoldFile = strrep(unfoldFile, '/', filesep);
    end
    if(exist(unfoldFile,'file'))
        warning(['Using a similarly-named file (' unfoldFile ')...']);
    else
        disp(unfoldFile);
        [f,p] = myUiGetFile(anatPath, {'*.mat';'*.*'}, ['Select ' hemiName ' unfold file...']);
        unfoldFile = fullfile(p,f);
    end
end

if(~isfield(view,'mesh') | size(view.mesh) < hemi | isempty(view.mesh{hemi}))
    unfold = load(unfoldFile);
    disp(['loaded unfold from ', unfoldFile,'...']);
    if(~isfield(unfold,'unfoldMeshSummary'))
        disp('*************************************************************');
        disp('This flat.mat file does not have an unfoldMeshSummary field!');
        disp('Redo the unfold and tell mrFlatMesh to save the extra info.');
        disp('For now, you can get an area estimate from measureFlatROIArea.');
        error('Missing the unfoldMeshSummary field.');
    end
    view.mesh{hemi} = unfold.unfoldMeshSummary;
end

locs2d = view.mesh{hemi}.locs2d;

% To convert raw glocs2d values to those used in mrLoadRet, we do:
minLocs2d = min(locs2d);
locs2d(:,1) = locs2d(:,1) - minLocs2d(1) + 1;
locs2d(:,2) = locs2d(:,2) - minLocs2d(2) + 1;
locs2d = locs2d';

scaledVertices(:,1) = view.mesh{hemi}.uniqueVertices(:,1) .* view.mesh{hemi}.scaleFactor(1);
scaledVertices(:,2) = view.mesh{hemi}.uniqueVertices(:,2) .* view.mesh{hemi}.scaleFactor(2);
scaledVertices(:,3) = view.mesh{hemi}.uniqueVertices(:,3) .* view.mesh{hemi}.scaleFactor(3);
areaList3d = findFaceArea(view.mesh{hemi}.connectionMatrix, ...
    scaledVertices, ...
    view.mesh{hemi}.uniqueFaceIndexList);
% Lets also measure the 2d area, just for fun.
areaList2d = findFaceArea(view.mesh{hemi}.connectionMatrix, ...
    [locs2d',zeros(size(locs2d,2),1)], ...
    view.mesh{hemi}.uniqueFaceIndexList);

%
% Find the triangles that overlap with the ROI.
%

% First we find all the vertices that fall within the ROI.
%
% Intersect returns only the unique matches, but we want them all.
% So, we use ismember, which tells us for each row of the first matrix if
% it matches any row in the second matrix.
roiVertexIndices = find(ismember(round(locs2d)', ROIcoords(1:2,:)', 'rows'));

% Now we find the triangles (faces) within the ROI by a simple heuristic-
% any one of the triangle's vertices is in the ROI. What we should really
% do is select only those triangles whose AREA falls mostly within the ROI.
% But that's too hard (ie. would be slow in matlab).
roiFaceIndices = find(ismember(view.mesh{hemi}.uniqueFaceIndexList(:,1), roiVertexIndices) ...
    | ismember(view.mesh{hemi}.uniqueFaceIndexList(:,2), roiVertexIndices) ...
    | ismember(view.mesh{hemi}.uniqueFaceIndexList(:,3), roiVertexIndices));
% 2003.01.30 RFD: NOTE: I tried making the above inclusion criteria more
% conservative- so that two of it's vertices needed to be within the ROI.
% However, this wasn't much better, especially for thin ROIs, since it
% erred in the other direction and eliminated too many triangles.
% But that more conservative criteria would have the very desireable
% property that any triangle will only get included in one of
% two abutting ROIs, never in both like the old method sometimes did.
% vertOne = ismember(view.mesh{hemi}.uniqueFaceIndexList(:,1), RoiVertexIndices);
% vertTwo = ismember(view.mesh{hemi}.uniqueFaceIndexList(:,2), RoiVertexIndices);
% vertThree = ismember(view.mesh{hemi}.uniqueFaceIndexList(:,3), RoiVertexIndices);
% roiFaceIndices = find((vertOne & vertTwo) | (vertOne & vertThree) | (vertTwo & vertThree));

area3d = sum(areaList3d(roiFaceIndices));
area2d = sum(areaList2d(roiFaceIndices));

areaStr = addText([],sprintf('%s area = %.0f mm^2 (%d triangles)\n',ROIname,area3d,length(roiFaceIndices)));
areaStr = addText(areaStr,sprintf('2d = %.0f~mm^2 and %.0f voxels',area2d,size(ROIcoords,2)));

if(gui)
    %msgbox([areaStr],[ROIname ' area']);
    % Make the ROI coords point to the upper left of each ROI pixel
    ROIcoords = ROIcoords-0.5;
    % now define four the lines for each ROI coordinate.
    roiLinesX = [[ ROIcoords(2,:);   ROIcoords(2,:)+1 ],...
            [ ROIcoords(2,:);   ROIcoords(2,:)   ],...
            [ ROIcoords(2,:)+1; ROIcoords(2,:)+1 ],...
            [ ROIcoords(2,:)+1; ROIcoords(2,:)   ]];
    roiLinesY =-[[ ROIcoords(1,:);   ROIcoords(1,:)   ],...
            [ ROIcoords(1,:);   ROIcoords(1,:)+1 ],...
            [ ROIcoords(1,:)+1; ROIcoords(1,:)   ],...
            [ ROIcoords(1,:)+1; ROIcoords(1,:)+1 ]];

    roiFaces = view.mesh{hemi}.uniqueFaceIndexList(roiFaceIndices,:);
    % This will color each triangle with the z-depth of the surface
    % (something like curvature).
    %colors = scaledVertices(roiFaces(:,1),3) + scaledVertices(roiFaces(:,2),3) + scaledVertices(roiFaces(:,3),3);
    %colors = colors-mean(colors);
    % Use abs to cope with the fact that we sometimes get negative numbers here...
    colors = abs(log(areaList3d(roiFaceIndices)./areaList2d(roiFaceIndices)));
    %areaStr = [areaStr ' (Color shows log 3D/2D area)'];

    figure;  hold on; axis equal; colormap(hot);
    patch([locs2d(2,roiFaces(:,1));...
            locs2d(2,roiFaces(:,2));...
            locs2d(2,roiFaces(:,3))], ...
        -[locs2d(1,roiFaces(:,1));...
            locs2d(1,roiFaces(:,2));...
            locs2d(1,roiFaces(:,3))], colors');
    lineH = line(roiLinesX, roiLinesY,'Color','blue');
    colorbar;
    %plot(locs2d(2,roiVertexIndices), -locs2d(1,roiVertexIndices), '.r');
    %plot(round(locs2d(2,roiVertexIndices)), -round(locs2dInt(1,roiVertexIndices)), '.g');
    hold off;
    title(areaStr);
end

return;


function [TW, TPW, TR, TPR] = ft_realtime_benchmark(target)

% FT_REALTIME_BENCHMARK times the reading and writing of data
%
% Use as
%   ft_realtime_benchmark(target)
% where target is the location of the buffer or the file, for
% example 'buffer://localhost:1972'
%
% Please note that any data in the target will be overwritten.

% Copyright (C) 2010, Stefan Klanke
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

N = 100;

hdr        = [];
hdr.Fs     = 1000; % this is not really used here
hdr.nChans = 64;

fprintf(1,'Writing header and flushing data in %s...\n', target);
ft_write_data(target, single([]), 'header', hdr, 'append', false);

blocklen = [8, 16, 32, 64, 128, 256, 512, 1024, 2048];
blocksize = hdr.nChans * blocklen;

TW  = zeros(N, length(blocklen));	% Times for writing
TPW = zeros(1, length(blocklen));	% Throughput writing
TR  = zeros(N, length(blocklen));	% Times for reading
TPR = zeros(1, length(blocklen));	% Throughput reading

for k=1:length(blocklen);
  fprintf(1,'\nDetermining WRITE throughput at %ix%i samples per block...\n', hdr.nChans, blocklen(k));
  % generate random data for writing
  X = single(randn(hdr.nChans, blocklen(k)));
  for n=1:N;
    tic;
    ft_write_data(target, X, 'header', hdr, 'append', true);
    TW(n,k) = toc;
  end
  mt = mean(TW(:,k));
  st = std(TW(:,k));
  ss = blocksize(k) * N / sum(TW(:,k));
  fprintf(1,'Time per block %f +/- %f  => %d samples/sec\n', mt, st, ss);
  TPW(k) = ss;

  hdr = ft_read_header(target);

  fprintf(1,'\nDetermining READ throughput at %ix%i samples per block...\n', hdr.nChans, blocklen(k));
  begS = 1;
  for n=1:N;
    endS = begS+blocklen(k)-1;
    tic;
    X = ft_read_data(target, 'header', hdr, 'begsample', begS, 'endsample', endS);
    TR(n,k) = toc;
    begS = endS+1;
  end
  mt = mean(TR(:,k));
  st = std(TR(:,k));
  ss = blocksize(k) * N / sum(TR(:,k));
  fprintf(1,'Time per block %f +/- %f  => %d samples/sec\n', mt, st, ss);

  TPR(k) = ss;
end

loglog(blocksize, TPW,'r+-', blocksize, TPR, 'b+-');
legend('Writing','Reading','Location','NorthWest');
xlabel('block size');
ylabel('samples per sec');


function h = rxVisualizeRx(rx, loc);
%
% h = rxVisualizeRx(rx, [loc]);
%
% Display a visualization of the current prescription in a separate
% figure. Shows a montage of interpolated slices, and, if the
% Rx Figure is also open, adds a panel with sagittal, coronal, and
% axial views with the prescription on it.
%
% 'loc' is an optional argument for visualizing the rx, specifying
% the [axi cor sag] slices to show in the Rx views.
%
% ras, 01/2007.
if notDefined('rx'), rx = get(findobj('Tag','rxControlFig'), 'UserData'); end
if ishandle(rx),	rx = get(rx, 'UserData');				end

h = figure('Color', 'w');

nSlices = rx.rxDims(3);
for slice = 1:nSlices
	[img2D images{slice}] = rxInterpSlice(rx, slice);
end

montage = imageMontage(images);
imagesc(montage); axis image; axis off;
title('Prescribed Slices');

%% draw ROIs if selected
if ~isempty(rx.rois) & get(rx.ui.rxDrawRx, 'Value')==1
	for r = 1:length(rx.rois)
		% convert coords from vol -> rx
		rxCoords = vol2rx(rx, rx.rois(r).volCoords, 1);

		% now convert these coords to 2-D image coordinates
		pts = rxCoords2Montage(rxCoords, nSlices, size(img2D));

        if isempty(pts), continue; end

        % remove pts outside range
        outside = find(pts(1,:) < 1 | pts(1,:) > size(montage, 1) ...
                       | pts(2,:) < 1 | pts(2,:) > size(montage, 2));
        ok = setdiff(1:size(pts, 2), outside);
        pts = pts(:,ok);

		% draw the outline of this ROI
		% (rx.rois(r) doubles as a prefs struct, to set the color)
		outline(pts, rx.rois(r));
	end
end

%% add panel with views of the Rx on each plane
if ishandle(rx.ui.rxAxes)
	if notDefined('loc'),   loc = round( rx.volDims / 2 );  end
	if ischar(loc),         loc = str2num(loc);             end

	%% open a panel to the right of the main figure w/ the Rxs
	hp = mrvPanel('right', .3);

	%% put up the prescriptions
	for ori = 1:3
		vol = rx.vol;
		volSlice = uint8(loc(ori));

		%% orient the slice properly
		if ori~=3   % do nothing for sagittal view
			% allow for radiological L/R
			hRadiological = findobj('Tag','rxRadiologicalMenu');
			if isequal(get(hRadiological, 'Checked'), 'on')
				vol = flipdim(vol, 3);
			end

			% permute volume as needed
			if ori==1, vol = permute(vol,[2 3 1]); end   % axial
			if ori==2, vol = permute(vol,[1 3 2]); end   % coronal
		end

		%% plot the slice
		% get slice
		volImg = vol(:,:,volSlice);
		volImg = rxClip(volImg, [], rx.ui.volBright, rx.ui.volContrast);

		% make subplot
		pos = [0 1-(ori/3) 1 .3];
		hax(ori) = axes('Parent', hp, 'Units', 'norm', 'Position', pos);

		% put up image
		htmp = image(volImg); colormap gray; axis off; axis equal;

		% draw Rx, label directions
		rxDrawRx(rx, volSlice, ori);  hold on
		rxLabelVolAxes(rx, ori);

		%% draw ROIs on prescription, if selected
		if ~isempty(rx.rois) & get(rx.ui.rxDrawRx, 'Value')==1
			for r = 1:length(rx.rois)
				R  = rx.rois(r);  % ROI struct
				C  = R.volCoords; % coords (being terse b/c of indents)

				% map the [axi cor sag] positions in C to the
				% 2D [x y] positions in pts:
				inSlice = find( round(C(ori,:)) == volSlice );
				switch ori
					case 1, % axi
						pts = C([2 3],inSlice);
					case 2, % cor
						pts = C([1 3],inSlice);
					case 3, % sag
						pts = C([1 2],inSlice);
				end

				% Draw the ROI outline
				if ~isempty(pts)
					outline(pts, R);
				end
			end
		end

	end
end

return
% /------------------------------------------------------------/ %

% /------------------------------------------------------------/ %
function pts = rxCoords2Montage(coords, nSlices, dims);
% convert 3-D volume coordinates into 2-D points on a montage
% of prescription slices.  This assumes that imageMontage has
% been called on all slices (1:nSlices), with the default #
% of columns and rows.
ncols = ceil( sqrt( nSlices ) );
nrows = ceil( nSlices / ncols );

% initialize pts output:
pts = [];

for slice = 1:nSlices
	% find (row, col) of this slice in the montage
	row = ceil(slice / ncols);
	col = mod(slice-1, ncols) + 1;

	% find columns of coords in this slice
	I = find( round(coords(3,:)) == slice );

	% main part: compute (x,y) locations given the montage offset
	y = coords(1,I) + (row-1) * dims(1);
	x = coords(2,I) + (col-1) * dims(2);

	% add to points list:
	% we will shuffle the order of coordinates in coords,
	% but remove points outside the Rx.
	pts = [pts [y; x]];
end

return


function newimg = GPReduce2(img,blocksize,displayflag)

if ~exist('displayflag')
	displayflag = 1;
end

dim = size(img);

newimg = [];
for b = 1:blocksize:dim(3)
    if b<dim(3)
        img2 = img(:,:,b:b+blocksize-1);
    else
        img2 = img(:,:,b);
    end
	newimg2 = GPReduce(img2,displayflag);
	newimg = cat(3,newimg,newimg2);
end


% The COBRAToolbox: testReadSBML.m
%
% Purpose:
%     - reads all the sbml files in this folder and
%       checks if all the parameters are correctly written
%     - loads certain xml files, runs and FBA, and compares the
%       solution to pre-calculated values from FBAs run with .mat files
%
% Authors:
%     - Partial original file: Joseph Kang 04/07/09
%     - CI integration: Laurent Heirendt
%
% Note:
%     - The solver libraries must be included separately

global CBTDIR

%Check the requirements (a LP solver is necessary)
solvers = prepareTest('needsLP',true,'requireOneSolverOf',{'gurobi','ibm_cplex','glpk','mosek','quadMinos'});

% save the current path
currentDir = pwd;

% initialize the test
fileDir = fileparts(which('testReadSBML'));
cd(fileDir);

%Check if the model contains the same information

% initialize the test
cd([CBTDIR, filesep, 'test', filesep, 'models']);

% load the models
modelArr = { 'Abiotrophia_defectiva_ATCC_49176.xml', 'STM_v1.0.xml', 'iIT341.xml'};

%loop through the models
for i = 1:length(modelArr)
    %reading the models takes quite a bit of time, so only do it once for
    %all solvers.
    % output a line before launching the test for model i
    fprintf('   Testing %s ...\n', modelArr{i});

    % load the model (actually supply the full filename of the path
    % where the model is found)
    model = getDistributedModel(modelArr{i});
    for k = 1:length(solvers.LP)
        fprintf(' -- Running testReadSBML using the solver interface: %s ... ', solvers.LP{k});
        solverOK = changeCobraSolver(solvers.LP{k}, 'LP', 0);

        fprintf('   Testing loaded model ... \n');

        % set the tolerance
        tol = 1e-6;

        % define the maximum objective values calculated from pre-converted .mat files
        modelFBAf_max = [0.149475406282249; 0.477833660760744; 0.692812693473487];

        % define the minimum objective values
        modelFBAf_min = [0.0; 0.0; 0.0];

        % solve the maximisation problem
        FBA = optimizeCbModel(model, 'max');

        % test the maximisation solution
        assert(FBA.stat == 1);
        assert(abs(FBA.f - modelFBAf_max(i)) < tol);
        assert(norm(model.S * FBA.x) < tol);

        % solve the minimisation problem
        FBA = optimizeCbModel(model, 'min');

        % test the minimisation solution
        assert(FBA.stat == 1);
        assert(abs(FBA.f - modelFBAf_min(i)) < tol);
        assert(norm(model.S * FBA.x) < tol);

        % print a line for success of loop i
        fprintf(' Done.\n');

        % test that gene rules are generated correctly
        % needs testing on model with large number of genes, i.e.,
        % 'Abiotrophia_defectiva_ATCC_49176.xml'
        if strcmp(modelArr{i}, 'Abiotrophia_defectiva_ATCC_49176.xml')
            % test that rules are correctly generated, i.e. no gene partially matched
            % no indication of x(1)23 instead of x(123).
            assert(~any(~cellfun(@isempty, regexp(model.rules, '\(\d+\)\d+')))) %incorrect
        end
    end
end

% test reading COBRA models with symbols in objective reactions and multiple objective reactions
for jTest = 1:2
    if jTest == 1
        % test objective reactions with symbols
        fprintf('   Testing readSBML for models with symbols in objective reactions ...\n');
        model = createModel({'EX_a(e)'; 'EX_b(e)'}, {'Test A'; 'Test B'}, {'a[e] <=>'; 'b[e] <=>'});
        model.c = [1; 0];

    elseif jTest == 2
        % test more than one objective reactions with >1 objective reactions
        fprintf('   Testing readSBML for models with >1 objective reactions ...\n');
        model = createModel({'EX_a'; 'EX_b'}, {'Test A'; 'Test B'}, {'a[e] <=>'; 'b[e] <=>'});
        model.c = [1; -2];
    end

    model.lb = model.lb(:);
    model.ub = model.ub(:);
    model.comps = {'e'};
    model.compNames = {'ExtraCellular'};
    % add the fields outputted by readSBML
    [model.modelVersion.SBML_level, model.modelVersion.SBML_version, model.modelVersion.fbc_version] = deal(3, 1, 2);
    model.metCharges = zeros(numel(model.mets), 1);
    model.metFormulas = {'C'; 'C'};
    model.osense = -1;
    model.description = 'test_sbml_obj.xml';
    model.genes = {'gene1'; 'gene2'};
    model.rules = {''; ''};
    model.rxnGeneMat = zeros(2, 2);

    model = convertOldStyleModel(model);

    % write the model
    writeCbModel(model, 'sbml', 'test_sbml_obj');

    % read in the model
    model2 = readCbModel('test_sbml_obj.xml');
    %We are creating a few default fields in readCbModel.
    DefaultFields = {'S','b','csense','lb','ub','c','osense','rxns','mets','genes','rules','subSystems'};

    model2 = convertOldStyleModel(model2);

    modelFields = fieldnames(model);
    modelFields = setdiff(modelFields,'rxnGeneMat'); %rxnGeneMat did not contain any information and is not created by readCbModel.
    for i = 1:numel(modelFields)
        if iscell(model.(modelFields{i}))
            if ~all(cellfun(@isempty, model.(modelFields{i})))  || any(ismember(modelFields{i},DefaultFields))
                assert(isequal(model.(modelFields{i}),model2.(modelFields{i})));
            else
                assert(~isfield(model2, modelFields{i}));
            end
        else
            if isnumeric(model.(modelFields{i}))
                if ~all(isnan(model.(modelFields{i})(:))) || any(ismember(modelFields{i},DefaultFields))
                    assert(isequal(model.(modelFields{i})(~isnan(model.(modelFields{i}))),model2.(modelFields{i})(~isnan(model.(modelFields{i})))));
                    assert(isequal(~isnan(model.(modelFields{i})),~isnan(model2.(modelFields{i}))));
                else
                    assert(~isfield(model2, modelFields{i}));
                end
            else
                %Not numeric, i.e. another field.
                assert(isequal(model.(modelFields{i}),model2.(modelFields{i})));
            end
        end
    end
    fprintf(' Done.\n\n');
end
delete('test_sbml_obj.xml')

% change the directory
cd(currentDir)


%CODEGENERATOR.GENFKINE Generate code for forward kinematics
%
% T = cGen.genfkine() generates a symbolic homogeneous transform matrix (4x4) representing
% the pose of the robot end-effector in terms of the symbolic joint coordinates q1, q2, ...
%
% [T, ALLT] = cGen.genfkine() as above but also generates symbolic homogeneous transform
% matrices (4x4xN) for the poses of the individual robot joints.
%
% Notes::
% - Side effects of execution depends on the cGen flags:
%   - saveresult: the symbolic expressions are saved to
%     disk in the directory specified by cGen.sympath
%   - genmfun: ready to use m-functions are generated and
%     provided via a subclass of SerialLink stored in cGen.robjpath
%   - genslblock: a Simulink block is generated and stored in a
%     robot specific block library cGen.slib in the directory
%     cGen.basepath
%   - genccode: generates C-functions and -headers in the directory
%     specified by the ccodepath property of the CodeGenerator object.
%   - mex: generates robot specific MEX-functions as replacement for the
%     m-functions mentioned above. Access is provided by the SerialLink
%     subclass. The MEX files rely on the C code generated before.
%
% Author::
%  Joern Malzahn, (joern.malzahn@tu-dortmund.de)
%
% See also CodeGenerator.CodeGenerator, CodeGenerator.geninvdyn, CodeGenerator.genjacobian.

% Copyright (C) 2012-2014, by Joern Malzahn
%
% This file is part of The Robotics Toolbox for Matlab (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com
%
% The code generation module emerged during the work on a project funded by
% the German Research Foundation (DFG, BE1569/7-1). The authors gratefully
% acknowledge the financial support.

function [t,allT] = genfkine(CGen)

%% Derivation of symbolic expressions
CGen.logmsg([datestr(now),'\tDeriving forward kinematics']);

q = CGen.rob.gencoords;
[t, allT] = CGen.rob.fkine(q);

CGen.logmsg('\t%s\n',' done!');

%% Save symbolic expressions
if CGen.saveresult
    CGen.logmsg([datestr(now),'\tSaving symbolic forward kinematics up to end-effector frame']);

    CGen.savesym(t,'fkine','fkine.mat')

    CGen.logmsg('\t%s\n',' done!');

    CGen.logmsg([datestr(now),'\tSaving symbolic forward kinematics for joint']);

    for iJoint = 1:CGen.rob.n
        CGen.logmsg(' %s ',num2str(iJoint));
        tName = ['T0_',num2str(iJoint)];
        eval([tName,' = allT(',num2str(iJoint),');']);
        CGen.savesym(eval(tName),tName,[tName,'.mat']);
    end

    CGen.logmsg('\t%s\n',' done!');
end

%% M-Functions
if CGen.genmfun
    CGen.genmfunfkine;
end

%% Embedded Matlab Function Simulink blocks
if CGen.genslblock
    genslblockfkine(CGen);
end

%% C-Code
if CGen.genccode
    CGen.genccodefkine;
end

%% MEX
if CGen.genmex
    CGen.genmexfkine;
end


function packFonts()
% run extra/genfont.sh first

fonts = dir(fullfile('extra','fonts')) ;
fonts = {fonts([fonts.isdir]).name} ;
fonts(ismember(fonts,{'.','..'})) = [] ;
chars = 'a':'z' ;

im = cell(numel(fonts), numel(chars)) ;
labels = cell(numel(fonts), numel(chars)) ;

for i = 1:numel(fonts)
  for j = 1:numel(chars)
    [p,cmap] = imread(fullfile('extra', 'fonts', fonts{i}, [chars(j) '.png'])) ;
    if isempty(cmap)
      im{i,j} = im2single(p) ;
    else
      im{i,j} = im2single(ind2gray(p,cmap)) ;
    end
    labels{i,j} = j ;
  end
end

imdb.meta.classes = chars ;
imdb.meta.sets = {'train', 'val'} ;
imdb.meta.fonts = fonts ;
imdb.images.id = 1:numel(im) ;
imdb.images.data = cat(3, im{:}) ;
imdb.images.label = cat(2, labels{:}) ;

% Create training and validation sets
sets = ones(numel(fonts), numel(chars)) ;
sets(end-349:end,:) = 2 ;
imdb.images.set = sets(:)' ;

save('data/charsdb.mat', '-struct', 'imdb') ;


function val = get(c,prop_name)

   % GET - Get correlation properties
   %
   % VAL = get(C,PROP_NAME)
   % See HELP CORRELATION for description of primary property names:
   %   Waveforms, Trig, Corr, Lag, Stat, Link, Clust
   %
   % Additional scalar properties:
   %   Traces:         number of traces
   %   Data_Length:    number of samples in each trace
   %   Fs:             Frequency
   %   Period:         Period
   %   Nyq:            Nyquist frequency
   %   **NOTE - The scalar properties listed above exist for each waveform
   %   (except Traces). However, the correlation object requires them to be
   %   the same. GET only reads the value associated with the first waveform
   %   Normally this will not be an issue. If the correlation object
   %   was created manually it is possible for these fields to be different.
   %
   % Additional vector properties (nx1):
   %   Start, Trig, End:             same as Start_Matlab, End_Matlab
   %   Start_Str, Trig_Str, End_Str:           String times
   %   Start_Matlab, Trig_Matlab, End_Matlab:  Matlab-format times
   %   Start_Epoch, Trig_Epoch, End_Epoch:     Epoch-format times
   %
   % Additional vector properties (nx1 cell vector)
   %   Sta (or Station):       Cell vector of station names
   %   Chan (or Component):    Cell vector of channel names
   %
   % Additional matrix properties (nxm matrix)
   %   Data:                    Matrix of raw trace data
   %                           (n traces) x (m samples)

   % Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
   % $Date$
   % $Revision$

   warning('using NewCorrelation/get, alternate ways exist of accessing data.')

   narginchk(2,inf)

   % got rid of test for c

   switch upper(prop_name)
      case {'WAVEFORMS', 'WAVEFORM', 'WAVES'}
         val = c.W;
      case {'TRIG'}
         val = c.trig;
      case {'TRIG_STR'}
         val = cellstr(datestr(c.trig,'dd-mmm-yyyy HH:MM:SS.FFF'));
      case {'TRIG_MATLAB'}
         val = c.trig;
      case {'CORR'}
         val = c.corrmatrix;
      case {'LAG'}
         val = c.lags;
      case {'STAT'}
         val = c.stat;
      case {'LINK'}
         val = c.link;
      case {'CLUST'}
         val = c.clust;
      case {'TRACES'}
         val = c.ntraces;

         % FROM WAVEFORM/GET (SCALAR OUTPUT)
      case {'DATA_LENGTH'}
         val = c.traces(1).nsamples();
      case {'FS'}
         val = c.traces(1).samplerate;

         %     % FROM WAVEFORM/GET (VECTOR)
      case {'STATION', 'STA'}
         val = c.stations;
      case {'COMPONENT', 'CHAN'}
         val = c.channels;

         % OTHER ROUTINES
      case {'DATA'}
         val = double(c.traces);

      otherwise
         try
            % see if it is a valid waveform property
            warning('accessing a waveform property %s. ', prop_name);
            val = get(c.W,prop_name);
         catch er
            error('''%s'' is not a valid argument for correlation/get\n%s\n', prop_name, er.message);
         end
   end;
end

%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% CHECKVALS
% This function checks to see if each waveform has the same parameters

function checkvals(vals)

   same = all(vals(:) == vals(1));

   if ~same
      % Warning is disabled because it gets called repeatedly (annoyingly)
      % from within other scripts
      % warning('Waveforms have different frequencies or different numbers of samples. Consider VERIFY function');
   end;
end


function str = get_structure( name )
%STRUCTURE Summary of this function goes here
%
% Structure used in this toolbox.
%
% individual structure:
% parameter: the parameter space point of the individual. it's a column-wise
% vector.
% objective: the objective space point of the individual. it's column-wise
% vector. It only have value after evaluate function is called upon the
% individual.
% estimation: Also a structure array of the individual. It's not used in
% MOEA/D but used in MOEA/D/GP. For every objective, the field contains the
% estimation from the GP model.
%
% estimation structure:
% obj: the estimated mean.
% std: the estimated standard deviation for the mean.
%
% subproblem structure:
% weight: the decomposition weight for the subproblem.
% optimal: the current optimal value of the current structure.
% curpoiont: the current individual of the subproblem.
% optpoint: the point that gain the optimal on the subproblem.
%

switch name
    case 'individual'
        str = struct('parameter',[],'objective'[],'estimation'[]);
    case 'subproblem'
        str = struct('weight',[],'optimal',[],'curpoint',[],'optpoint',[]);
    case 'estimation'
        str = struct();
    otherwise
end


function [elec_realigned] = ft_electroderealign(cfg, elec_original)

% FT_ELECTRODEREALIGN rotates, translates, scales and warps electrode positions. The
% default is to only rotate and translate, i.e. to do a rigid body transformation in
% which only the coordinate system is changed. With the right settings if can apply
% additional deformations to the input sensors (e.g. scale them to better fit the
% skin surface). The different methods are described in detail below.
%
% INTERACTIVE - You can display the skin surface together with the electrode or
% gradiometer positions, and manually (using the graphical user interface) adjust the
% rotation, translation and scaling parameters, so that the electrodes correspond
% with the skin.
%
% FIDUCIAL - You can apply a rigid body realignment based on three fiducial
% locations. After realigning, the fiducials in the input electrode set (typically
% nose, left and right ear) are along the same axes as the fiducials in the template
% electrode set.
%
% TEMPLATE - You can apply a spatial transformation/deformation that automatically
% minimizes the distance between the electrodes or gradiometers and a template or
% sensor array. The warping methods use a non-linear search to minimize the distance
% between the input sensor positions and the corresponding template sensors.
%
% HEADSHAPE - You can apply a spatial transformation/deformation that automatically
% minimizes the distance between the electrodes and the head surface. The warping
% methods use a non-linear search to minimize the distance between the input sensor
% positions and the projection of the electrodes on the head surface.
%
% PROJECT - This projects all electrodes to the nearest point on the
% head surface mesh.
%
% MOVEINWARD - This moves all electrodes inward according to their normals
%
% Use as
%   [elec_realigned] = ft_sensorrealign(cfg)
% with the electrode or gradiometer details in the configuration, or as
%   [elec_realigned] = ft_sensorrealign(cfg, elec_orig)
% with the electrode or gradiometer definition as 2nd input argument.
%
% The configuration can contain the following options
%   cfg.method         = string representing the method for aligning or placing the electrodes
%                        'interactive'     realign manually using a graphical user interface
%                        'fiducial'        realign using three fiducials (e.g. NAS, LPA and RPA)
%                        'template'        realign the electrodes to match a template set
%                        'headshape'       realign the electrodes to fit the head surface
%                        'project'         projects electrodes onto the head surface
%                        'moveinward'      moves electrodes inward along their normals
%   cfg.warp          = string describing the spatial transformation for the template and headshape methods
%                        'rigidbody'       apply a rigid-body warp (default)
%                        'globalrescale'   apply a rigid-body warp with global rescaling
%                        'traditional'     apply a rigid-body warp with individual axes rescaling
%                        'nonlin1'         apply a 1st order non-linear warp
%                        'nonlin2'         apply a 2nd order non-linear warp
%                        'nonlin3'         apply a 3rd order non-linear warp
%                        'nonlin4'         apply a 4th order non-linear warp
%                        'nonlin5'         apply a 5th order non-linear warp
%                        'dykstra2012'     back-project ECoG onto the cortex using energy minimzation
%                        'hermes2010'      back-project ECoG onto the cortex along the local norm vector
%                        'fsaverage'       surface-based realignment with FreeSurfer fsaverage brain (left->left or right->right)
%                        'fsaverage_sym'   surface-based realignment with FreeSurfer fsaverage_sym left hemisphere (left->left or right->left)
%                        'fsinflated'      surface-based realignment with FreeSurfer individual subject inflated brain (left->left or right->right)
%   cfg.channel        = Nx1 cell-array with selection of channels (default = 'all'),
%                        see  FT_CHANNELSELECTION for details
%   cfg.keepchannel    = string, 'yes' or 'no' (default = 'no')
%   cfg.fiducial       = cell-array with the name of three fiducials used for
%                        realigning (default = {'nasion', 'lpa', 'rpa'})
%   cfg.casesensitive  = 'yes' or 'no', determines whether string comparisons
%                        between electrode labels are case sensitive (default = 'yes')
%   cfg.feedback       = 'yes' or 'no' (default = 'no')
%
% The electrode positions can be present in the 2nd input argument or can be specified as
%   cfg.elec          = structure with electrode positions or filename, see FT_READ_SENS
%
% If you want to realign the EEG electrodes using anatomical fiducials, you should
% specify the target location of the three fiducials, e.g.
%   cfg.target.pos(1,:) = [110 0 0]     % location of the nose
%   cfg.target.pos(2,:) = [0  90 0]     % location of the left ear
%   cfg.target.pos(3,:) = [0 -90 0]     % location of the right ear
%   cfg.target.label    = {'NAS', 'LPA', 'RPA'}
%
% If you want to align EEG electrodes to a single or multiple template electrode sets
% (which will be averaged), you should specify the template electrode sets either as
% electrode structures (i.e. when they are already read in memory) or their file
% names using
%   cfg.target          = single electrode set that serves as standard
% or
%   cfg.target{1..N}    = list of electrode sets that will be averaged
%
% If you want to align EEG electrodes to the head surface, you should specify the head surface as
%   cfg.headshape      = a filename containing headshape, a structure containing a
%                        single triangulated boundary, or a Nx3 matrix with surface
%                        points
%
% If you want to align ECoG electrodes to the pial surface, you first need to compute
% the cortex hull with FT_PREPARE_MESH. Then use either the algorithm described in
% Dykstra et al. (2012, Neuroimage) or in Hermes et al. (2010, J Neurosci methods) to
% snap the electrodes back to the cortical hull, e.g.
%   cfg.method         = 'headshape'
%   cfg.warp           = 'dykstra2012', or 'hermes2010'
%   cfg.headshape      = a filename containing headshape, a structure containing a
%                        single triangulated boundary, or a Nx3 matrix with surface
%                        points
%   cfg.feedback       = 'yes' or 'no' (default), feedback of the iteration procedure
%
% Additional configuration options for cfg.warp = 'dykstra2012'
%   cfg.maxiter        = number (default: 50), maximum number of optimization iterations
%   cfg.pairmethod     = 'pos' (default) or 'label', the method for electrode
%                        pairing on which the deformation energy is based
%   cfg.isodistance    = 'yes', 'no' (default) or number, to enforce isotropic
%                        inter-electrode distances (pairmethod 'label' only)
%   cfg.deformweight   = number (default: 1), weight of deformation relative
%                        to shift energy cost (lower increases grid flexibility)
%
% If you want to move the electrodes inward, you should specify
%   cfg.moveinward     = number, the distance that the electrode should be moved
%                        inward (negative numbers result in an outward move)
%
% If you want to align ECoG electrodes to the freesurfer average brain, you should
% specify the path to your headshape (e.g., lh.pial), and ensure you have the
% corresponding registration file (e.g., lh.sphere.reg) in the same directory.
% Moreover, the path to the local freesurfer home is required. Note that, because the
% electrodes are being aligned to the fsaverage brain, the corresponding brain should
% be also used when plotting the data, i.e. use freesurfer/subjects/fsaverage/surf/lh.pial
% rather than surface_pial_left.mat
%   cfg.method         = 'headshape'
%   cfg.warp           = 'fsaverage'
%   cfg.headshape      = string, filename containing subject headshape (e.g. <path to freesurfer/surf/lh.pial>)
%   cfg.fshome         = string, path to freesurfer
%
% See also FT_READ_SENS, FT_VOLUMEREALIGN, FT_INTERACTIVEREALIGN,
% FT_DETERMINE_COORDSYS, FT_PREPARE_MESH

% Copyright (C) 2005-2019, Robert Oostenveld, Arjen Stolk
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% the interactive method uses a global variable to get the data from the figure when it is closed
global norm

% these are used by the ft_preamble/ft_postamble function and scripts
ft_revision = '$Id$';
ft_nargin   = nargin;
ft_nargout  = nargout;

% do the general setup of the function
ft_defaults
ft_preamble init
ft_preamble debug
ft_preamble loadvar    elec_original
ft_preamble provenance elec_original
ft_preamble trackconfig

% the ft_abort variable is set to true or false in ft_preamble_init
if ft_abort
  return
end

% check if the input cfg is valid for this function
cfg = ft_checkconfig(cfg, 'renamed',    {'template', 'target'});
cfg = ft_checkconfig(cfg, 'renamedval', {'method', 'realignfiducials', 'fiducial'});
cfg = ft_checkconfig(cfg, 'renamedval', {'method', 'realignfiducial',  'fiducial'});
cfg = ft_checkconfig(cfg, 'renamedval', {'warp', 'homogenous', 'rigidbody'});
cfg = ft_checkconfig(cfg, 'renamedval', {'warp', 'homogeneous', 'rigidbody'});
cfg = ft_checkconfig(cfg, 'forbidden', 'outline');

% set the defaults
cfg.warp          = ft_getopt(cfg, 'warp', 'rigidbody');
cfg.channel       = ft_getopt(cfg, 'channel',  'all');
cfg.keepchannel   = ft_getopt(cfg, 'keepchannel', 'no');
cfg.feedback      = ft_getopt(cfg, 'feedback', 'no');
cfg.casesensitive = ft_getopt(cfg, 'casesensitive', 'no');
cfg.headshape     = ft_getopt(cfg, 'headshape', []);     % for triangulated head surface, without labels
cfg.target        = ft_getopt(cfg, 'target',  []);       % for electrodes or fiducials, always with labels
cfg.coordsys      = ft_getopt(cfg, 'coordsys');          % this allows for automatic template fiducial placement

if isempty(cfg.target)
  % remove the field, otherwise ft_checkconfig will complain
  cfg = rmfield(cfg, 'target');
end

if ~isempty(cfg.coordsys) && isempty(cfg.target)
  % set the template fiducial locations according to the coordinate system
  switch lower(cfg.coordsys)
    case 'ctf'
      cfg.target = [];
      cfg.target.coordsys = 'ctf';
      cfg.target.pos(1,:) = [100  0 0];
      cfg.target.pos(2,:) = [0   80 0];
      cfg.target.pos(3,:) = [0  -80 0];
      cfg.target.label{1} = 'NAS';
      cfg.target.label{2} = 'LPA';
      cfg.target.label{3} = 'RPA';
    otherwise
      ft_error('the %s coordinate system is not automatically supported, please specify fiducial details in cfg.target')
  end
end

% ensure that the right cfg options have been set corresponding to the method
switch cfg.method
  case 'template'        % realign the sensors to match a template set
    cfg = ft_checkconfig(cfg, 'required', 'target', 'forbidden', 'headshape');
  case 'headshape'     % realign the sensors to fit the head surface
    cfg = ft_checkconfig(cfg, 'required', 'headshape', 'forbidden', 'target');
  case 'fiducial'        % realign using the NAS, LPA and RPA fiducials
    cfg = ft_checkconfig(cfg, 'required', 'target', 'forbidden', 'headshape');
  case 'moveinward'      % moves eletrodes inward
    cfg = ft_checkconfig(cfg, 'required', 'moveinward');
end % switch cfg.method

if strcmp(cfg.method, 'fiducial') && isfield(cfg, 'warp') && ~isequal(cfg.warp, 'rigidbody')
  ft_warning('The method ''fiducial'' implies a rigid body tramsformation. See also http://bugzilla.fieldtriptoolbox.org/show_bug.cgi?id=1722');
  cfg.warp = 'rigidbody';
end

if isfield(cfg, 'headshape') && isa(cfg.headshape, 'config')
  % convert the nested config-object back into a normal structure
  cfg.headshape = struct(cfg.headshape);
end

if isfield(cfg, 'target') && isa(cfg.target, 'config')
  % convert the nested config-object back into a normal structure
  cfg.target = struct(cfg.target);
end

% the data can be passed as input arguments or can be read from disk
hasdata = exist('elec_original', 'var');

% get the electrode definition that should be warped
if ~hasdata
  elec_original = ft_fetch_sens(cfg);
else
  % the input electrodes were specified as second input argument
  % or read from cfg.inputfile
end

% ensure that the units are specified
elec_original = ft_determine_units(elec_original);

% ensure up-to-date sensor description (Oct 2011)
elec_original = ft_datatype_sens(elec_original);

% ensure that channel and electrode positions are the same
assert(isequaln(elec_original.elecpos, elec_original.chanpos), 'this function requires same electrode and channel positions');

% remember the original electrode locations and labels and do all the work with a
% temporary copy, this involves channel selection and changing to lower case
elec = elec_original;

% instead of working with all sensors, only work with the fiducials
% this is useful for gradiometer structures
if strcmp(cfg.method, 'fiducial') && isfield(elec, 'fid')
  fprintf('using the fiducials instead of the sensor positions\n');
  elec.fid.unit = elec.unit;
  elec          = elec.fid;
end

usetarget    = isfield(cfg, 'target')    && ~isempty(cfg.target);
useheadshape = isfield(cfg, 'headshape') && ~isempty(cfg.headshape);

if usetarget
  % get the template electrode definitions
  if ~iscell(cfg.target)
    cfg.target = {cfg.target};
  end
  Ntemplate = length(cfg.target);
  for i=1:Ntemplate
    if isstruct(cfg.target{i})
      target(i) = cfg.target{i};
    else
      target(i) = ft_read_sens(cfg.target{i}, 'senstype', 'eeg');
    end
  end
  clear tmp
  for i=1:Ntemplate
    % ensure up-to-date sensor description
    % ensure that the units are consistent with the electrodes
    tmp(i) = ft_convert_units(ft_datatype_sens(target(i)), elec.unit);
  end
  target = tmp;
end

if useheadshape
  % get the surface describing the head shape
  [headshape.pos, headshape.tri] = headsurface([], [], 'headshape', cfg.headshape);

  % ensure that the units are consistent with the electrodes
  headshape = ft_convert_units(headshape, elec.unit);
end

% convert all labels to lower case for string comparisons
cfg.channel = ft_channelselection(cfg.channel, elec.label);
if strcmp(cfg.casesensitive, 'no')
  elec.label  = lower(elec.label);
  cfg.channel = lower(cfg.channel);
  if usetarget
    for j=1:length(target)
      for i=1:length(target(j).label)
        target(j).label{i} = lower(target(j).label{i});
      end
    end
  end
end
[cfgsel, datsel] = match_str(cfg.channel, elec.label);
% keep the original channel labels
label_original = elec_original.label(datsel);

% start with an empty structure, this will be returned at the end
norm = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if strcmp(cfg.method, 'template')
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % determine electrode selection and overlapping subset for warping
  cfg.channel = ft_channelselection(cfg.channel, elec.label);
  for i=1:Ntemplate
    cfg.channel = ft_channelselection(cfg.channel, target(i).label);
  end

  % make consistent subselection of electrodes
  [cfgsel, datsel] = match_str(cfg.channel, elec.label);
  elec.label = elec.label(datsel);
  elec.elecpos   = elec.elecpos(datsel,:);
  for i=1:Ntemplate
    [cfgsel, datsel] = match_str(cfg.channel, target(i).label);
    target(i).label   = target(i).label(datsel);
    target(i).elecpos = target(i).elecpos(datsel,:);
  end

  % compute the average of the target electrode positions
  average = ft_average_sens(target);

  fprintf('warping electrodes to average template... '); % the newline comes later
  [norm.elecpos, norm.m] = ft_warp_optim(elec.elecpos, average.elecpos, cfg.warp);
  norm.label = elec.label;

  dpre  = mean(sqrt(sum((average.elecpos - elec.elecpos).^2, 2)));
  dpost = mean(sqrt(sum((average.elecpos - norm.elecpos).^2, 2)));
  fprintf('mean distance prior to warping %f, after warping %f\n', dpre, dpost);

  if strcmp(cfg.feedback, 'yes')
    % create an empty figure, continued below...
    figure
    axis equal
    axis vis3d
    hold on
    xlabel('x')
    ylabel('y')
    zlabel('z')

    % plot all electrodes before warping
    ft_plot_sens(elec, 'r*');

    % plot all electrodes after warping
    ft_plot_sens(norm, 'm.', 'label', 'label');

    % plot the template electrode locations
    ft_plot_sens(average, 'b.');

    % plot lines connecting the input and the realigned electrode locations with the template locations
    my_line3(elec.elecpos, average.elecpos, 'color', 'r');
    my_line3(norm.elecpos, average.elecpos, 'color', 'm');
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif strcmp(cfg.method, 'headshape')
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % determine electrode selection and overlapping subset for warping
  cfg.channel = ft_channelselection(cfg.channel, elec.label);
  [cfgsel, datsel] = match_str(cfg.channel, elec.label);
  elec.label   = elec.label(datsel);
  elec.elecpos = elec.elecpos(datsel,:);

  norm.label = elec.label;
  if strcmp(cfg.warp, 'dykstra2012')
    norm.elecpos = warp_dykstra2012(cfg, elec, headshape);
  elseif strcmp(cfg.warp, 'hermes2010')
    norm.elecpos = warp_hermes2010(cfg, elec, headshape);
  elseif strcmp(cfg.warp, 'fsaverage')
    norm.elecpos = warp_fsaverage(cfg, elec);
  elseif strcmp(cfg.warp, 'fsaverage_sym')
    norm.elecpos = warp_fsaverage_sym(cfg, elec);
  elseif strcmp(cfg.warp, 'fsinflated')
    norm.elecpos = warp_fsinflated(cfg, elec);
  else
    fprintf('warping electrodes to skin surface... '); % the newline comes later
    [norm.elecpos, norm.m] = ft_warp_optim(elec.elecpos, headshape, cfg.warp);

    dpre  = ft_warp_error([],     elec.elecpos, headshape, cfg.warp);
    dpost = ft_warp_error(norm.m, elec.elecpos, headshape, cfg.warp);
    fprintf('mean distance prior to warping %f, after warping %f\n', dpre, dpost);
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif strcmp(cfg.method, 'fiducial')
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  % the fiducials have to be present in the electrodes and in the template set
  label = intersect(lower(elec.label), lower(target.label));

  if ~isfield(cfg, 'fiducial') || isempty(cfg.fiducial)
    % try to determine the names of the fiducials automatically
    option1 = {'nasion' 'left' 'right'};
    option2 = {'nasion' 'lpa' 'rpa'};
    option3 = {'nz' 'left' 'right'};
    option4 = {'nz' 'lpa' 'rpa'};
    option5 = {'nas' 'left' 'right'};
    option6 = {'nas' 'lpa' 'rpa'};
    if length(match_str(label, option1))==3
      cfg.fiducial = option1;
    elseif length(match_str(label, option2))==3
      cfg.fiducial = option2;
    elseif length(match_str(label, option3))==3
      cfg.fiducial = option3;
    elseif length(match_str(label, option4))==3
      cfg.fiducial = option4;
    elseif length(match_str(label, option5))==3
      cfg.fiducial = option5;
    elseif length(match_str(label, option6))==3
      cfg.fiducial = option6;
    else
      ft_error('could not determine consistent fiducials in the input and the target, please specify cfg.fiducial or cfg.coordsys')
    end
  end
  fprintf('matching fiducials {''%s'', ''%s'', ''%s''}\n', cfg.fiducial{1}, cfg.fiducial{2}, cfg.fiducial{3});

  % determine electrode selection
  cfg.channel = ft_channelselection(cfg.channel, elec.label);
  [cfgsel, datsel] = match_str(cfg.channel, elec.label);
  elec.label     = elec.label(datsel);
  elec.elecpos   = elec.elecpos(datsel,:);

  if length(cfg.fiducial)~=3
    ft_error('you must specify exactly three fiducials');
  end

  % do case-insensitive search for fiducial locations
  nas_indx = match_str(lower(elec.label), lower(cfg.fiducial{1}));
  lpa_indx = match_str(lower(elec.label), lower(cfg.fiducial{2}));
  rpa_indx = match_str(lower(elec.label), lower(cfg.fiducial{3}));
  if length(nas_indx)~=1 || length(lpa_indx)~=1 || length(rpa_indx)~=1
    ft_error('not all fiducials were found in the electrode set');
  end
  elec_nas = elec.elecpos(nas_indx,:);
  elec_lpa = elec.elecpos(lpa_indx,:);
  elec_rpa = elec.elecpos(rpa_indx,:);

  % FIXME change the flow in the remainder
  % if one or more template electrode sets are specified, then align to the average of those
  % if no template is specified, then align so that the fiducials are along the axis

  % find the matching fiducials in the template and average them
  tmpl_nas = nan(Ntemplate,3);
  tmpl_lpa = nan(Ntemplate,3);
  tmpl_rpa = nan(Ntemplate,3);
  for i=1:Ntemplate
    nas_indx = match_str(lower(target(i).label), lower(cfg.fiducial{1}));
    lpa_indx = match_str(lower(target(i).label), lower(cfg.fiducial{2}));
    rpa_indx = match_str(lower(target(i).label), lower(cfg.fiducial{3}));
    if length(nas_indx)~=1 || length(lpa_indx)~=1 || length(rpa_indx)~=1
      ft_error('not all fiducials were found in template %d', i);
    end
    tmpl_nas(i,:) = target(i).elecpos(nas_indx,:);
    tmpl_lpa(i,:) = target(i).elecpos(lpa_indx,:);
    tmpl_rpa(i,:) = target(i).elecpos(rpa_indx,:);
  end
  tmpl_nas = mean(tmpl_nas,1);
  tmpl_lpa = mean(tmpl_lpa,1);
  tmpl_rpa = mean(tmpl_rpa,1);

  % realign both to a common coordinate system
  elec2common  = ft_headcoordinates(elec_nas, elec_lpa, elec_rpa);
  templ2common = ft_headcoordinates(tmpl_nas, tmpl_lpa, tmpl_rpa);

  % compute the combined transform
  norm         = [];
  norm.m       = templ2common \ elec2common;

  % apply the transformation to the fiducials as sanity check
  norm.elecpos(1,:) = ft_warp_apply(norm.m, elec_nas, 'homogeneous');
  norm.elecpos(2,:) = ft_warp_apply(norm.m, elec_lpa, 'homogeneous');
  norm.elecpos(3,:) = ft_warp_apply(norm.m, elec_rpa, 'homogeneous');
  norm.label        = cfg.fiducial;

  nas_indx = match_str(lower(elec.label), lower(cfg.fiducial{1}));
  lpa_indx = match_str(lower(elec.label), lower(cfg.fiducial{2}));
  rpa_indx = match_str(lower(elec.label), lower(cfg.fiducial{3}));
  dpre  = mean(sqrt(sum((elec.elecpos([nas_indx lpa_indx rpa_indx],:) - [tmpl_nas; tmpl_lpa; tmpl_rpa]).^2, 2)));
  nas_indx = match_str(lower(norm.label), lower(cfg.fiducial{1}));
  lpa_indx = match_str(lower(norm.label), lower(cfg.fiducial{2}));
  rpa_indx = match_str(lower(norm.label), lower(cfg.fiducial{3}));
  dpost = mean(sqrt(sum((norm.elecpos([nas_indx lpa_indx rpa_indx],:) - [tmpl_nas; tmpl_lpa; tmpl_rpa]).^2, 2)));
  fprintf('mean distance between fiducials prior to realignment %f, after realignment %f\n', dpre, dpost);

  if strcmp(cfg.feedback, 'yes')
    % create an empty figure, continued below...
    figure
    axis equal
    axis vis3d
    hold on
    xlabel('x')
    ylabel('y')
    zlabel('z')

    % plot the first three electrodes before transformation
    my_plot3(elec.elecpos(1,:), 'r*');
    my_plot3(elec.elecpos(2,:), 'r*');
    my_plot3(elec.elecpos(3,:), 'r*');
    my_text3(elec.elecpos(1,:), elec.label{1}, 'color', 'r');
    my_text3(elec.elecpos(2,:), elec.label{2}, 'color', 'r');
    my_text3(elec.elecpos(3,:), elec.label{3}, 'color', 'r');

    % plot the template fiducials
    my_plot3(tmpl_nas, 'b*');
    my_plot3(tmpl_lpa, 'b*');
    my_plot3(tmpl_rpa, 'b*');
    my_text3(tmpl_nas, ' nas', 'color', 'b');
    my_text3(tmpl_lpa, ' lpa', 'color', 'b');
    my_text3(tmpl_rpa, ' rpa', 'color', 'b');

    % plot all electrodes after transformation
    my_plot3(norm.elecpos, 'm.');
    my_plot3(norm.elecpos(1,:), 'm*');
    my_plot3(norm.elecpos(2,:), 'm*');
    my_plot3(norm.elecpos(3,:), 'm*');
    my_text3(norm.elecpos(1,:), norm.label{1}, 'color', 'm');
    my_text3(norm.elecpos(2,:), norm.label{2}, 'color', 'm');
    my_text3(norm.elecpos(3,:), norm.label{3}, 'color', 'm');
  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif strcmp(cfg.method, 'interactive')
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  tmpcfg = [];
  tmpcfg.individual.elec = elec;
  if isfield(cfg, 'headshape') && ~isempty(cfg.headshape)
    tmpcfg.template.headshape = cfg.headshape;
  end
  if isfield(cfg, 'target') && ~isempty(cfg.target)
    if iscell(cfg.target)
      if numel(cfg.target)>1
        ft_notice('computing the average electrode positions');
        tmpcfg.template.elec = ft_average_sens(cfg.target);
      else
        tmpcfg.template.elec = cfg.target{1};
      end
    elseif isstruct(cfg.target)
      tmpcfg.template.elec = cfg.target;
    end
    tmpcfg.template.elecstyle = {'facecolor', 'blue'};
    ft_info('plotting the target electrodes in blue');
  end

  % use the more generic ft_interactiverealign for the actual work
  tmpcfg = ft_interactiverealign(tmpcfg);
  % only keep the transformation, it will be applied to the electrodes further down
  norm.m = tmpcfg.m;

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif strcmp(cfg.method, 'project')
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % determine electrode selection
  cfg.channel = ft_channelselection(cfg.channel, elec.label);
  [cfgsel, datsel] = match_str(cfg.channel, elec.label);
  elec.label     = elec.label(datsel);
  elec.elecpos   = elec.elecpos(datsel,:);

  norm.label = elec.label;
  [dum, norm.elecpos] = project_elec(elec.elecpos, headshape.pos, headshape.tri);

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif strcmp(cfg.method, 'moveinward')
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % determine electrode selection
  cfg.channel = ft_channelselection(cfg.channel, elec.label);
  [cfgsel, datsel] = match_str(cfg.channel, elec.label);
  elec.label     = elec.label(datsel);
  elec.elecpos   = elec.elecpos(datsel,:);

  norm.label = elec.label;
  norm.elecpos = moveinward(elec.elecpos, cfg.moveinward);

else
  ft_error('unknown method');
end % if method

% apply the spatial transformation to all electrodes, and replace the
% electrode labels by their case-sensitive original values
switch cfg.method
  case {'template', 'headshape'}
    if strcmpi(cfg.warp, 'dykstra2012') || strcmpi(cfg.warp, 'hermes2010') || ...
        strcmpi(cfg.warp, 'fsaverage') || strcmpi(cfg.warp, 'fsaverage_sym') || strcmpi(cfg.warp, 'fsinflated')
      elec_realigned = norm;
      elec_realigned.label = label_original;
    else
      % the transformation is a linear or non-linear warp, i.e. a vector
      try
        % convert the vector with fitted parameters into a 4x4 homogenous transformation
        % apply the transformation to the original complete set of sensors
        elec_realigned = ft_transform_geometry(feval(cfg.warp, norm.m), elec_original);
      catch
        % the previous section will fail for nonlinear transformations
        elec_realigned.label = label_original;
        try
          elec_realigned.elecpos = ft_warp_apply(norm.m, elec_original.elecpos, cfg.warp);
        end % FIXME why is an error here not dealt with?
      end
      % remember the transformation
      elec_realigned.(cfg.warp) = norm.m;
    end

  case  {'fiducial' 'interactive'}
    % the transformation is a 4x4 homogenous matrix
    % apply the transformation to the original complete set of sensors
    elec_realigned = ft_transform_geometry(norm.m, elec_original);
    % remember the transformation
    elec_realigned.homogeneous = norm.m;

  case {'project', 'moveinward'}
    % nothing to be done
    elec_realigned = norm;
    elec_realigned.label = label_original;

  otherwise
    ft_error('unknown method');
end

% the coordinate system is in general not defined after transformation
if isfield(elec_realigned, 'coordsys')
  elec_realigned = rmfield(elec_realigned, 'coordsys');
end

% in some cases the coordinate system matches that of the input target or headshape
switch cfg.method
  case 'template'
    if isfield(target, 'coordsys')
      elec_realigned.coordsys = target.coordsys;
    end
  case 'headshape'
    if isfield(headshape, 'coordsys')
      elec_realigned.coordsys = headshape.coordsys;
    end
    if isfield(elec_original, 'coordsys')
      if strcmp(cfg.warp, 'dykstra2012') || strcmp(cfg.warp, 'hermes2010')  % this warp simply moves the electrodes in the same coordinate space
        elec_realigned.coordsys = elec_original.coordsys;
      elseif strcmp(cfg.warp, 'fsaverage')
        elec_realigned.coordsys = 'fsaverage';
      elseif strcmp(cfg.warp, 'fsaverage_sym')
        elec_realigned.coordsys = 'fsaverage_sym';
      end
    end
  case 'fiducial'
    if isfield(target, 'coordsys')
      elec_realigned.coordsys = target.coordsys;
    end
  case 'interactive'
    % the coordinate system is not known
  case {'project', 'moveinward'}
    % the coordinate system remains the same
    if isfield(elec_original, 'coordsys')
      elec_realigned.coordsys = elec_original.coordsys;
    end
  otherwise
    ft_error('unknown method');
end

if istrue(cfg.keepchannel)
  % append the channels that are not realigned
  [dum, idx] = setdiff(elec_original.label, elec_realigned.label);
  idx = sort(idx);
  elec_realigned.label = [elec_realigned.label; elec_original.label(idx)];
  elec_realigned.elecpos = [elec_realigned.elecpos; elec_original.elecpos(idx,:)];
end

% channel positions are identical to the electrode positions (this was checked at the start)
elec_realigned.chanpos = elec_realigned.elecpos;
elec_realigned.tra = eye(numel(elec_realigned.label));

% copy over unit, chantype, chanunit, and tra information in case this was not already done
if ~isfield(elec_realigned, 'unit') && isfield(elec_original, 'unit')
  elec_realigned.unit = elec_original.unit;
end
if ~isfield(elec_realigned, 'chantype') && isfield(elec_original, 'chantype')
  idx = match_str(elec_original.label, elec_realigned.label);
  elec_realigned.chantype = elec_original.chantype(idx);
end
if ~isfield(elec_realigned, 'chanunit') && isfield(elec_original, 'chanunit')
  elec_realigned.chanunit = elec_original.chanunit;
  idx = match_str(elec_original.label, elec_realigned.label);
  elec_realigned.chanunit = elec_original.chanunit(idx);
end

% update it to the latest version
elec_realigned = ft_datatype_sens(elec_realigned);

% do the general cleanup and bookkeeping at the end of the function
ft_postamble debug
ft_postamble trackconfig
ft_postamble previous   elec_original
ft_postamble provenance elec_realigned
ft_postamble history    elec_realigned

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% some simple SUBFUNCTIONs that facilitate 3D plotting
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = my_plot3(xyz, varargin)
h = plot3(xyz(:,1), xyz(:,2), xyz(:,3), varargin{:});
function h = my_text3(xyz, varargin)
h = text(xyz(:,1), xyz(:,2), xyz(:,3), varargin{:});
function my_line3(xyzB, xyzE, varargin)
for i=1:size(xyzB,1)
  line([xyzB(i,1) xyzE(i,1)], [xyzB(i,2) xyzE(i,2)], [xyzB(i,3) xyzE(i,3)], varargin{:})
end


function im = recomputeImage2(view,clipMode)
%
% im = recomputeImage2(view,clipMode)
%
% Recomputes the image (underlay/anat + overlay) for the
% given view, returning the image. This is different from
% recomputeImage in 3 ways:
%       1) It produces a truecolor image;
%       2) It returns the image rather than the view
%          (I do this because I'm adding the option to
%           work on mosaics of many of these images -- e.g.
%           many inplane slices or flat levels)
%       3) It doesn't take numGrays or numColors as arguments;
%          (since it's true color, they're set to 256 each).
%
%
% djh, sometime in '98
% djh, 2/2001. version 3.0
% ras, 3/2004. An attempt to make the images true-color (3 image planes
% (R,G,B), each ranging from 0-255, to allow for a full 256-value dynamic
% range for both the anatomies and overlays). Will check that this doesn't
% critically slow down updating the screen (with modern processors, this
% seems unlikely).

% Initialize images
anatIm=[];
overlay=[];

% Get cothresh, phWindow, and mapWindow from sliders
cothresh = getCothresh(view);
phWindow = getPhWindow(view);
mapWindow = getMapWindow(view);

% although we accept numGrays and numColors for back-compatibility, now
% that it's truecolor, we'll set these directly:
% numGrays = 64;
% numColors = 64;

numGrays = 256;
numColors = 256;

% Get anatClip from sliders
anatClip = getAnatClip(view);

% Get anatomy image
anatIm = cropCurAnatSlice(view);

% Get overlay
overlay = [];
if ~strcmp(view.ui.displayMode,'anat')
  overlay = cropCurSlice(view,view.ui.displayMode);
end

% Select pixels that satisfy cothresh, phWindow, and mapWindow
pts = [];
if ~isempty(overlay)
  pts = ones(size(overlay));
  curCo=cropCurSlice(view,'co');
  curPh=cropCurSlice(view,'ph');
  curMap=cropCurSlice(view,'map');
  if ~isempty(curCo) & cothresh>0
    ptsCo = curCo > cothresh;
    pts = pts & ptsCo;
  end
  if ~isempty(curPh)
    if diff(phWindow) > 0
      ptsPh = (curPh>=phWindow(1) & curPh<=phWindow(2));
    else
      ptsPh = (curPh>=phWindow(1) | curPh<=phWindow(2));
    end
    pts = pts & ptsPh;
  end
  if strcmp(view.ui.displayMode, 'amp')
    curAmp = cropCurSlice(view, 'amp');
    mnv = min(curAmp(:));
    mxv = max(curAmp(:));
    curMap = (curAmp - mnv) ./ (mxv - mnv);
  end
  if ~isempty(curMap)
    ptsMap = (curMap>=mapWindow(1) & curMap<=mapWindow(2));
    pts = pts & ptsMap;
  end
end

% Rescale anatIm to [1:numGrays], anatClip determines the range
% of anatomy values that gets mapped to the available grayscales.
% If anatClip=[0,1] then there is no clipping and the entire
% range of anatomy values is scaled to the range of available gray
% scales.
minVal = double(min(anatIm(:)));
maxVal = double(max(anatIm(:)));
anatClipMin = min(anatClip)*(maxVal-minVal) + minVal;
anatClipMax = max(anatClip)*(maxVal-minVal) + minVal;
warning off;
anatIm = (rescale2(double(anatIm),[anatClipMin,anatClipMax],[1,numGrays]));
%keyboard

warning backtrace;

% Rescale overlay to [0 numGrays-1]
if ~isempty(overlay)
   if strcmp(clipMode,'auto')
      if ~isempty(find(pts));
         overClipMin = min(overlay(pts));
         overClipMax = max(overlay(pts));
      else
         overClipMin = min(overlay(:));
         overClipMax = max(overlay(:));
      end
   else
      overClipMin = min(clipMode);
      overClipMax = max(clipMode);
   end
   overlay=rescale2(overlay,[overClipMin overClipMax],[0 numGrays-1]);
end

% get, and threshold, the anatomical cmap:
lightRng = [0.6 0.8];
darkRng = [0.2 0.4];
thresh = 0.6;
anatCmap = gray(256);
anatCmap(anatCmap < thresh) = ...
    normalize(anatCmap(anatCmap < thresh),darkRng(1),darkRng(2));
anatCmap(anatCmap >= thresh) = ...
    normalize(anatCmap(anatCmap >= thresh),lightRng(1),lightRng(2));

% % convert into a truecolor RGB image, combining
% % the overlay with the anatomy, if necessary
% % Combine overlay with anatomy image
% im = normalize(repmat(anatIm,[1 1 3]));
%
 if ~isempty(overlay) & ~all(pts==0)
     % for truecolor, we need to get the color map
     % info directly.
     cmapname = eval(['view.ui.' view.ui.displayMode 'Mode.name']);
     cmapname = cmapname(1:end-4); % last 4 chars are 'Cmap'
     cmap = feval(cmapname,0,256);
     cmap = [anatCmap; cmap];
     im = ind2rgb(overlay,cmap);

%     % get a set of R,G,B columns of values for each data point in the
%     % overlay
%     vals = cmap(overlay(pts)+1,:);
%
%     % plug in the overlay into each R/G/B plane of the truecolor image:
%     [indx,indy] = ind2sub(size(anatIm),find(pts));
%     indz = [ones(length(indx),1); 2*ones(length(indx),1); 3*ones(length(indx),1)];
%     indx = repmat(indx,3,1);
%     indy = repmat(indy,3,1);
%     final_ind = sub2ind(size(im),indx,indy,indz);
%    im(final_ind) = vals
else
    im = ind2rgb(anatIm,anatCmap);
end

% 2003.01.10 RFD: the following is no longer necessary- the uint8 data
% can't have any NaNs! Also, it caused problems in matlab versions <6.5.
% 2003.01.23 ARW: But without it, Matlab 6.5 fills in the Nans in the flat map as white.
% Do a version check for now and replace NaNs if >=R13
if (version('-release')>=13)
    indices = find(isnan(im));
    im(indices) = 1;
end

% Finally, set the view.ui.image field
%view.ui.image = uint8(double(im)-1);
view.ui.image = im;

if isempty(overlay)
   view.ui.cbarRange = [];
else
   view.ui.cbarRange = [overClipMin overClipMax];
end

return;


function [DataMat, ChannelMat] = in_data_besa(DataFile)
% IN_DATA_BESA: Read BESA EEG files.
%
% USAGE:  OutputData = in_data_besa( DataFile )
%
% INPUT:
%     - DataFile : Full path to a recordings file.
% OUTPUT:
%     - DataMat : Brainstorm data (recordings) structure

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2012-2015

% Get format
[fPath, fBase, fExt] = bst_fileparts(DataFile);
% Initialize returned structure
DataMat = db_template('DataMat');
DataMat.Comment  = fBase;
DataMat.Device   = 'BESA';
DataMat.DataType = 'recordings';
DataMat.nAvg     = 1;
ChanNames = {};

% Open file
fid = fopen(DataFile, 'r');
if (fid == -1)
    error('Cannot open file.');
end

% Switch according to file format
switch lower(fExt)
    case {'.avr', '.mul'}
        % Read header (first line)
        hdr = fgetl(fid);
        % Split to get all the parameters
        hdr = str_split(hdr, ' =');
        % Multiplexed/averaged files
        if strcmpi(fExt, '.mul')
            nTime     = str2num(hdr{2});
            nChannels = str2num(hdr{4});
            timeStart = str2num(hdr{6}) / 1000;  % Convert to seconds
            timeStep  = str2num(hdr{8}) / 1000;  % Convert to seconds
        else
            nTime     = str2num(hdr{2});
            timeStart = str2num(hdr{4}) / 1000;  % Convert to seconds
            timeStep  = str2num(hdr{6}) / 1000;  % Convert to seconds
            nChannels = 0;
        end
        % Read second line: Either the channel names or the first sensor
        hdr = fgetl(fid);
        % If there are alphabetical characters in the line: sensor names
        if any(ismember(double(hdr), double('ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz()[]/\_@')))
            % Replace spaces in channel names with _ (eg. "TP 1" to "TP_1")
            for iDig = 0:9
                hdr = strrep(hdr, sprintf(' %d', iDig), sprintf('_%d', iDig));
            end
            % Try to get the channel names
            ChanNames = str_split(hdr, ' ');
        % Else: It was the first sensor
        else
            % Restart reading the file at the beginning
            fseek(fid, 0, 'bof');
            hdr = fgetl(fid);
        end
        % Read the recordings in multiplexed/averaged mode
        if strcmpi(fExt, '.mul')
            DataMat.F = fscanf(fid, '%f', [nChannels, nTime]);
        else
            DataMat.F = fscanf(fid, '%f', [nTime, Inf])';
        end

    case {'.mux'}
        % Skip three lines
        hdr = fgetl(fid);
        hdr = fgetl(fid);
        hdr = fgetl(fid);
        % Read the recordings, line by line
        allLines = {};
        while 1
            newLine = fgetl(fid);
            if ~ischar(newLine)
                break;
            end
            allLines{end+1} = str2num(newLine);
        end
        % Concatenate everything
        DataMat.F = cat(1, allLines{:})';

        % Ask for time window
        res = java_dialog('input', {'Start time (in miliseconds):', 'Sampling frequency'}, ...
                                    'Time definition (in Hz)', [], {'0','1000'});
        if isempty(res) || (length(str2num(res{1})) ~= 1) || (length(str2num(res{2})) ~= 1)
            DataMat = [];
        else
            timeStart = str2num(res{1}) / 1000;
            timeStep  = 1 / str2num(res{2});
        end
    otherwise
        error(['Unsupported file extension: ' fExt]);
end
% Close file
fclose(fid);

% Rebuild time vector
DataMat.Time = timeStart + (0:size(DataMat.F,2)-1) .* timeStep;
% No bad channels defined in those files: all good
nChannels = size(DataMat.F,1);
DataMat.ChannelFlag = ones(nChannels, 1);

% Try to build a channel file
if ~isempty(ChanNames) && (length(ChanNames) == nChannels)
    % Default channel structure
    ChannelMat = db_template('channelmat');
    ChannelMat.Comment = 'BESA channels';
    ChannelMat.Channel = repmat(db_template('channeldesc'), [1, nChannels]);
    % For each channel
    for i = 1:nChannels
        if ~isempty(ChanNames{i})
            ChannelMat.Channel(i).Name = ChanNames{i};
        elseif (length(ChannelMat.Channel) > 99)
            ChannelMat.Channel(i).Name = sprintf('E%03d', i);
        else
            ChannelMat.Channel(i).Name = sprintf('E%02d', i);
        end
        ChannelMat.Channel(i).Type    = 'EEG';
        ChannelMat.Channel(i).Loc     = [0; 0; 0];
        ChannelMat.Channel(i).Orient  = [];
        ChannelMat.Channel(i).Weight  = 1;
        ChannelMat.Channel(i).Comment = [];
    end
else
    ChannelMat = [];
end


function [scoreF,score0] = argmax_fit_token(Mfit_init,lib)
% ARGMAX_FIT_TOKEN Fit a parse to an image, with token-level parameters
% only
%
%  Mfit_init : instance of class MotorProgramFit
%
% Output:
%  scoreF : final score
%  score0 : initial score
%

    % set up objective function
    Mfit = Mfit_init.copy();
    fmin = @(theta) myscore(theta,Mfit,lib);

    % Set-up the optimization problem
    [theta0,lb,ub] = model_to_vec_fit_token(Mfit);
    options = optimset('Display','off');
    options = optimset(options,'Display','iter');
    options = optimset(options,'TolFun',1e-4,'Algorithm','active-set');

    % Run the optimization
    score0 = -fmin(theta0);
    try
        thetaF = fmincon(fmin,theta0,[],[],[],[],lb,ub,[],options);
    catch
        fprintf(1,'*Warning*: optimization failed. Error caught.\n');
        thetaF = theta0;
    end
    scoreF = -fmin(thetaF);

    refill(thetaF,Mfit_init);

end

function refill(theta,M)
    vec_to_model_fit_token(theta,M);
end

% fill the MotorProgram with parameters
% and then score
function minscore = myscore(theta,Mfit,lib)
    Qfit = Mfit.copy(); % we don't want to modify the shared MotorProgramFit base
    refill(theta,Qfit);
    ll = scoreMP_fit(Qfit,lib);
    minscore = -ll;
end