function G = slpwmetricgraph(X, varargin)
%SLPWMETRICGRAPH Constructs a graph based on pairwise metrics
%
% $ Syntax $
%   - G = slpwmetricgraph(X, ...)
%   - G = slpwmetricgraph(X, Xt, ...)
%
% $ Arguments $
%   - X:        The sample matrix with each column as a (source) node
%   - Xt:       The sample matrix with each column as a (target) node
%   - G:        The constructed graph
%
% $ Description $
%   - G = slpwmetricgraph(X, ...) constructs a graph based on computation
%     of pairwise metric between vector samples. You can specify the
%     following properties:
%     \*
%     \t   The Properties of Graph Matrix construction           \\
%     \h      name       &      description
%            'sparse'    & whether the target graph G is sparse
%                          (default = true)
%            'valtype'   & The type of values in G: 'logical'|'numeric'
%                          (default = 'numeric')
%                          The value output by evalfunctor should conform
%                          to the specified valtype
%            'maxblk'    & The maximum number of elements that can be
%                          computed in each batch. (default = 1e7)
%            'mfunctor'  & The functor to compute the metrics. In the form:
%                          V = f(X1, X2, ...)
%                          default = {@slmetric_pw, 'eucdist')
%            'tfunctor'  & The functor to transform the metric values,
%                          like tv = f(sv, ...)
%                          The transform is taken before the threshold
%                          filtering. default = []
%            'thres'     & The threshold values. The values not in the
%                          valid range set by the threshold is regarded as
%                          zeros.
%                          (default = [], means not to use threshold)
%            'threstype' & The type of threshold value, (default = 'ub')
%                           - 'lb': a lower bound scalar
%                                   valid_range: x >= thres
%                           - 'ub': a upper bound scalar
%                                   valid_range: x <= thres
%                           - 'lub': a vector of lower and upper bounds
%                                    valid_range: thres(1) <= x <= thres(2)
%     \*
%
%   - G = slpwmetricgraph(X, Xt, ...) constructs a bigraph with different
%     source and target node set. The properties mentioned above are
%     all applicable here.
%
% $ Remarks $
%   - The implementation is based on slpwgraph.
%
% $ History $
%   - Created by Dahua Lin, on Sep 8th, 2006
%   - Modified by Dahua Lin, on Sep 10th, 2006
%       - Base on upgraded slpwgraph function
%       - Add support for bigraph
%

%% parse and verify input

if nargin >= 2
    if isnumeric(varargin{1})
        Xt = varargin{1};
        if nargin == 2
            params = {};
        else
            params = varargin(2:end);
        end
    else
        Xt = X;
        params = varargin;
    end
else
    Xt = X;
    params = {};
end

opts.sparse = true;
opts.valtype = 'numeric';
opts.maxblk = 1e7;
opts.mfunctor = {@slmetric_pw, 'eucdist'};
opts.tfunctor = [];
opts.thres = [];
opts.threstype = 'ub';
opts = slparseprops(opts, params{:});

if ~ismember(opts.threstype, {'lb', 'ub', 'lub'})
    error('sltoolbox:invalidarg', ...
        'Invalid type of threshold: %s', opts.threstype);
end

%% Main skeleton

n = size(X, 2);
nt = size(Xt, 2);
mfunctor = opts.mfunctor;
tfunctor = opts.tfunctor;
filter = getvaluefilter(opts);
evalfunctor = {@metricevalfunc, mfunctor, tfunctor, filter};

G = slpwgraph(X, Xt, n, nt, evalfunctor, ...
    'sparse', opts.sparse, ...
    'valtype', opts.valtype, ...
    'maxblk', opts.maxblk);

%% Core functions

function V = metricevalfunc(X, Xt, inds1, inds2, mfunctor, tfunctor, filter)

X1 = X(:, inds1);
X2 = Xt(:, inds2);
V = slevalfunctor(mfunctor, X1, X2);

if ~isempty(tfunctor)
    V = slevalfunctor(tfunctor, V);
end

if ~isempty(filter)
    zero_range = ~filter(V);
    V(zero_range) = 0;
end

function filter = getvaluefilter(opts)

if isempty(opts.thres)
    filter = [];
else
    thr = opts.thres;
    switch opts.threstype
        case 'lb'
            if ~isscalar(thr)
                error('sltoolbox:invalidarg', 'For lb type, the thres should be a scalar');
            end
            filter = @(x) x >= thr;
        case 'ub'
            if ~isscalar(thr)
                error('sltoolbox:invalidarg', 'For ub type, the thres should be a scalar');
            end
            filter = @(x) x <= thr;
        case 'lub'
            if ~isvector(thr) || length(thr) ~= 2
                error('sltoolbox:invalidarg', 'For lub type, the thres should be a length-2 vector');
            end
            filter = @(x) x >= thr(1) & x <= thr(2);
    end
end


function gamma=ref_gabdualns_3(g,V);
%REF_GABDUALNS_3  GABDUALNS by multiwindow method.
%   Usage:  gamma=ref_gabdualns_3(g,V);
%

[gm,a,M]=ref_nonsep2multiwin(g,V);

gmd=gabdual(gm,a,M);

gamma=gmd(:,1);


function rs = gmi(s, D, eps, NNR, len, Nref)

%tstoolbox/@signal/gmi
%   Syntax:
%     * gmi(s, D, eps, NNR, len, Nref)
%
%   Input arguments:
%     * D -
%     * eps -
%     * NNR -
%     * len -
%     * Nref -
%
%   Generalized mutual information function for a scalar time series
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt
narginchk(6,6);

if (ndim(s) > 1) | (~isreal(data(s)))
	help(mfilename)
	return
end

L = dlens(s, 1);
[y,i] = sort(data(s));
ra(i) =  0:(1/L):(1-1/L);		% create rank values

e = embed(signal(ra'), D, 1);		%'

c = core(gmi(data(e), ceil(rand(Nref,1)*(L-len-D-3))+2, eps, NNR, len));

rs = signal(c, s);      % special constructor calling syntax for working routines
a = getaxis(s, 1);
dl = delta(a);
a = setfirst(a, 0);
rs = setaxis(rs, 1, a);
rs = setyunit(rs, unit);                % gmi values are scalars without unit
rs = addhistory(rs, 'Generalized mutual information function');
rs = addcommandlines(rs, 's = gmi(s', D, eps, NNR, len, Nref);


% -------------------------------------------------------------------------
% Author: Jai Juneja
% Date: 12/02/2013
% -------------------------------------------------------------------------
function World = ekfSLAM(handles, AxisDim, Lmks, Wpts, Obstacles)

    %%%%%%%%%%%%%%%%%%%%%%%%% INITIALISATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    cla; % Clear axes
    rob = Robot; % Create new robot object
    sen = Sensor;
    sen_rf = Sensor;
    sen_rf.noise = [0.1; 3 * pi/180];
    sen_rf.range = 10;
    sen.range = 30;
    sen.fov = 270 * pi/180;
    rob.q = [0.1;2*pi/180];

    [World, rob] = configuration(rob, sen_rf, Lmks, Wpts, AxisDim);

    Graphics = initGraphics(World, Obstacles, handles);

    % Determine which obstacles are moving
    numObstacles = length(Obstacles);
    numSegments = 0;
    movingObstacles = zeros(1, numObstacles);
    for i = 1:numObstacles
        if ~isequal([0; 0], Obstacles(i).velocity)
            movingObstacles(i) = 1;
        end
        numSegments = numSegments + size(Obstacles(i).vertices, 2) - 1;
    end
    movingObstacles = find(movingObstacles);

    %%%%%%%%%%%%%%%%%%%%%%%% TEMPORAL LOOP %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % Initialise some loop variables
    pose_this_scan = [];
    this_scan_good = 0;
    big_turn = 0;
    r = World.r;    % Location of robot pose in mapspace

    for t = 1:World.tend

        World.t = t;

        %%%%%%%%%%%%%%%%%%%%%%%%% SIMULATE WORLD %%%%%%%%%%%%%%%%%%%%%%%%%%
        R_old = rob.R;
        if ~isempty(World.Wpts)
            rob.steer(World.Wpts);
        end
        for i = movingObstacles
            Obstacles(i).move(AxisDim);
        end
        % n = rob.q .* randn(2,1);    % Control noise in real world
        rob.R = rob.move(zeros(2,1), 'true');        % Move with noise
        for lid = 1:size(World.W,2)
            v = sen_rf.noise .* randn(2,1);
            World.y(:,lid) = scanPoint(rob.R, World.W(:,lid)) + v;
        end
        lmks_all = World.y;
        % Determine landmarks that are within the sensor range
        [World.y, lmks_visible] = sen_rf.constrainMeasurement(World.y);

        %%%%%%%%%%%%%%%%%%%%%%%%%%% START EKF %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%%%%%%%%%%%%%%%%%%%%% EKF PREDICTION STEP %%%%%%%%%%%%%%%%%%%%%%%

        % 1. POSE PREDICTION USING SCAN MATCHING %%%%%%%%%%%%%%%%%%%%%%%%%%
        % TODO: if scan_matching, then do this step. Scan match only if
        % ~isempty(Obstacles)
        enough_correlations = 0;

        % Current stored scan is now the last scan
        last_scan_good = this_scan_good;
        pose_last_scan = pose_this_scan;
        this_scan_good = 0; % Initialise this_scan_good for this iteration

        % Put the previous scan's data in scan_ref
        scan_ref = World.scan_data;
        % Initialise scan data vector at maximum possible size it can take
        World.scan_data = -ones(3, numSegments * (round(sen.fov/sen.angular_res) + 1));
        scan_ndx = 1;
        for i = 1:length(Obstacles)
            scan_data = Obstacles(i).getMeasured(sen, rob);
            % Build scan_data vector
            if ~isempty(scan_data)
                World.scan_data(:, scan_ndx:scan_ndx+size(scan_data, 2)-1) = scan_data;
                scan_ndx = scan_ndx + size(scan_data, 2);
            end
        end
        % Reduce scan to only those appended in above for loop
        World.scan_data = World.scan_data(:, World.scan_data(2, :) ~= -1);

        % Reduce scan so that each laser angle only has one associated
        % measurement
        World.scan_data = removeDuplicateLasers(World.scan_data);
        World.scan_data = World.scan_data(2:3, :); % Remove index row

        % First add Gaussian white noise to scan
        v = repmat(sen.noise, 1, size(World.scan_data, 2)) .* randn(2,size(World.scan_data, 2));
        World.scan_data = World.scan_data + v;

        if ~isempty(World.scan_data)
            obstaclesDetected = 1;

            % Convert to Cartesian co-ordinates for scan matching
            World.scan_data = getInvMeasurement(World.scan_data);

            % If the number of point scanned exceeds the specified
            % tolerance, it is suitable for scan matching
            if size(World.scan_data, 2) > World.scan_corr_tolerance
                this_scan_good = 1;
            end

            % Convert to global co-ordinates for graphical display
            % World.scan_global = transToGlobal(rob.R, World.scan_data);
        else
            obstaclesDetected = 0;
            World.scan_global = [];
        end
        % If both last and current scans are good for scan matching,
        % proceed to match scans:
        if last_scan_good && this_scan_good
            n = rob.q .* randn(2,1); % Noise in odometry measurement

            % Do ICP scan matching using odometry input as initial guess
            [R, T, correl, icp_var] = doICP(scan_ref, World.scan_data, 1, rob.u + n);
            % If there are enough points correlated between the two
            % scans:
            if length(correl) > World.scan_corr_tolerance
                enough_correlations = 1;
                % Determine estimated pose from scan match
                r_scan = zeros(3, 1);
                da = getPiToPi(asin(R(2,1)));
                r_scan(1:2) = transToGlobal(pose_last_scan, T);
                r_scan(3) = pose_last_scan(3) + da;

                % Transform scan data to global co-ordinates
                scan_data_corr = World.scan_data;
                scan_data_corr = transToGlobal(r_scan, scan_data_corr);
                % Grab data points that were successfully corellated
                scan_data_corr = scan_data_corr(:, correl(:, 2)');

                [r_odo, R_r, R_n] = rob.move(n);

                % Compute covariance matrix of scan match
                C = getICPCovariance(icp_var, scan_data_corr);
                J_u = [R zeros(2, 1); 0 0 1];
                cov_scan = J_u * C * J_u';
                cov_scan_norm = trace(cov_scan);
            end
        end

        % 2. POSE PREDICTION USING ODOMETRY MEASUREMENTS %%%%%%%%%%%%%%%%%%

        % If there isn't a useful scan, just use odometry data
        if ~enough_correlations
            n = rob.q .* randn(2,1);
            [r_odo, R_r, R_n] = rob.move(n);
            cov_scan = zeros(3, 3);
            dr_scan = zeros(3, 1);
        else
            dr_scan = r_scan - rob.r;
            dr_scan(3) = getPiToPi(dr_scan(3));
        end

        % 3. WEIGHTAGE OF ODOMETRY AND SCAN MATCH PREDICTIONS %%%%%%%%%%%%%

        cov_odo = R_n * World.Q * R_n';
        cov_odo_norm = trace(cov_odo);

        dr_odo = r_odo - rob.r;
        dr_odo(3) = getPiToPi(dr_odo(3));

        dr_true = rob.R - R_old;

        % Check if the robot is making a large turn
        if abs(dr_scan(2)) > pi/6 || abs(dr_odo(2)) > pi/6
            big_turn = 1;
        elseif abs(dr_odo(2)) < pi/18
            big_turn = 0;
        end

        % Determine weights:
        % If not enough correlations, or turning is large, use odometry
        if ~enough_correlations % || big_turn
            weight_odo = 1;
            weight_scan = 0;
        else
            weight_odo = cov_scan_norm / (cov_scan_norm + cov_odo_norm);
            weight_scan = 1 - weight_odo;
            weight_scan = 1;
            weight_odo = 0;
        end

        % Determine robot pose using weights:
        rob.r = weight_odo * dr_odo + weight_scan * dr_scan + rob.r;
        World.x(r) = rob.r;

        % Covariance update
        % Caution: this method of update is suboptimal for CPU processing.
        % Alternative is to have a single equation that updates both
        % the pose and landmark covariances in a single step, but this
        % equation is rather complicated.
        P_rr = World.P(r,r);
        World.P(r,:) = R_r * World.P(r,:);
        World.P(:,r) = World.P(r,:)';
        World.P(r,r) = R_r * P_rr * R_r' + ...
            weight_scan * cov_scan + ...
            weight_odo * cov_odo;

        %%%%%%%%%%%%%%%%%%%%%% EKF UPDATE STEP %%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % 1. CORRECT KNOWN VISIBLE LANDMARKS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        % Find the visible landmarks. The below code circumvents the data
        % association problem:
        r_old = rob.r;

        lmks_visible_known = find(World.l(1,lmks_visible));
        lids = lmks_visible(lmks_visible_known);

        for lid = lids
            % Measurement prediction
            [e, E_r, E_l] = scanPoint(rob.r, World.x(World.l(:,lid)));

            E_rl = [E_r E_l];
            rl   = [r World.l(:,lid)'];
            E    = E_rl * World.P(rl,rl) * E_rl';

            % Actual Measurement
            yi = World.y(:,lid);

            % Innovation
            z = yi - e;
            z(2) = getPiToPi(z(2));
            Z = World.M + E;

            % Kalman gain
            K = World.P(:, rl) * E_rl' * Z^-1;

            % Update state and covariance
            World.x = World.x + K * z;
            World.P = World.P - K * Z * K'; % The complexity of this line is
                                            % very high (subtraction of two
                                            % large matrices)
            rob.r = World.x(r);
        end

        % 2. INITIALISE NEW LANDMARKS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        % Check landmark availabiliy.
        % Again data association is avoided. A new landmark is initialised
        % in the storage space reserved specifically for that landmark:
        lmks_visible_unknown = find(World.l(1,lmks_visible)==0);
        lids = lmks_visible(lmks_visible_unknown);

        if ~isempty(lids)
            for lid = lids
                s = find(World.mapspace, 2);
                if ~isempty(s)
                    World.mapspace(s) = 0;
                    World.l(:,lid) = s';
                    % Measurement
                    yi = World.y(:,lid);

                    [World.x(World.l(:,lid)), L_r, L_y] = invScanPoint(rob.r, yi);
                    World.P(s,:) = L_r * World.P(r,:);
                    World.P(:,s) = World.P(s,:)'; % Cross variance of lmk with all other lmks
                    World.P(s,s) = L_r * World.P(r,r) * L_r' + L_y * World.M * L_y';
                end
            end
        end
        pose_this_scan = rob.r; % The corrected pose at which the scan was taken

        %%%%%%%%%%%%%%%%%%%%%%%%%% MAPPING %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Determine pose correction during update step
        update = rob.r - r_old;

        if enough_correlations && ~isequal(rob.r, r_old)
            % Adjust latest correlated scan points to account for pose correction
            World.scan_data = transformPoints(World.scan_data, update);

            World = doMapping(World, sen, rob.r, World.scan_data, handles);
        elseif ~obstaclesDetected && isequal(mod(World.t, 5), 0)
            % If no obstacles have been detected, only compute map every 5
            % iterations
            World = doMapping(World, sen, rob.r, World.scan_data, handles);
        end

        if ~isempty(World.scan_data)
            World.scan_global = transToGlobal(rob.r, World.scan_data);
        end

        %%%%%%%%%%%%%%%%%%%%%%% HISTORICAL DATA COLECTION %%%%%%%%%%%%%%%%%
        World.R_hist(:, t) = rob.R(1:2);	% True position history
        World.r_hist(:, t) = rob.r(1:2);   % Estimated position history
        pose_error2 = (rob.R(1:2) - rob.r(1:2)).^2;
        World.error_hist(t) = sqrt(pose_error2(1) + pose_error2(2));
        World.Pr_hist(:, t) = [World.P(r(1),r(1)); World.P(r(2),r(2))];

        if enough_correlations
            scan_error = abs(dr_scan-dr_true); scan_error(3) = abs(getPiToPi(scan_error(3)));
            scan_error(1) = pdist([0 0; scan_error(1) scan_error(2)]); scan_error(2) = [];
        else
            scan_error = [0; 0];
        end

        odo_error = abs(dr_odo-dr_true); odo_error(3) = abs(getPiToPi(odo_error(3)));
        odo_error(1) = pdist([0 0; odo_error(1) odo_error(2)]); odo_error(2) = [];

        World.scan_error_hist(:, t) = scan_error;
        World.odo_error_hist(:, t) = odo_error;
        World.turning_hist(t) = rob.u(2);
        World.weight_scan_hist(t) = weight_scan;
        World.weight_odo_hist(t) = weight_odo;

        %%%%%%%%%%%%%%%%%%%%%%%%%%%% GRAPHICS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        Graphics.lmks_all = lmks_all; Graphics.lmks_visible = lmks_visible;
        doGraphics(rob, World, Graphics, Obstacles(movingObstacles), AxisDim);

    end

    %%%%%%%%%%%%%%%%%%%%%%%% PLOT HISTORICAL DATA %%%%%%%%%%%%%%%%%%%%%%%%%
    % Plot trajectories;
    set(Graphics.R_hist, 'xdata', World.R_hist(1,:), 'ydata', World.R_hist(2, :))
    set(Graphics.r_hist, 'xdata', World.r_hist(1,:), 'ydata', World.r_hist(2, :))

    % Plot position uncertainties
    figure('color', 'white')
    subplot(2, 1, 1)
    plot(1:World.tend, World.error_hist)
    title('EKF SLAM: position error over time')
    xlabel('Time (s)'), ylabel('Position Error (m)')
    subplot(2, 1, 2)
    plot(1:World.tend, sqrt(World.Pr_hist(1, :)), 'r', ...
        1:World.tend, sqrt(World.Pr_hist(2, :)), 'b')
    title('EKF SLAM: position uncertainty over time')
    xlabel('Time (s)'), ylabel('Standard Deviation (m)')
    legend('St. Dev. in x', 'St. Dev. in y')

    figure('color', 'white')
    subplot(4, 1, 1)
    AX = plotyy(1:World.tend, World.scan_error_hist(1,:), ...
        1:World.tend, World.scan_error_hist(2,:), 'plot');
    set(get(AX(1),'Ylabel'),'String',['Position' char(10) 'Error (m)'])
    set(get(AX(2),'Ylabel'),'String',['Angular' char(10) 'Error (rad)'])
    title('Scan errors over time')
    xlabel('Time (s)')

    subplot(4, 1, 2)
    AX2 = plotyy(1:World.tend, World.odo_error_hist(1,:), ...
        1:World.tend, World.odo_error_hist(2,:), 'plot');
    set(get(AX2(1),'Ylabel'),'String',['Position' char(10) 'Error (m)'])
    set(get(AX2(2),'Ylabel'),'String',['Angular' char(10) 'Error (rad)'])
    title('Odometry error over time')

    subplot(4, 1, 3)
    plot(1:World.tend, World.weight_scan_hist, 'r', ...
        1:World.tend, World.weight_odo_hist, 'b')
    title('Scan and odometry weights over time')
    xlabel('Time (s)'), ylabel('Weight')
    legend('Scan', 'Odometry')
    axis tight

    subplot(4, 1, 4)
    plot(1:World.tend, World.turning_hist)
    title('Turning control over time')
    xlabel('Time (s)'), ylabel('Angle (rad)')
    axis tight
end


function [x,w,x_variables,w_variables,aux_variables,F,failure] = robust_classify_variables(F,h,ops,w);

failure = 0;
% Variables before expanding nonlinear operators
initial_variables = unique([depends(F) depends(h)]);

% Uncertain variables
w_variables = getvariables(w);

% Decision variables
x_variables = 1:yalmip('nvars');
x_variables = setdiff(x_variables,w_variables);

% Any fancy modelling goin on here? If so, we need to expand the model, and
% associate the new lifted variables either to the uncertainty set or to
% the set of general variables. There are some classes of variables
% 1. Uncertain variables
% 2. Original decision variables
% 4. Lifted variables used for uncertain variables in uncertainty description
% 5. Lifted variables used for uncertain variables in uncertain model
% 6. Lifted variables used for mixed  variables in uncertain model
% FIX : This code is currently a miserable hack and should be cleaned up
extended = yalmip('extvariables');
aux_variables = [];
if ~isempty(extended) & any(ismember(initial_variables,extended))

    % Remove auxilliary variables (defined by YALMIP)
    aux_variables = intersect(extended,x_variables);
    % These variables are original decision variables
    x_variables = setdiff(x_variables,aux_variables);

    % Original constraint index
    id = getlmiid(F(find(~lifted(F))));

    % This is a tweak to allow epxansion of bilinear terms in robust problems,
    % is expression such as abs(x*w) < 1 for all -1 < w < 1
    ops.expandbilinear = 1;

    % Expand the model to model norms etc
    [F,failure,cause] = expandmodel(F,h,ops,w);
    if failure % Convexity propgation failed
        interfacedata = [];
        recoverdata = [];
        solver = '';
        diagnostic.solvertime = 0;
        diagnostic.problem = 14;
        diagnostic.info = yalmiperror(14,cause);
        x = [];
        w = [];
        x_variables=[];
        w_variables=[];
        F = [];
        return
    end

    % Auxillary variables (introduced by YALMIP). These variables are lifting
    % variables that are used to model advanced constructs suck as norms etc in
    % the nonlinear operator framework
    new_aux_variables = setdiff(unique([depends(F) depends(h)]),initial_variables);
    aux_variables = setdiff(union(aux_variables,new_aux_variables),w_variables);

    % We know try to find the auxillary variables that are used to model
    % functions that only depends on w
    F_lifted = F(find(~ismember(getlmiid(F),id)));
    aux_variables_w = aux_variables;
    for i = 1:length(F_lifted)
        variables = depends(F_lifted(i));
        with_x = any(ismember(variables,x_variables));
        with_w = any(ismember(variables,w_variables));
        if with_x
            aux_variables_w = setdiff(aux_variables_w,intersect(variables,aux_variables));
            x_variables = union(x_variables,intersect(variables,aux_variables));
        end
    end

    aux_variables_x = setdiff(aux_variables,aux_variables_w);
    x_variables = union(aux_variables_x, x_variables);
    w_variables = union(aux_variables_w, w_variables);
end

x_variables = intersect([depends(F) depends(h)],x_variables);
w_variables = intersect([depends(F) depends(h)],w_variables);

x = recover(x_variables);
x = recover(setdiff(depends(x),w_variables));
x_variables = getvariables(x);

% % Could be some of these two cases
% % F = (abs(x) + w < 1) + (norm(w,1) < 0.3);
% % F = (x + abs(w) < 1) + (-2 < w < 2) ;
%
% w_variables = setdiff(w_variables,heh);
% aux_variables = union(aux_variables,heh);

w = recover(w_variables);


%% Simulation Data
simu = simulationClass();                       % Initialize simulationClass
simu.simMechanicsFile = 'OSWEC.slx';            % Simulink Model File
simu.startTime = 0;                             % Simulation Start Time [s]
simu.rampTime = 100;                            % Wave Ramp Time [s]
simu.endTime=400;                               % Simulation End Time [s]
simu.dt = 0.1;                                  % Simulation Time-Step [s]

%% Wave Information
% Regular Waves
waves = waveClass('regular');                   % Initialize waveClass
waves.height = 2.5;                                  % Wave Height [m]
waves.period = 8;                                    % Wave Period [s]

%% Body Data
% Flap
body(1) = bodyClass('hydroData/oswec.h5');      % Initialize bodyClass for Flap
body(1).geometryFile = 'geometry/flap.stl';     % Geometry File
body(1).mass = 127000;                          % Mass [kg]
body(1).inertia = [1.85e6 1.85e6 1.85e6];  % Moment of Inertia [kg-m^2]

% Base
body(2) = bodyClass('hydroData/oswec.h5');      % Initialize bodyClass for Base
body(2).geometryFile = 'geometry/base.stl';     % Geometry File
body(2).mass = 'fixed';                         % Creates Fixed Body

%% PTO and Constraint Parameters
% Fixed
constraint(1)= constraintClass('Constraint1');  % Initialize constraintClass for Constraint1
constraint(1).location = [0 0 -10];                  % Constraint Location [m]

% Rotational PTO
pto(1) = ptoClass('PTO1');                      % Initialize ptoClass for PTO1
pto(1).stiffness = 0;                           % PTO Stiffness [Nm/rad]
pto(1).damping = 0;                                   % PTO Damping [Nsm/rad]
pto(1).location = [0 0 -8.9];                        % PTO Location [m]


function [ result ] = plot_neb_spline( directory  )
%PLOT_NEB_SPLINE Plot the energy along a NEB path.
%   result = PLOT_NEB_SPLINE() plots the energy as a function of
%   hyperdistance using the results of a NEB calculation. Each image
%   directory (01, 02, ... NIMG) must contain an OUTCAR and CONTCAR file.
%   The end points (00 and NIMG+1) must contain an OUTCAR and POSCAR file.
%   The energies are fit with a spline using NEB_SPLINE.
%
%   See also NEB_ENERGIES, NEB_SPLINE, HYPERDISTANCE.

    startDir = pwd();
    if nargin > 0
       cd(directory);
    end
    result = 0;

    [ energy0 x0 ] = neb_energies();
    [ energy x ] = neb_spline();

    figure
    hold on
    plot(x0, energy0, 'ok')
    plot(x, energy, 'k')

    xlabel(['Hyperdistance (' char(197) ')'])
    ylabel('Energy (eV)')

    cd(startDir);
end


function Training_Data = ReadFace(Training_Path)
% ---------- Construct 2D matrix from all of the 1D image vectors in the training data file ------------
flist = dir(strcat(Training_Path,'\*.jpg'));
Training_Data = [];
for imidx = 1:length(flist)
    fprintf('Constructing Training Image Data Space [%d] \n', imidx);
    path = strcat(Training_Path,strcat('\',int2str(imidx),'.jpg'));
    img = imread(path);
    [irow icol] = size(img);
    temp = reshape(img',irow*icol,1);   % Reshaping 2D images to 1D image vectors
    Training_Data = [Training_Data temp];
end
fprintf('\n');


function [g, nu] = cmpndNoiseNuG(noise, mu, varSigma, y)

% CMPNDNOISENUG  Update nu and g parameters associated with compound noise model.

% NOISE

% NOISE

g = zeros(size(mu));
nu = zeros(size(g));
for i = 1:length(noise.comp)
  [g(:, i), nu(:, i)] = ...
      noiseUpdateNuG(noise.comp{i}, ...
                       mu(:, i), varSigma(:, i), ...
                       y(:, i));
  if any(nu(:, i)< 0)
    if noise.comp{i}.logconcave
      warning('nu less than zero in log concave model.')
    else
      fprintf('nu less than zero\n')
    end
  end
end


function [score] = FRIQUEE(I)

testFriqueeFeats = extractFRIQUEEFeatures(I);

% Load a learned model
load('support_functions/FRIQUEE/data/friqueeLearnedModel.mat');

% Scale the features of the test image accordingly.
% The minimum and the range are computed on features of all the images of
% LIVE Challenge Database
testFriqueeALL = testFeatNormalize(testFriqueeFeats.friqueeALL, friqueeLearnedModel.trainDataMinVals, friqueeLearnedModel.trainDataRange);

score = svmpredict (0, double(testFriqueeALL), friqueeLearnedModel.trainModel, '-b 1 -q');

end


addpath /home/fowlkes/gpb_owt/grouping/lib/
outFile = '/home/fowlkes/gpb_owt/grouping/data/101087_gPb.mat_pbs.mat';
simFile = '/home/fowlkes/gpb_owt/grouping/data/101087_gPb.mat_pbs.mat_sim.tmp';
imgFile = '/home/fowlkes/gpb_owt/grouping/data/101087_gPb.mat_pbs.mat_img.jpg';
latFile = '/home/fowlkes/gpb_owt/grouping/data/101087_gPb.mat_pbs.mat_lat.tmp';
load /home/fowlkes/gpb_owt/grouping/data/mPb.mat  %load in mPb data.

%
% old code
%
l{1} = zeros(size(mPb, 1) + 1, size(mPb, 2));
l{1}(2:end, :) = mPb;
l{2} = zeros(size(mPb, 1), size(mPb, 2) + 1);
l{2}(:, 2:end) = mPb;
latFile = strcat(outFile, '_lat.tmp');
write_array(latFile, l);

system(sprintf('/home/fowlkes/gpb_owt/grouping/lib/segment -image %s -smatrixfile %s -readlattice true -latticefile %s -dthresh 5', imgFile, simFile, latFile));
Wold = read_smatrix(simFile);

%
% new code
%

% mex buildW.cpp -Iutil smatrix.cc ic.cc affinity.cc util/libutil.a

[x,y,z] = buildW(l{1},l{2});
Wnew = sparse(x,y,z);

sum(sum( (Wold-Wnew).^2 ))


%% Script to test the optical flow estimation
clear all
close all

img1f = '/datos/research/deblurring/data/jue/videoDeblur/snow/input/0001.png';
img2f = '/datos/research/deblurring/data/jue/videoDeblur/snow/input/0002.png';

%img1 = single(rgb2gray(imread(img1f)));
%img2 = single(rgb2gray(imread(img2f)));

img1 = double((imread(img1f)));
img2 = double((imread(img2f)));

img1 = .299*img1(:,:,1) + .587*img1(:,:,2) + .114*img1(:,:,3);
img2 = .299*img2(:,:,1) + .587*img2(:,:,2) + .114*img2(:,:,3);

img1 = single(floor(img1));
img2 = single(floor(img2));

figure(1)
imagesc(img2)

%%
tau=0.25;
lambda = 0.1;
theta=0.3;
nscales=4;
zfactor=0.5;
nwarps=5;
epsilon=0.01;
verbose=1;

[m,n,~] = size(img1);
tagstruct.ImageLength = m;
tagstruct.ImageWidth = n;
tagstruct.Compression = Tiff.Compression.None;
tagstruct.SampleFormat = Tiff.SampleFormat.IEEEFP;
tagstruct.Photometric = Tiff.Photometric.MinIsBlack;
tagstruct.BitsPerSample =  32;                        % float data
tagstruct.SamplesPerPixel = 2; %u and v (2 channels)
tagstruct.PlanarConfiguration = Tiff.PlanarConfiguration.Chunky;

flowF = tvl1flow(img1,img2,tau,lambda,theta,nscales,zfactor,nwarps, epsilon,verbose);

%Save into a TIFF file
t = Tiff('aux1.tiff', 'w');
t.setTag(tagstruct);
t.write(single(flowF));
t.close();

%% OLD

setenv('DYLD_LIBRARY_PATH', '/usr/local/lib'),
tvl1flowS='/datos/research/deblurring/code/heat/videodeblur/tvl1flow_3_mex/tvl1flow';

flowB = 'aux.tiff';
system(sprintf('%s %s %s %s 0 0.25 0.1 0.3 4 0.5 5 0.01 1',tvl1flowS,img1f,img2f,flowB)); % flowB.tiff

%%
aux1 = imread('aux1.tiff');
aux = imread('aux.tiff');

figure(1)
imagesc(flowF(:,:,1)-aux(:,:,1))

figure(2)
imagesc(flowF(:,:,2)-aux(:,:,2))


%% This script simulates the LODCO-based epsilon-Greedy algorithm.
%  author: Hailiang Zhao
clc, clear
opt = optimset('Display', 'none');

%% basic parameter settings (had better not change those paras)
k = 1e-28;                % effective switched capacitance (a constant decided by the chip architecture)
tau = 0.002;              % the length of time slot (in second)
phi = 0.002;              % the cost of task dropping (in second)
omega = 1e6;              % the bandwidth of MEC server (in Hz)
sigma = 1e-13;            % the noise power of the receiver (in W)
p_tx_max = 1;             % the maximum transmit power of mobile device (in W)
f_max = 1.5e9;            % the maximum CPU-cycle frequency of mobile device (in Hz)
E_max = 0.002;            % the maximum amout of battery output energy (in J)
L = 1000;                 % the input size of the computation task (in bit)
X = 737.5;                % the number of CPU cycles needed on processing one bit of task
W = L * X;                % the number of CPU cycles needed on processing one task
E_H_max = 48e-6;          % the upper bound of the energy arrive at the mobile device (in J)
p_H = E_H_max / (2*tau);  % the average Energy Harvesting (EH) power (in W)
g0 = power(10, -4);       % the path-loss constant
d0 = 1;                   % the relative distance between each mobile device and each MEC server

%% parameter control
N = 10;                   % the number of mobile devices
M = 8;                    % the number of MEC servers
T = 2000;                 % the number of time slot (a.k.a. the size of the time horizon)
tau_d = 0.002;            % execution deadline (in second)
d = 50;                   % the distance between the mobile device and the MEC server (in meter)
E_min = 0.02e-3;          % the minimum amout of battery output energy (in J)
V = 1e-5;                 % the weight of penalty (the control parameter introduced by Lyapunov Optimization)
rho = 0.6;                % the probability that the computation task is requested
max_connects = 4;         % the maximum number of processible mobile devices for each MEC server ($ \frac{f_s^{max} \tau}{L X} $)
min_distance = 10;        % the minimum distance from mobile device to MEC server
max_distance = 50;        % the maximum distance from mobile device to MEC server
eps = 0.8;                % if rand() <= eps, for the best i-j pair, we always choose MEC server execution mode (except no MEC server to choose)

% the lower bound of perturbation parameter
E_max_hat = min(max(k * W * (f_max)^2, p_tx_max * tau), E_max);
theta = E_max_hat + V * phi / E_min;

%% allocate storage for valuable results
B = zeros(T, N);                   % the battery energy level (in J)
B_hat = zeros(T, N);               % the virtual battery energy level ($B_hat = B - theta$)
e = zeros(T, N);                   % the amout of the harvested and stored energy (in J)
chosen_mode = zeros(T, N);         % {1: local, 2: remote, 3: drop, 4: no task request}
chosen_server = zeros(T, N);       % record the index of chosen server for each mobile device if its choice is MEC server execution
f = zeros(T, N);                   % the CPU-cycle frequency of local execution (in Hz)
p = zeros(T, N);                   % the transmit power of computation offloading (in W)

mobile_exe_cost = zeros(T, N);     % the mobile execution cost (delay) (in second)
server_exe_cost = zeros(T, N);     % the MEC server execution cost (delay) (in second)
final_chosen_cost = zeros(T, N);   % the final execution delay under currently chosen modes (in second)

mobile_exe_E = zeros(T, N);        % the energy consumption for mobile execution (in J)
server_exe_E = zeros(T, N);        % the energy consumption for MEC server execution (in J)
final_chosen_E = zeros(T, N);      % the energy consumption of the final chosen modes (in J)

%% simulation begin
t = 1;
while t <= T
    disp(['===> Time slot #', num2str(t), ' <==='])

    %% allocate storage for mode-chosen
    device_server_pairs = [];      % each column represents i, j, J_s^{\star}(i, j), respectively
    remained_connects = max_connects * ones(M, 1);    % the available connections of MEC servers
    J_m = zeros(N, 1); J_s = zeros(N, M);             % the matrices for J_m and J_s values
    p_mat = zeros(N, M);                              % the matrix for transmit power
    server_cost_mat = zeros(N, M);                    % the matrix for MEC server execution cost
    server_E_mat = zeros(N, M);                       % the matrix for energy consumption of MEC server

    %% initialization
    % generate the virtual battery energy level
    B_hat(t, :) = B(t, :) - theta;
    % generate the channel power gain (from each mobile device to each MEC sever)
    distances = unifrnd(min_distance, max_distance, N, M);
    gamma = exprnd(1, N, M);
    h_mat = g0 * gamma .* power(d0 ./ distances, 4);

    %% step 1: for each mobile device, choose the initial mode
    for i = 1: N
        disp(['Mobile device #', num2str(i)])

        %% step 1.1: get the optimal energy harvesting no matter whether task is requested
        E_H_t = unifrnd(0, E_H_max);
        if B_hat(t, i) <= 0
            e(t, i) = E_H_t;
        end

        %% step 1.2: get the (initial) optimal computation offloading strategy (I_m(i), I_s(i, :), I_d(i), f(t, i), p(t, i))
        % generate the task request
        zeta = binornd(1, rho);
        if zeta == 0
            % chosen mode has to be 4
            disp('no task request generated!')
            chosen_mode(t, i) = 4;
        else
            % chosen_mode is chosen from {1, 2, 3}
            disp('task request generated!')

            %% step 1.2.1: solve the optimization problem $\mathcal{P}_{ME}$ (f(t, i) > 0)
            % calculate f_L and f_U
            f_L = max(sqrt(E_min / (k * W)), W / tau_d);
            f_U = min(sqrt(E_max / (k * W)), f_max);
            if f_L <= f_U
                % the sub-problem is feasible
                disp('mobile execution ($\mathcal{P}_{ME}$) is feasible!')

                if B_hat(t, i) < 0
                    f_0 = (V / (-2 * B_hat(t, i) * k))^(1/3);
                else
                    % complex number may exist, which may lead to error
                    f_0 = -(V / (2 * B_hat(t, i) * k))^(1/3);
                end

                if (f_0 > f_U && B_hat(t, i) < 0) || (B_hat(t, i) >= 0)
                    f(t, i) = f_U;
                elseif f_0 >= f_L && f_0 <= f_U && B_hat(t, i) < 0
                    f(t, i) = f_0;
                elseif f_0 < f_L && B_hat(t, i) < 0
                    f(t, i) = f_L;
                end
                % check whether f(t, i) is zero
                if f(t, i) == 0
                    disp('Something wrong! f is 0!')
                end

                % calculate the delay of mobile execution
                mobile_exe_cost(t, i) = W / f(t, i);
                % calculate the energy consumption of mobile execution
                mobile_exe_E(t, i) = k * W * (f(t, i)^2);
                % calculate the value of optimization goal
                J_m(i) = -B_hat(t, i) * k * W * (f(t, i)^2 + V * W / f(t, i));
            else
                % the sub-problem is not fasible because (i) the limited
                % computation capacity or (ii) time cosumed out of deadline
                % or (iii) the energy consumed out of battery energy level
                % If it is not feasible, it just means that we cannot choose
                % 'I_m(i)=1'. It dosen't mean that the task has to be dropped.
                disp('mobile execution ($\mathcal{P}_{ME}$) is not feasible!')
                f(t, i) = 0;
                mobile_exe_cost(t, i) = 0;
                mobile_exe_E(t, i) = 0;
                % 'I_m(i)=1' can never be chosen if mobile execution goal is inf
                J_m(i) = inf;
            end

            %% step 1.2.2: solve the optimization problem $\mathcal{P}_{SE}$ (p(t, i) > 0)
            % calculate J_s(i, j) from mobile device i to each MEC server j
            for j = 1: M
                disp(['MEC server #', num2str(j)])
                h = h_mat(i, j);

                E_tmp = sigma * L * log(2) / (omega * h);
                p_L_taud = (power(2, L / (omega * tau_d)) - 1) * sigma / h;
                % calculate p_L
                if E_tmp >= E_min
                    p_L = p_L_taud;
                else
                    % calculate p_E_min (use inline function and fsolve)
                    y = @(x) x * L - omega * log2(1 + h*x/sigma) * E_min;
                    % accroding to the function figure, p_L_taud is a positive
                    % number around 0.2
                    p_E_min = fsolve(y, 0.2, opt);
                    p_L = max(p_L_taud, p_E_min);
                end
                % calculate p_U
                if E_tmp >= E_max
                    p_U = 0;
                else
                    % caculate p_E_max (use inline function and fsolve)
                    y = @(x) x * L - omega * log2(1 + h*x/sigma) * E_max;
                    % accroding to the function figure, p_E_max is a large positive
                    % number around 20
                    p_E_max = fsolve(y, 100, opt);
                    p_U = min(p_tx_max, p_E_max);
                end

                if p_L <= p_U
                    % the sub-problem is feasible
                    disp('MEC server execution ($\mathcal{P}_{SE}$) is feasible!')
                    % calculate p_0
                    virtual_battery = B_hat(t, i);
                    y = @(x) virtual_battery * log2(1 + h*x/sigma) + ...
                        h * (V - virtual_battery*x) / log(2) / (sigma + h*x);
                    p_0 = fsolve(y, 0.5, opt);

                    if (p_U < p_0 && B_hat(t, i) < 0) || B_hat(t, i) >= 0
                        p_mat(i, j) = p_U;
                    elseif p_0 < p_L && B_hat(t) < 0
                        p_mat(i, j) = p_L;
                    elseif p_0 >= p_L && p_0 <= p_U && B_hat(t, i) < 0
                        p_mat(i, j) = p_0;
                    end
                    % check whether p_mat(i, j) is zero
                    if p_mat(i, j) == 0
                        disp('Something wrong! p is 0!')
                    end

                    % calculate the delay of MEC server execution
                    server_cost_mat(i, j) = L / (omega * log2(1 + h*p_mat(i, j)/sigma));
                    % calculate the energy consumption of MEC server execution
                    server_E_mat(i, j) = p_mat(i, j) * server_cost_mat(i, j);
                    % calculate the value of optimization goal
                    J_s(i, j) = (-B_hat(t, i) * p_mat(i, j) + V) * server_cost_mat(i, j);

                    % (we can not set server_exe_cost(t, i) and server_exe_E(t, i) for now)
                else
                    % the sub-problem is not feasible because (i) the limited transmit
                    % power or (ii) time cosumed out of deadline or (iii) the energy
                    % consumed out of battery energy level
                    % If it is not feasible, it just means that we cannot choose
                    % 'I_s(i,j)=1'. It dosen't mean that the task has to be dropped.
                    disp('MEC server execution ($\mathcal{P}_{SE}$) is not feasible!')
                    p_mat(i, j) = 0;
                    server_cost_mat(i, j) = 0;
                    server_E_mat(i, j) = 0;
                    % 'I_s(i,j)=1' can never be chosen if MEC server execution goal is inf
                    J_s(i, j) = inf;

                    % (we can not set server_exe_cost(t, i) and server_exe_E(t, i) for now)
                % Similarly, we do not check whether the energy cunsumed is larger than
                % battery energy level because the problem $\mathcal{J}_{CO}$ does
                % not have constraint (8).
                end
            end

            %% step 1.2.3: choose the (initial) optimal execution mode
            J_d = V * phi;
            disp(['J_m(i):', num2str(J_m(i))])
            disp(['J_s(i,:):', num2str(J_s(i, :))])
            [~, mode] = min([J_m(i), J_s(i, :), J_d]);
            if mode == 1
                % mobile execution
                chosen_mode(t, i) = 1;
                final_chosen_cost(t, i) = mobile_exe_cost(t, i);
                final_chosen_E(t, i) = mobile_exe_E(t, i);
            elseif mode == (M+2)
                % drop
                chosen_mode(t, i) = 3;
                final_chosen_cost(t, i) = phi;
                final_chosen_E(t, i) = 0;
            else
                % MEC servre execution
                chosen_mode(t, i) = 2;
                % add i, the chosen j, and their J_s(i, j) value into the pairs
                device_server_pairs = [device_server_pairs; [i, mode-1, J_s(i, mode-1)]];
            end
        end
    end

    %% step 2: allocate connection right for each mobile device who chooses MEC server execution
    while ~isempty(device_server_pairs)
        if rand() <= eps
            % find the {i, j, J_s(i, j)} with the minimum J_s(i, j) in the device_server_pairs, choose MEC server execution mode,
            % except no MEC server to choose
            [min_Js, idx] = min(device_server_pairs(:, 3));
            i = device_server_pairs(idx, 1); j = device_server_pairs(idx, 2);    % find the i and its j
            if remained_connects(j) >= 1
                % the chosen i can be allocated to the j-th MEC server
                % set its final modes as 2 and record the final cost and energy consumption for it
                chosen_mode(t, i) = 2;          % this is not necessary
                p(t, i) = p_mat(i, j);
                server_exe_cost(t, i) = server_cost_mat(i, j);
                server_exe_E(t, i) = server_E_mat(i, j);
                chosen_server(t, i) = j;
                final_chosen_cost(t, i) = server_exe_cost(t, i);
                final_chosen_E(t, i) = server_exe_E(t, i);

                % update remained_connects for j
                remained_connects(j) = remained_connects(j) - 1;
                % finally, remove it from global pairs, it is done
                device_server_pairs(device_server_pairs(:, 1) == i, :) = [];
            else
                % the chosen i can not be allocated to the j-th MEC server
                % set the mobile device's J_s(i, j) as inf
                % (which means no matter what happens, the MEC server j can never be chosen)
                J_s(i, j) = inf;
                % remove it from global pairs
                device_server_pairs(device_server_pairs(:, 1) == i, :) = [];
                % if there still exists j' where J_s(i, j') is not inf, we choose the best j',
                % otherwise we can only choose from mobile execution and drop
                if min(J_s(i, :)) ~= inf
                    [second_min_Js, second_min_j] = min(J_s(i, :));
                    % add the new pair into global pairs
                    device_server_pairs = [device_server_pairs; [i, second_min_j, second_min_Js]];
                else
                    % choose from mobile execution and drop for i
                    [~, mode] = min([J_m(i), inf, J_d]);
                    chosen_mode(t, i) = mode;
                    if mode == 1
                        final_chosen_cost(t, i) = mobile_exe_cost(t, i);
                        final_chosen_E(t, i) = mobile_exe_E(t, i);
                    else
                        final_chosen_cost(t, i) = phi;
                        final_chosen_E(t, i) = 0;
                    end
                end
            end
        else
            % do as LODCO-based Greedy algorithm do
            for j = 1: M
                % find every pair who chooses the MEC server j
                device_j_pairs = device_server_pairs(device_server_pairs(:, 2) == j, :);
                % find those devices is
                is = device_j_pairs(:, 1);
                if isempty(is)
                    disp(['For current MEC server #', num2str(j), ', no mobile device choose it!'])
                    % go to handle next MEC server
                    continue;
                end
                if remained_connects(j) >= length(is)
                    % every mobile device who chooses j can be connected,
                    % set their final modes as 2 and record the final cost and energy consumption for them
                    chosen_mode(t, is) = 2;     % this is not necessary
                    p(t, is) = transp(p_mat(is, j));
                    server_exe_cost(t, is) = transp(server_cost_mat(is, j));
                    server_exe_E(t, is) = transp(server_E_mat(is, j));
                    chosen_server(t, is) = repmat(j, 1, length(is));
                    final_chosen_cost(t, is) = server_exe_cost(t, is);
                    final_chosen_E(t, is) = server_exe_E(t, is);

                    % update remained_connects for j
                    remained_connects(j) = remained_connects(j) - length(is);
                    % finally, remove them from global pairs, they are done
                    device_server_pairs(device_server_pairs(:, 2) == j, :) = [];
                else
                    if remained_connects(j) == 0
                        % no mobile device can be connected, remove all mobile devices who chooses j from global pairs
                        device_server_pairs(device_server_pairs(:, 2) == j, :) = [];
                        % set those mobile devices' J_s(i, j) as inf
                        % (which means no matter what happens, the MEC server j can never be chosen)
                        J_s(is, j) = inf;
                        % choose mode again for those i and insert new potential pairs into global pairs
                        for idx = 1: numel(is)
                            i = is(idx);
                            [~, mode] = min([J_m(i), J_s(i, :), J_d]);
                            if mode == 1
                                chosen_mode(t, i) = 1;
                                final_chosen_cost(t, i) = mobile_exe_cost(t, i);
                                final_chosen_E(t, i) = mobile_exe_E(t, i);
                            elseif mode == (M+2)
                                chosen_mode(t, i) = 3;
                                final_chosen_cost(t, i) = phi;
                                final_chosen_E(t, i) = 0;
                            else
                                chosen_mode(t, i) = 2;
                                device_server_pairs = [device_server_pairs; [i, mode-1, J_s(i, mode-1)]];
                            end
                        end
                    else
                        % some mobile devices can be connected, set their final modes as 2 and remove them from global pairs
                        % besides, record the final cost and energy consumption for them
                        % for the left i', remove them from global pairs and set J_s(i', j) as inf,
                        % choose mode again for those i' and insert new potential pairs into global pairs

                        % sort the J_s(is, j) and return those lucky idxs
                        [~, idxs] = sort(device_j_pairs(:, 3));
                        for idx = 1: remained_connects(j)
                            i = idxs(idx);
                            chosen_mode(t, i) = 2;
                            p(t, i) = p_mat(i, j);
                            server_exe_cost(t, i) = server_cost_mat(i, j);
                            server_exe_E(t, i) = server_E_mat(i, j);
                            chosen_server(t, i) = j;
                            final_chosen_cost(t, i) = server_exe_cost(t, i);
                            final_chosen_E(t, i) = server_exe_E(t, i);

                            % remove i from global pairs
                            device_server_pairs(device_server_pairs(:, 1) == i, :) = [];
                        end

                        % update remained_connects for j
                        remained_connects(j) = 0;

                        % for those unlucky mobile devices i', set J_s(i', j) as inf (i' are in currently device_server_pairs(: , j) now)
                        residual_is = device_server_pairs(device_server_pairs(:, 2) == j, 1);
                        J_s(residual_is, j) = inf;
                        for idx = 1: numel(residual_is)
                            residual_i = residual_is(idx);
                            [~, mode] = min([J_m(residual_i), J_s(residual_i, :), J_d]);
                            if mode == 1
                                chosen_mode(t, residual_i) = 1;
                                final_chosen_cost(t, residual_i) = mobile_exe_cost(t, residual_i);
                                final_chosen_E(t, residual_i) = mobile_exe_E(t, residual_i);
                            elseif mode == (M+2)
                                chosen_mode(t, residual_i) = 3;
                                final_chosen_cost(t, residual_i) = phi;
                                final_chosen_E(t, residual_i) = 0;
                            else
                                chosen_mode(t, residual_i) = 2;
                                device_server_pairs = [device_server_pairs; [residual_i, mode-1, J_s(residual_i, mode-1)]];
                            end
                        end
                    end
                end
            end
        end
    end

    %% step 3: update the battery energy level and go to the next time slot
    B(t + 1, :) = B(t, :) - final_chosen_E(t, :) + e(t, :);
    t = t + 1;

end

%% step 4: evaluate the simulation results
% 1. the battery energy level vs. time slot
figure
plot(1:T, B(1:T, :))
hold on
plot(1:T, repmat(theta + E_H_max, [T, 1]), '-')
title('Envolution of battery energy level')
xlabel('time slot')
ylabel('battery energy level $B_t$ of each mobile device', 'Interpreter','latex')

% 2. the average execution cost vs. time slot
accumulated = 0;
average_cost = zeros(T, N);
figure
for i = 1: N
    % draw for each mobile device
    request_num = 0;
    for t = 1: T
        accumulated = accumulated + final_chosen_cost(t, i);
        if chosen_mode(t, i) ~= 4
            % there exists task request
            request_num = request_num + 1;
        end
        average_cost(t, i) = accumulated / request_num;
    end
    plot(1:T, average_cost(:, i));
    hold on
end
title('Envolution of average execution cost')
xlabel('time slot')
ylabel('average execution cost $\frac{1}{T} \sum_{t=0}^{T-1} cost^t$ of each mobile device', 'Interpreter','latex')

% 3. the average ratio of each chosen mode of the ith mobile device vs. time slot
average_ratio = zeros(T, 3);
mobile_exe = 0; server_exe = 0; drop = 0;
request_num = 0;
i = 1;          % we simply choose the first mobile device
for t = 1: T
    if final_chosen_cost(t, i) == 0
        continue
    else
        request_num = request_num + 1;
        if chosen_mode(t, i) == 1
            mobile_exe = mobile_exe + 1;
        elseif chosen_mode(t, i) == 2
            server_exe = server_exe + 1;
        else
            drop = drop + 1;
        end
    end
    average_ratio(t, :) = [mobile_exe, server_exe, drop] / request_num;
end
figure
plot(1:T, average_ratio(:, 1));
hold on
plot(1:T, average_ratio(:, 2));
hold on
plot(1:T, average_ratio(:, 3));
legend('mobile execution', 'MEC server execution', 'drop')
title('Envolution of average ratio of chosen modes')
xlabel('time slot')
ylabel('average  ratio of chosen modes $\frac{1}{T} \sum_{t=0}^{T-1} \{I_m^t, I_s^t, I_d^t\}$ of the i-th mobile device', 'Interpreter','latex')


classdef prtPreProcFunction < prtPreProc
    % prtPreProcFunction   Applies a function to observations
    %
    %   FUN = prtPreProcFunction creates a pre processing object that
    %   applies a specified function to the observations.
    %
    %   The function is set as a function handle in the property
    %   "transformationFunction" The default is @(x)(x), which does
    %   nothing.
    %
    %   A prtPreProcFunction object also inherits all properties and
    %   methods from the prtAction class
    %
    %   Example:
    %
    %   dataSet = prtDataGenIris;       % Load a data set.
    %   dataSet = dataSet.retainFeatures(1:2);
    %   fun = prtPreProcFunction('transformationFunction',@(x)x.^2);
    %   fun = fun.train(dataSet);       % All prtAction's must be trained
    %   dataSetNew = fun.run(dataSet);  % Normalize the data
    %
    %   % Plot
    %   subplot(2,1,1); plot(dataSet);
    %   title('Original Dataset')
    %   subplot(2,1,2); plot(dataSetNew);
    %   title('x.^2 Dataset')
    %
    %   See Also: prtPreProc, prtPreProcPca, prtPreProcPls,
    %   prtPreProcHistEq, prtPreProcZeroMeanColumns, prtPreProcLda,
    %   prtPreProcZeroMeanRows, prtPreProcLogDisc, prtPreProcZmuv,
    %   prtPreProcMinMaxRows

    properties (SetAccess=private)
        name = 'Function'
        nameAbbreviation = 'FUN'
    end
    properties
        transformationFunction = @(x)x;
    end

    properties (Hidden)
        operateOnMatrix = false; % Set to true for faster operation, but be careful
        operateOnDataStream = false;
    end

    methods
        function self = prtPreProcFunction(varargin)
            self = prtUtilAssignStringValuePairs(self,varargin{:});
        end
    end

    methods (Access=protected,Hidden=true)

        function self = trainAction(self,ds) %#ok<INUSD>
            % Nothing to do here
        end

        function ds = runAction(self,ds)
            if self.operateOnDataStream
                ds = self.transformationFunction(ds);
            else
               ds.X = runActionFast(self,ds.X);
            end
        end

        function x = runActionFast(self,x)
            if self.operateOnDataStream
                error('Can''t use runActionFast with operateOnDataStream = true');
            end
            if self.operateOnMatrix
                x = feval(self.transformationFunction,x);

            else % operateOnRows
                xOut = repmat(self.transformationFunction(x(1,:)),size(x,1),1);
                for i = 2:size(x,1)
                    xOut(i,:) = self.transformationFunction(x(i,:));
                end
                x = xOut;

             end
        end
    end
    methods
        function self = set.transformationFunction(self,val)
            if isa(val,'char')
                % convert strings to function handles
                % might be a good idea to do this to avoid other bundling
                % the current workspace into the function used
                val = str2func(val);
            end
            assert(isa(val,'function_handle'), 'transformationFunction must be a function handle that accepts at least one input argument.');
            self.transformationFunction = val;
        end
    end

    methods (Hidden = true)

        function yOut = runStream(self,vector)
            % yOut = runStream(self,vector)
            yOut = self.transformationFunction(vector);
        end
    end
end


%%***********************************************************************
%%  Read in a problem in SDPA sparse format.
%%
%%  [blk,At,C,b] = read_sdpa(fname)
%%
%%  Input: fname = name of the file containing SDP data in
%%                 SDPA foramt.
%%  Important: the data is assumed to contain only
%%             semidefinite and linear blocks.
%%
%%***********************************************************************
%% SDPNAL+
%% Copyright (c) 2014 by
%% Liuqin Yang, Defeng Sun, and Kim-Chuan Toh
%%***********************************************************************
   function [blk,At,C,b] = read_sdpa(fname)

%%
%%  Open the file for input
%%
   compressed = 0;
   if exist(fname)
      fid = fopen(fname,'r');
   elseif exist([fname,'.Z']);
      compressed = 1;
      unix(['uncompress ',fname,'.Z']);
      fid = fopen(fname,'r');
   elseif exist([fname,'.gz']);
      compressed = 2;
      unix(['gunzip ',fname,'.gz']);
      fid = fopen(fname,'r');
   else
      fprintf(['*** Problem "',fname,'" not found, please specify the correct path or problem name. \n']);
      blk = []; At = []; C = []; b = [];
      return;
   end
%%
%%  Clean up special characters and comments from the file
%%
   [datavec,count] = fscanf(fid,'%c');
   linefeeds = findstr(datavec,char(10));
   comment_chars = '*"=';
   cumidx = [];
   for i=1:length(comment_chars)
      idx = findstr(datavec,comment_chars(i));
      cumidx = [cumidx,idx];
   end
   for j=length(cumidx):-1:1
      if (cumidx(j)==1) || (strcmp(datavec(cumidx(j)-1),char(10)))
         datavec(cumidx(j):linefeeds(min(find(cumidx(j)<linefeeds))))='';
      else
         datavec(cumidx(j):linefeeds(min(find(cumidx(j)<linefeeds)))-1)='';
      end
   end
   special_chars = ',{}()';
   cumidx=[];
   for i=1:length(special_chars)
      idx = findstr(datavec,special_chars(i));
      cumidx = [cumidx,idx];
   end
   datavec(cumidx) = blanks(length(cumidx));
   clear linefeeds;
%%
%% Close the file
%%
   fclose('all');
   if compressed==1; unix(['compress ',fname]); end;
   if compressed==2; unix(['gzip ',fname]); end;
%%
%%  Next, read in basic problem size parameters.
%%
   datavec = sscanf(datavec,'%f');
   if size(datavec,1) < size(datavec,2); datavec = datavec'; end;
   m = datavec(1);
   numblk  = datavec(2);
   blksize = datavec(2+[1:numblk]);
   if size(blksize,1) > size(blksize,2); blksize = blksize'; end
%%
%% Get input  b.
%%
   idxstrt = 2+numblk;
   b = datavec(idxstrt+[1:m]);
   idxstrt = idxstrt+m;
   b = -b;
%%
%% Construct blk
%%
   deblksize = 80;
   spblkidxtmp = find( (blksize>1) & (blksize < deblksize) );
   spblkidxtmp = sort(spblkidxtmp);
   deblkidx = find( (blksize<=1) | (blksize >= deblksize) );
   denumblk = length(deblkidx);
   linblkidx = zeros(1,denumblk);
   for p = 1:denumblk
      n = blksize(deblkidx(p));
      if (n > 1);
         blk{p,1} = 's'; blk{p,2} = n;
         n2 = n*(n+1)/2;
         At{p,1} = sparse(n2,m);
         C{p,1} = sparse(n,n);
      else
         linblkidx(p) = p;
         blk{p,1} = 'l'; blk{p,2} = abs(n);
         At{p,1} = sparse(abs(n),m);
         C{p,1} = sparse(abs(n),1);
      end
   end
   if ~isempty(spblkidxtmp)
      maxnumblk = 200;
      spnumblk = ceil(length(spblkidxtmp)/maxnumblk);
      for q = 1:spnumblk
         if (q < spnumblk)
            spblkidxall{q} = spblkidxtmp([(q-1)*maxnumblk+1: q*maxnumblk]);
         else
            spblkidxall{q} = spblkidxtmp([(q-1)*maxnumblk+1: length(spblkidxtmp)]);
         end
         tmp = blksize(spblkidxall{q});
         blk{denumblk+q,1} = 's';
         blk{denumblk+q,2} = tmp;
         n2 = sum(tmp.*(tmp+1))/2;
         At{denumblk+q,1} = sparse(n2,m);
         C{denumblk+q,1} = sparse(sum(tmp),sum(tmp));
      end
   else
      spnumblk = 0;
   end
   linblkidx(denumblk+[1:spnumblk]) = zeros(1,spnumblk);
%%
%% Construct single blocks of A,C
%%
   len = length(datavec);
   Y = reshape(datavec(idxstrt+1:len),5,(len-idxstrt)/5)';
   clear datavec;
   Y = sortrows(Y,[1 2]);
   matidx = [0; find(diff(Y(:,1)) ~= 0); size(Y,1)];
%%
   for k = 1:length(matidx)-1
      idx = [matidx(k)+1 : matidx(k+1)];
      Ytmp  = Y(idx,1:5);
      matno = Ytmp(1,1);
      Ytmp2 = Ytmp(:,2);
      for p = 1:denumblk
         n  = blksize(deblkidx(p));
         idx = find(Ytmp2 == deblkidx(p));
         ii = Ytmp(idx,3); jj = Ytmp(idx,4); vv =Ytmp(idx,5);
         len = length(idx);
         if (n > 1)
            idxtmp = find(ii > jj);
            if ~isempty(idxtmp);
               tmp = jj(idxtmp);
               jj(idxtmp) = ii(idxtmp); ii(idxtmp) = tmp;
            end
            tmp = -sparse(ii,jj,vv,n,n);
            tmp = tmp + triu(tmp,1)';
         else
            tmp = -sparse(ii,ones(len,1),vv,abs(n),1);
         end
         if (matno == 0)
            C{p,1} = tmp;
         else
            if (n > 1)
               At{p,1}(:,matno) = svec(blk(p,:),tmp,1);
            else
               At{p,1}(:,matno) = tmp;
            end
         end
      end
   end
%%
%% Construct big sparse block of A,C
%%
if (spnumblk > 0)
   Y1 = Y(:,1);
   diffY1 = find(diff([-1; Y1; inf]));
   for kk = 1:length(diffY1)-1
      idx = [diffY1(kk) : diffY1(kk+1)-1];
      matno = Y1(diffY1(kk));
      Ytmp  = Y(idx,1:5);
      Ytmp2 = Ytmp(:,2);
      maxYtmp2  = Ytmp2(length(Ytmp2));
      minYtmp2  = Ytmp2(1);
      diffYtmp2 = Ytmp2(find(diff([-1; Ytmp2])));
      for q = 1:spnumblk
         spblkidx    = spblkidxall{q};
         maxspblkidx = spblkidx(length(spblkidx));
         minspblkidx = spblkidx(1);
         count = 0;
         if (minYtmp2 <= maxspblkidx) & (maxYtmp2 >= minspblkidx)
            tmpblksize = blksize(spblkidx);
            n = sum(tmpblksize);
            cumspblksize = [0 cumsum(tmpblksize)];
            n2 = sum(tmpblksize.*(tmpblksize+1))/2;
            idx = zeros(n2,1); offset = zeros(n2,1);
            for t = [1:length(diffYtmp2)]
  	       p = find(spblkidx == diffYtmp2(t));
               if ~isempty(p)
	          idxtmp = find(Ytmp2 == spblkidx(p));
                  len = length(idxtmp);
     	          idx(count+[1:len]) = idxtmp;
                  offset(count+[1:len]) = cumspblksize(p);
                  count = count + len;
               end
            end
            idx = idx(1:count); offset = offset(1:count);
            ii = Ytmp(idx,3)+offset; jj = Ytmp(idx,4)+offset; vv = Ytmp(idx,5);
            idxtmp = find(ii > jj);
            if ~isempty(idxtmp);
               tmp = jj(idxtmp);
               jj(idxtmp) = ii(idxtmp); ii(idxtmp) = tmp;
            end
	    idxeq = find(ii==jj);
            tmp = spconvert([ii jj -vv; jj ii -vv; n n 0]) ...
                  + spconvert([ii(idxeq) jj(idxeq) vv(idxeq); n n 0]);
            if (matno == 0)
               C{denumblk+q,1} = tmp;
            else
               At{denumblk+q,1}(:,matno) = svec(blk(denumblk+q,:),tmp,1);
            end
         end
      end
   end
end
%%
%% put all linear blocks together as a single linear block
%%
   idx = find(linblkidx);
   if (length(idx) > 1)
      sdpidx = find(linblkidx==0);
      blktmp = 0; Atmp = []; Ctmp = [];
      for k = 1:length(idx)
          tmp = linblkidx(idx(k));
          blktmp = blktmp+blk{tmp,2};
          Atmp = [Atmp; At{tmp}];
          Ctmp = [Ctmp; C{tmp}];
      end
      At = At(sdpidx); C = C(sdpidx); blk = blk(sdpidx,:);
      len = length(sdpidx);
      blk(2:len+1,:) = blk;
      blk{1,1} = 'l'; blk{1,2} = blktmp;
      At(2:len+1,1) = At; C(2:len+1,1) = C;
      At{1,1} = Atmp; C{1,1} = Ctmp;
   end
%%******************************************************************


function [v,psnrall] = gapwnnm_int_fwise( y, opt )
%GAPWNNM_INT_FWISE Integrated GAP and WNNM for snapshot compressive
%imaging.
%   Note integrated version of GAP-WNNM instead of embedded for
%   acceleration of WNNM with less block-match process for multiple
%   iterations.
%   See also GAPDENOISE.
if nargin<2
    opt = [];
end
% [0] default parameter configuration, to be specified
A  = @(x) M_func(x);
At = @(z) Mt_func(z);
if isfield(opt,'Mfunc'),  A  = @(x) opt.Mfunc(x);  end
if isfield(opt,'Mtfunc'), At = @(z) opt.Mtfunc(z); end
% Phisum   = ;
% v0       = At(y); % start point (initialization of iteration)
lambda   = 1;     % correction coefficiency
maxiter  = 300;     % maximum number of iteration
acc      = 1;       % enable acceleration
sigma    = 10/255;  % noise deviation
flag_iqa = true;    % flag of showing image quality assessments
save_iter_image = false; % flag of saving all the images of the iteration
                         %  process (true only necessary)
                         %  require assigned directory opt.iter_image_dir

if isfield(opt,'Phisum'),     Phisum = opt.Phisum;   end
if isfield(opt,'v0'),             v0 = opt.v0;       end
if isfield(opt,'lambda'),   lambda   = opt.lambda;   end
if isfield(opt,'maxiter'),   maxiter = opt.maxiter;  end
if isfield(opt,'acc'),           acc = opt.acc;      end
if isfield(opt,'sigma'),       sigma = opt.sigma;    end
if isfield(opt,'flag_iqa'), flag_iqa = opt.flag_iqa; end
if isfield(opt,'save_iter_image'), save_iter_image = opt.save_iter_image; end

if ~exist('v0','var') || isempty(v0)
    v0 = At(y); % start point (initialization of iteration)
end
y1 = zeros(size(y),'like',y);
psnrall = []; % return empty with no ground truth
% ssimall = []; % return empty with no ground truth
% [1] start iteration
v = v0; % initialization
% opt.sigma = sigma;
k = 1; % current number of iteration
nonlocalarr = uint32.empty;
vindarr     = uint8.empty;
for isig = 1:length(maxiter)
    opt.sigma = sigma(isig);
    for iter = 1:maxiter(isig)
        for ii = 1:1
            % [1.1] Euclidean projection
            yb = A(v);
            if acc % enable acceleration
                y1 = y1+(y-yb);
                v = v+lambda*(At((y1-yb)./Phisum)); % v=v+lambda*(At*A)^-1*At*dy
            else
                v = v+lambda*(At((y-yb)./Phisum));
            end
        end
        % [1.2] Denoising to match the video prior
        % WNNM video denoising (MATLAB-style matrix version)
        % v = wnnm_vdenoise(v,[],opt); % opt.sigma
        noisyv = v;
        para = vdefparaconf(opt); % default parameter configuration
        para.lambda = para.vlambda; % resolve the conflict of the parameter name

        [nrow,ncol,nframe] = size(noisyv); % grayscale video (color todo)
        % [1] pre-calculation of the indexes of the neighbors within the search
        %     window
        [neighborindarr,neighbornumarr,selfindarr] = neighborind([nrow ncol],para);

        % [2] WNNM denoisng for several iterations
        estv = noisyv;
        for iit = 1:para.iternum
            % correction between adjacent iterations
            if ~para.adaptboost %
                estv = estv + para.delta*(noisyv-estv);
            else % adaptive boosting for WNNM-based denoising
                Eestv = mean(abs(estv(:)).^2);
                Enoise  = para.abeta*abs(para.nsigma^2-var(noisyv(:)-estv(:)));
                rho = sqrt(Eestv)/(sqrt(Eestv)+sqrt(max(Eestv-Enoise,0)));
                fprintf('    Iteration % 2d, rho = %.3f.\n',iit,rho);
                estv = estv + (1-rho)*(noisyv-estv);
            end

            blockmatch_period = para.blockmatch_period;
            % inner loop to reuse the block-matching results
                if mod(k,blockmatch_period) == 1 % block matching periodically
                    nonlocalarr = uint32.empty;
                    parfor (iframe = 1:nframe,nframe) % maximum nframe parpool for optimal
                        estim = estv(:,:,iframe);
                        noisyim = noisyv(:,:,iframe);
                        % [2.1] video to patches
                        [rawpatchmat,~] = im2patch(estim,noisyim,para);
                        % [2.2] calculate the patches with non-local similarity for each
                        %       key patch
                        curnonlocalarr = blockmatch(rawpatchmat,...
                            neighborindarr,neighbornumarr,selfindarr,para);
                        nonlocalarr(:,:,iframe) = curnonlocalarr;
                    end
                end
            % frame-wise denosing using parfor instead of for to get accelerated
            % performance, requiring Parallel Computing Toolbox.

            parfor (iframe = 1:nframe,nframe) % maximum nframe parpool for optimal
            % for iframe = 1:nframe % maximum nframe parpool for optimal
                estim = estv(:,:,iframe);
                noisyim = noisyv(:,:,iframe);
                % [2.1] video to patches
                [rawpatchmat,nsigmamat] = im2patch(estim,noisyim,para);
                if iit==1 % initial noise level of each patch
                    nsigmamat = para.nsigma*ones(size(nsigmamat));
                end
                curnonlocalarr = nonlocalarr(:,:,iframe);
                % [2.3] patch estimation by means of WNNM
                [estpatchmat,frqpatchmat] = patchestimate(curnonlocalarr,...
                    rawpatchmat,nsigmamat,selfindarr,para);
                % [2.4] aggregate overlapped patches to the whole image
                curim = patch2im(estpatchmat,frqpatchmat,size(noisyim),...
                    para.patchsize);

                estv(:,:,iframe) = curim;
            end

            if mod(iit-1,para.innerloop)==0
                % % [2.2] calculate the patches with non-local similarity for
                % %       each key patch
                % [nonlocalarr,vindarr] = vblockmatch(cframe,rawpatchmat,...
                %     neighborindarr,neighbornumarr,selfindarr,para);
                % less non-local patches with lower noise level
                para.patchnum = para.patchnum-10;
            end
            % [1.4] save and show intermediate results of psnr and ssim
            if flag_iqa && isfield(opt,'orig') && (~isempty(opt.orig))
                psnrall(k) = psnr(double(estv),double(opt.orig)); % record all psnr
                % ssimall(k) = ssim(double(estv),opt.orig); % record all ssim
                if (mod(k,5)==0)
                    fprintf('  GAP-%s iteration % 4d, sigma %.1f, PSNR %2.2f dB.\n',...
                        upper(opt.denoiser),k,opt.sigma*255,psnrall(k));
                end
                if save_iter_image % save all the images of the iteration process
                    MAXB = opt.MAXB;
                    % save each frame in the assigned directory
                    nim = size(estv,ndims(estv)); % number of frames in the video
                    for iim = 1:nim
                        im = estv(:,:,iim);
                        impsnr = psnr(im,opt.orig(:,:,iim));
                        imssim = ssim(im,opt.orig(:,:,iim));
                        if k == 1 % mkdir for the first iteration
                            subdir = sprintf('%s/frame%02d',opt.iter_image_dir,iim); % frame-wise
                            if ~exist(subdir,'dir')
                                mkdir(subdir);
                            end
                        end
                        imwrite(uint8(im*MAXB),sprintf('%s/frame%02d/frame%02d_iter%03d_sigma%.1f_psnr%2.2f_ssim%.4f.png',...
                            opt.iter_image_dir,iim,iim,k,opt.sigma*MAXB,impsnr,imssim));
                    end
                end
            end
            k = k+1;
        end % WNNM loop [iternum]
        v = estv;
        % % [1.3] update noise standard deviation
        % nsigma = eta*sqrt(abs(sigma^2-var(v(:)-v0(:))));
        % opt.sigma = nsigma;

    end % GAP loop [maxiter]
end % sigma loop [length(sigma)]

end


classdef ipfSpotKey < ipfColorKey
%
% Colorizes single spots in the inverse pole figure with individual colors.
%
% Syntax
%   ipfKey = ipfSpotKey
%   ipfKey.inversePoleFigureDirection = zvector;
%   ipfKey.center = Miller(1,0,0,cs); % the centers of the spots in the inverse pole figure
%   ipfKey.color = [0 0 1];           % the color of the spots
%   ipfKey.psi = S2DeLaValleePoussinKernel('halfwidth',7.5*degree);
%
%   color = ipfKey.orientation2color(ori)
%
% Class Properties
%  center  - list of crystal directions @Miller
%  color   - n-by-3 list representing RGB values, one for each center
%  psi     - @S2Kernel providing the width and brightness for colored fibre
%  inversePoleFigureDirection - specimen direction @vector3d
%
% See also
% EBSDAdvancedMapping

  properties
    center % list of crystal directions @Miller
    color  % list of RGB values, one for each center
    psi    % @2Kernel providing the width and brightness for colored fibre
  end

  methods
    function oM = ipfSpotKey(varargin)
      oM = oM@ipfColorKey(varargin{:});

      oM.center = get_option(varargin,'center',Miller(0,0,1,oM.CS1));
      oM.CS1 = oM.center.CS;
      oM.color = get_option(varargin,'color',[1 0 0]);
      oM.psi = get_option(varargin,'S2Kernel',...
        S2DeLaValleePoussinKernel('halfwidth',get_option(varargin,'halfwidth',10*degree)));

      oM.dirMap = directionColorKey(oM.CS1,'dir2color',@(varargin) oM.dir2color(varargin{:}));

    end

    function rgb = dir2color(oM,h)

      h = normalize(h);
      rgb = ones(length(h),3);

      for k=1:length(oM.center)

        w = oM.psi.eval(dot(h,normalize(oM.center(k)))) ./ oM.psi.eval(1);

        if ~any(oM.color(k,:))% fix in case of black
          cdata = repmat([0 1 1],length(h),1);
          cdata(:,2) = w(:).*cdata(:,2);
          cdata = reshape(hsv2rgb(cdata),[],3);
          cdata(:,1)=cdata(:,2);
          rgb = rgb.*cdata;
        else
          cdata = rgb2hsv(repmat(oM.color(k,:),length(h),1));
          cdata(:,2) = w(:).*cdata(:,2);
          cdata(:,3) = 1;
          cdata = reshape(hsv2rgb(cdata),[],3);
          rgb = rgb.*cdata;
        end
      end
    end
  end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   ABB IRB140.
%
%   Author: Arturo Gil. Universidad Miguel Hernandez de Elche.
%   email: arturo.gil@umh.es date:   09/01/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.
function robot = parameters()

robot.name= 'Epson_Prosix_VT6';
robot.path = 'robots/Epson/Prosix_VT6';

robot.DH.theta= '[q(1)+pi q(2)-pi/2 q(3) q(4) q(5) q(6)]';
robot.DH.d='[0.412 0 0 0.400 0 0.080]';
robot.DH.a='[0.1 0.420 0 0 0 0]';
robot.DH.alpha= '[-pi/2 0 -pi/2 pi/2 -pi/2 0]';
robot.J=[];

robot.inversekinematic_fn = 'inversekinematic_Prosix_VT6(robot, T)';
robot.directkinematic_fn = 'directkinematic(robot, q)';

%number of degrees of freedom
robot.DOF = 6;

%rotational: 0, translational: 1
robot.kind=['R' 'R' 'R' 'R' 'R' 'R'];

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-170) deg2rad(170); %Axis 1, minimum, maximum
                deg2rad(-160) deg2rad(65); %Axis 2, minimum, maximum
                deg2rad(-51) deg2rad(190); %Axis 3
                deg2rad(-200) deg2rad(200); %Axis 4: Unlimited (400� default)
                deg2rad(-125) deg2rad(125); %Axis 5
                deg2rad(-360) deg2rad(360)]; %Axis 6: Really Unlimited to (800� default)

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(183.7); %Axis 1, rad/s
                deg2rad(122.5); %Axis 2, rad/s
                deg2rad(118.8); %Axis 3, rad/s
                deg2rad(271.4); %Axis 4, rad/s
                deg2rad(296.8); %Axis 5, rad/s
                deg2rad(296.8)];%Axis 6, rad/s

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time

% end effectors maximum velocity
robot.linear_velmax = 2.5; %m/s

%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);
robot.path = pwd;

% GRAPHICS
robot.graphical.has_graphics=1;
robot.graphical.color = [94 104 245]./255;
%for transparency
robot.graphical.draw_transparent=30;
%draw DH systems
robot.graphical.draw_axes=1;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=0.5;
%adjust for a default view of the robot
robot.axis=[-2 2 -2 5 0 2];
%read graphics files
robot = read_graphics(robot);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DYNAMIC PARAMETERS
%   WARNING! These parameters do not correspond to the actual IRB 140
%   robot. They have been introduced to demonstrate the necessity of
%   simulating the robot and should be used only for educational purposes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
robot.has_dynamics=1;

%consider friction in the computations
robot.dynamics.friction=0;

%link masses (kg)
robot.dynamics.masses=[16.1 17.16 9.18 6.12 1.53 0.9];

%COM of each link with respect to own reference system
robot.dynamics.r_com=[0       0          0; %(rx, ry, rz) link 1
                     -0.05	 0.006	 0.1; %(rx, ry, rz) link 2
                    -0.0203	-0.0141	 0.070;  %(rx, ry, rz) link 3
                     0       0.019       0;%(rx, ry, rz) link 4
                     0       0           0;%(rx, ry, rz) link 5
                     0       0         0.032];%(rx, ry, rz) link 6

%Inertia matrices of each link with respect to its D-H reference system.
% Ixx	Iyy	Izz	Ixy	Iyz	Ixz, for each row
robot.dynamics.Inertia=[0      0.35	0   	0	0	0;
    .13     .524	.539	0	0	0;
    .066	.086	.0125	0	0	0;
    1.8e-3	1.3e-3	1.8e-3	0	0	0;
    .3e-3	.4e-3	.3e-3	0	0	0;
    .15e-3	.15e-3	.04e-3	0	0	0];

robot.motors=load_motors([5 5 5 4 4 4]);
%Speed reductor at each joint
% my values are
% G=[230 450 20 1 5 1]
robot.motors.G=[300 300 300 300 300 300];


%ADDPOSTPROC Add mapppings in POSTPROC field of a datafile
%
%   A = ADDPOSTPROC(A,MAPPING)
%   A = ADDPOSTPROC(A)
%
% INPUT
%   A        - Datafile
%   POSTPROC - Postprocessing mapping command
%
% OUTPUT
%   A       - Datafile
%
% DESCRIPTION
% Extends the set of mappings stored in A.POSTPROC.
% Existing mappings are extended sequentially:
%
%      A.POSTPROC = A.POSTPROC * MAPPING
%
% Mappings in A.POSTPROC are stored only and executed just
% after A is converted from a DATAFILE into a PRDATASET.
% The feature size of the datafile A is reset to the output
% size of PRMAPPING.
% The POSTPROC field of A can be reset by SETPOSTPROC.
%
% SEE ALSO (PRTools Guide)
% DATAFILES, SETPREPROC, SETPOSTPROC


function R = slsumstat_blks(data, statfunc, varargin)
%SLSUMSTAT_BLKS Sums up statistics on all blocks for partitioned data
%
% $ Syntax $
%   - R = slsumstat_blks(data, statfunc, ...)
%
% $ Arguments $
%   - data:         the data array of cell array of data array filenames
%   - statfunc:     the function to compute statistics on data array
%   - R:            the sum statistics
%
% $ Description $
%   - R = slsumstat_blks(data, statfunc, ...) computes statistics on
%     all data blocks and sums them by plus. When data is an array,
%     it just invokes statfunc on it and return, if data is a cell array
%     of filenames, it loads data of each block, computes statistics
%     blockwise and sums them up to give the final result.
%
% $ Remarks $
%   - The function is widely applicable for diverse types of computation.
%     The only conditions is that the function values on the whole matrix
%     is equivalent to the sum of function values on all blocks.
%
%   - The function values can be either a scalar or an array of any
%     dimensions, provided that the values produced on all blocks have
%     equal size.
%
% $ History $
%   - Created by Dahua Lin, on Aug 8th, 2006
%

%% parse and verify input arguments

if nargin < 2
    raise_lackinput('slsumstat_blks', 2);
end
if ~isnumeric(data) && ~iscell(data)
    error('sltoolbox:invalidargs', ...
        'data should be either a numeric array or a cell array of strings');
end

%% compute

if isnumeric(data)
    R = feval(statfunc, data, varargin{:});
else
    n = numel(data);

    % first section
    curdata = slreadarray(data{1});
    R = feval(statfunc, curdata, varargin{:});

    % other section
    if n > 1
        for i = 2 : n
            curdata = slreadarray(data{i});
            curR = feval(statfunc, curdata, varargin{:});
            R = R + curR;
        end
    end
end


% procedure: use draw_anatomical_ROI_2008 to draw saggital slices
% then mask with cortical ribbon from segmentation
% then clean up using draw...

cl = mask2clusters('insula_ribbon_R.img');
cl = mask2clusters('SPM8_insula_ribbon_L.img');
mask_union([],'SPM8_insula_ribbon_LR.img','SPM8_insula_ribbon_L.img','insula_ribbon_R.img');
dat = fmri_data('SPM8_insula_ribbon_LR.img'); orthviews(dat)

dat = apply_mask(dat, fmri_data('SPM8_colin27T1_cortical_ribbon.img'));
orthviews(dat)

dat.fullpath = fullfile(pwd, 'insula_ribbon1.img');
write(dat);

%% clean up

draw_anatomical_roi_2008('load', 'insula_ribbon1.img')
draw_anatomical_roi_2008('moveslice')
draw_anatomical_roi_2008('write')
SPM8_insula_ribbon_LR.img

% smooth and threshold
dat = fmri_data('SPM8_insula_ribbon_LR.img')
dat = preprocess(dat, 'smooth', 1);
dat = threshold(dat, [.02 Inf], 'raw-between');
orthviews(dat)

%%
% viz
cl = mask2clusters('SPM8_insula_ribbon_LR.img');

cluster_orthviews(cl, {[1 .7 0]});
cluster_orthviews(cl, {[1 .7 0]}, 'add');

spm_orthviews('xhairs', 'off');
delete(findobj(gcf, 'Type', 'text'));

%%
dat = fmri_data('SPM8_insula_ribbon_LR.img')
orthviews(dat)
surface(dat)


% AUTHOR:         Aaron Nicolson
% AFFILIATION:    Signal Processing Laboratory, Griffith University
%
% This Source Code Form is subject to the terms of the Mozilla Public
% License, v. 2.0. If a copy of the MPL was not distributed with this
% file, You can obtain one at http://mozilla.org/MPL/2.0/.

clear all; close all; clc

ver = {''};

for i = 1:length(ver)
    T = readtable(['./iter/', ver{i}, '.csv']);
    iter = 1:height(T);
    plot(iter, T.loss); hold on;
    xlabel('Iteration');
    ylabel('Loss');
end
legend(ver);
hold off;


function [eta,etaf]=finishat(frac,tol,fmt)
%FINISHAT print estimated finish time of a long computation (FRAC,TOL,FMT)
% Usage: (1)  finishat(0);
%              for i=1:many
%                  ... computation ...
%                  finishat(i/many);
%              end
%
%         (2)  finishat(0);
%              for i=1:NI
%                  for j=1:NJ
%                      for k=1:NK
%                          ... computation ...
%                          finishat((((i-1)*NJ+j-1)*NK+k)/(NI*NJ*NK));
%                      end
%                  end
%              end
%
% Inputs: FRAC = fraction of total comutation that has been completed
%                As a special case, 0 is used to initialize the routine
%         TOL  = Tolerance in minutes. If the estimated time has changed by less
%                than this, then nothing will be printed. [default 10% of remaining time]
%         FMT  = Format string which should include %s for estimated finish time, %d for remaining minutes and %f for fraction complete
%
% Output: ETA  = string containing the expected finish time
%                specifying this will suppress printing message to std err (fid=2)
%         ETAF = expected finish time as a daynumber
%
% Example:       finishat(0);
%                for i=1:many
%                    long computation;
%                    finishat(i/many);
%                end

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: finishat.m 8893 2016-10-27 16:52:30Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program 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 2 of the License, or
%   (at your option) any later version.
%
%   This program 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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

persistent oldt oldnw
if nargin<3
    fmt='Estimated finish at %s (%.2f done, %d min remaining)\n';

end
if frac<=0
    oldt=0;
    eta='Unknown';
    tic;
else
    nw=now;                             % current time as serial number
    sectogo=(1/frac-1)*toc;      % seconds to go
    newt=nw+sectogo/86400;       % add estimated time in days
    if nargin<2 || ~numel(tol)
        tol=max(0.1*(newt-nw)*1440,1);
    end
    if ~exist('oldt','var') || oldt==0 || (abs(newt-oldt)>tol/1440 && (nw-oldnw)>10/86400) || (nw-oldnw)>10/1440 || nargout>0
        oldt=newt;
        if floor(oldt)==floor(nw)
            df='HH:MM';
        else
            df='HH:MM dd-mmm-yyyy';
        end
        eta=datestr(oldt,df);
        if ~nargout
            ix=find(fmt=='%',1);
            while ~isempty(ix)
                fprintf(2,fmt(1:ix-1));
                fmt=fmt(ix:end);
                ix=find(fmt>='a' & fmt<='z',1); % find letter
                switch fmt(ix)
                    case 's'
                        fprintf(2,fmt(1:ix),eta);
                    case 'd'
                        fprintf(2,fmt(1:ix),round(sectogo/60));
                    case 'f'
                        fprintf(2,fmt(1:ix),frac);
                end
                fmt=fmt(ix+1:end);
                ix=find(fmt=='%',1);
            end
            fprintf(2,fmt);
        end
        oldnw=nw;                           %
    end
end
etaf=oldt;


function [net, info] = train_DCFNet(varargin)
%Training DCFNet
training_env();
opts.networkType = 12;
opts.lossType = 1;
opts.inputSize = 125;
opts.padding = 2; % padding size for crop
opts.gpus = 1;
[opts] = vl_argparse(opts, varargin) ;

opts.expDir = fullfile('..', 'data', sprintf('DCFNet-net-%d-%d-%1.1f', opts.networkType, opts.inputSize, opts.padding)) ;
opts.imdbPath = fullfile('..', 'data', sprintf('imdb-vid2015-DCFNet-%d-%1.1f', opts.inputSize, opts.padding), 'imdb.mat');

trainOpts.momentum = 0.9;
trainOpts.weightDecay = 0.0005;
trainOpts.numEpochs = 50;
trainOpts.learningRate = logspace(-2, -5, trainOpts.numEpochs) ; % from SiameseFC
trainOpts.batchSize = 32;
trainOpts.gpus = [opts.gpus]; %only support single gpu
opts.train = trainOpts;

if ~isfield(opts.train, 'gpus')
    opts.train.gpus = [];
elseif numel(opts.train.gpus) ~=0
    gpuDevice(opts.train.gpus);
end

% --------------------------------------------------------------------
%                                                 Prepare net and data
% --------------------------------------------------------------------
net = init_DCFNet('networkType', opts.networkType,...
    'inputSize', opts.inputSize, 'padding', opts.padding);

if exist(opts.imdbPath, 'file')
    imdb = load(opts.imdbPath) ;
else
    imdb = getImdbDCFNet('output_size', opts.inputSize,...
        'padding', opts.padding) ;
    if ~exist(fileparts(opts.imdbPath), 'dir')
        mkdir(fileparts(opts.imdbPath));
    end
    save(opts.imdbPath, '-v7.3', '-struct', 'imdb') ;
end

% --------------------------------------------------------------------
%                                                                Train
% --------------------------------------------------------------------
[net, info] = cnn_train_dag(net, imdb, getBatch(opts), ...
    'expDir', opts.expDir, opts.train, 'val', find(imdb.images.set == 2)) ;

transition_net('expDir', opts.expDir);
transition_net_multi('expDir', opts.expDir);
end

% --------------------------------------------------------------------
function fn = getBatch(opts)
% --------------------------------------------------------------------
bopts = struct('numGpus', numel(opts.train.gpus), 'batchSize', opts.train.batchSize, 'maxStep', 10) ;
fn = @(x,y) getDagNNBatch(bopts,x,y) ;
end

% --------------------------------------------------------------------
function inputs = getDagNNBatch(opts, imdb, batch)
% --------------------------------------------------------------------
rand_next = randi([1, opts.maxStep], size(batch));
rand_next = min(rand_next, imdb.images.up_index(batch));

if opts.numGpus > 0
    target = gpuArray(single(imdb.images.images(:,:,:,batch)));
    search = gpuArray(single(imdb.images.images(:,:,:,batch+rand_next)));
else
    target = single(imdb.images.images(:,:,:,batch));
    search = single(imdb.images.images(:,:,:,batch+rand_next));
end
target = bsxfun(@minus, target, imdb.images.data_mean);
search = bsxfun(@minus, search, imdb.images.data_mean);
inputs = {'target', target, 'search', search} ;
end


% UNDER construction...
%
%
%
% Usage
% >> waverage = stdbin(ERP, bop)
%
% For Bin Operations GUI/scripting purposes
%
% Author: Javier
%

function  standard = stdbin(ERP, bop)

if nargin<1
        help stdbin
        return
end
if nargin<2
        error('ERPLAB says: You must specify bin indexes to be processed!')
end
if ~iserpstruct(ERP)
        error('ERPLAB says: Your data structure is not an ERP structure!')
end
if ischar(bop)
        bop = str2num(char(regexp(bop,'\d+','match')'))';
end
binarray = unique_bc2(bop);
if length(binarray)~=length(bop)
        fprintf('\n*** WARNING: Repeated bins were ignored.\n\n')
end
if length(binarray)<2
        error('ERPLAB says: You must specify 2 bin indexes at leat!')
end
if max(binarray)>ERP.nbin
        error('ERPLAB says: Some specified bins do not exist!')
end
datavg   = ERP.bindata(:,:,binarray);
standard = std(datavg, 0, 3);


function [Tshed,nShedsT] = BSLwatershed(regionPT,maskPT)
%"BSLwatershed"
%   BSL subfunction -- Returns watersheds for BSL estimate and the total
%   number of non-zero watersheds
%
% CRS 05/20/13
%
%Usage:
%   [Tshed,nShedsT] = BSLwatershed(regionPT,maskPT)
%       regionPT = subregion from PET images in SUV
%       maskPT   = VOI mask for restricting the watersheds
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.%
%
%% Smooth region to limit WS over-segmentation
smoothPT = fspecial('gaussian', 9, 3);
bPT = imfilter(regionPT,smoothPT,'replicate','same','conv');
bPT(bPT < 0) = 0;

%% Build 2D Watershed Transform of Region
Tshed = zeros(size(regionPT));
for i = 1:size(regionPT,3)

    % get slices
    PTtemp  = regionPT(:,:,i);
    bPTtemp = bPT(:,:,i);

    % get sheds
    % performs a 2D WS on an upsampled gradient image to remove boundary
    % elements
    hy = fspecial('sobel'); hx = hy';
    DelY = imfilter(double(PTtemp), hy, 'replicate');
    DelX = imfilter(double(PTtemp), hx, 'replicate');
    gradmagS = imresize(sqrt(DelX.^2 + DelY.^2),32*size(bPTtemp),'bicubic');
    Ls = double(watershed(gradmagS,8));
    % Down Samples the WS image to reduce the number of boundary elements
    L = imresize(Ls,size(bPTtemp),'nearest');

    %% Detemines the number of sheds for memory allocation
    Shed = []; ShedVal = []; meanShed = [];
    k = 0;
    for j = 1:max(L(:));
        if ( numel(find(maskPT(:,:,i).*L == j)) ~= 0 )
            k = k + 1;
        end
    end
    nSheds(i) = k;
    Shed = zeros([ size(PTtemp) nSheds(i)]);

    % reorders sheds from low to highest mean SUV and builds masks
    k = 0;
    for j = 1:max(L(:));
        if ( numel(find(maskPT(:,:,i).*L == j)) ~= 0 )
            k = k + 1;
            mShed = double(L);
            mShed(mShed ~= j) = 0;
            mShed(mShed > 0) = 1;
            Shed(:,:,k) = mShed.*PTtemp;
            ShedVal(k) = mean(nonzeros(Shed(:,:,k)));
            meanShed(:,:,k) = mShed*ShedVal(k);
        end
    end
    [~,Ival] = sort(ShedVal);
    Shed = Shed(:,:,Ival);
    meanShed = meanShed(:,:,Ival);

    % bulds a total shed from the means of each WS
    TmpShed = zeros(size(L));
    for j = 1:nSheds(i), TmpShed = meanShed(:,:,j) + TmpShed; end
    TmpShed1 = TmpShed;

    %% Unclassified voxel assignment
    % Finds unclassified voxels
    Cracks = double(L);
    Cracks(Cracks ~= 0) = 0.5; Cracks(Cracks == 0) = 1; Cracks(Cracks < 1)  = 0;
    iCrack = find(Cracks); Cracks = Cracks.*PTtemp;
    % Fill in unclassified voxels using the nearest mean shed value from
    % NEWS (k < 3) avoids infinite loop
    k = 0;
    while (sum(Cracks(:)) > 0  && k < 3)
        k = k + 1;
        [mSize, nSize] = size(L);
        for j = 1:numel(iCrack)
            q = floor( ( iCrack(j)-1 ) / mSize ) + 1;
            p = mod(iCrack(j),mSize);
            if (p == 0), p = mSize; end
            jT = p-1 + (q-1)*mSize; jB = p+1 + (q-1)*mSize;
            jL = p   + (q-2)*mSize; jR = p   + (q)*mSize;

            iVal = []; jCross = [];
            if (p > 1 && p < mSize)
                if (q > 1 && q < nSize), jCross = [ jT jB jL jR ];
                else
                    if (q > 1), jCross = [ jT jB jL ];
                    else jCross = [ jT jB jR ];
                    end
                end
            else
                if (p > 1)
                    if (q > 1 && q < nSize), jCross = [ jT jL jR ];
                    else
                        if (q > 1), jCross = [ jT jL ];
                        else jCross = [ jT jR ];
                        end
                    end
                else
                    if (q > 1), jCross = [ jB jL ];
                    else jCross = [ jB jR ];
                    end
                end
            end
            [~, iVal] = min(abs( TmpShed(jCross) - Cracks(iCrack(j)) ));
            if (TmpShed(jCross(iVal)) == 0)
                TmpShed1(iCrack(j)) = Cracks(iCrack(j));
            else
                TmpShed1(iCrack(j)) = TmpShed(jCross(iVal));
            end
        end
        TmpShed = TmpShed1;
        Cracks = TmpShed;
        Cracks(Cracks ~= 0) = 0.5; Cracks(Cracks == 0) = 1; Cracks(Cracks < 1)  = 0;
        iCrack = find(Cracks); Cracks = Cracks.*PTtemp;
    end
    Tshed(:,:,i) = TmpShed1;

end
nShedsT = sum(nSheds);


classdef ComplianceVoigt2TensorConverterPS < FourthOrderVoigt2TensorConverterPS

    properties
    end

    methods (Access = public)

        function obj = ComplianceVoigt2TensorConverterPS(tensor)
           obj.computeConversion(tensor)
        end

    end

    methods (Access = protected,Static)

        function f = getVoigtFactor(iv,jv)
            if ((iv > 2 && jv <= 2) )  || ((iv <= 2 && jv > 2) )
                f = 2;
            elseif (iv>2 && jv>2)
                f = 4;
            else
                f = 1;
            end
        end

    end

    methods (Access = protected)

        function selectTensorClass(obj)
            obj.tensor = CompliancePlaneStressTensor();
        end

    end

end


%%*****************************************************************
%% randmat: generate an mxn matrix using matlab's
%%          rand or randn functions using state = k.
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

function v = randmat(m,n,k,randtype)

try
   s = rng;
   rng(k);
   if strcmp(randtype,'n')
      v = randn(m,n);
   elseif strcmp(randtype,'u')
      v = rand(m,n);
   end
   rng(s);
catch
   if strcmp(randtype,'n')
      s = randn('state');
      randn('state',k);
      v = randn(m,n);
      randn('state',s);
   elseif strcmp(randtype,'u')
      s = rand('state');
      rand('state',k);
      v = rand(m,n);
      rand('state',s);
   end
end
%%******************************************************