function [Eft1, Eft2, Covf, lpyt] = pred_coxph(gp, x, y, xt, varargin)
% PRED_COXPH Wrapper for returning useful values for coxph likelihood
%
%  Description
%     [EF1, EF2, COVF, LPYT] = PRED_COXPH(GP,X,Y,XT, OPTIONS)
%     Returns predictions for the two latent processes EF1 and EF2,
%     covariance matrix COVF and logarithms of predictive densities at XT.

% Copyright (c) 2012-2013 Ville Tolvanen

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

ip=inputParser;
ip.FunctionName = 'PRED_COXPH';
ip.addRequired('gp',@isstruct);
ip.addRequired('x', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addRequired('y', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addRequired('xt',  @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addParamValue('yt', [], @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('z', [], @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('zt', [], @(x) isreal(x) && all(isfinite(x(:))))
ip.addParamValue('predcf', [], @(x) isempty(x) || ...
                 isvector(x) && isreal(x) && all(isfinite(x)&x>0))
ip.addParamValue('tstind', [], @(x) isempty(x) || iscell(x) ||...
                 (isvector(x) && isreal(x) && all(isfinite(x)&x>0)))
ip.parse(gp, x, y, xt, varargin{:});

if ~strcmp(gp.lik.type, 'Coxph')
  error('Likelihood not Coxph')
end
if nargout > 3
  if numel(gp.jitterSigma2)==1
    [Ef, Covf, lpyt] = gp_pred(gp, x, y, xt, varargin{:});
  else
    [Ef, Covf, lpyt] = gpmc_preds(gp, x, y, xt, varargin{:});
  end
else
  if numel(gp.jitterSigma2)==1
    [Ef, Covf] = gp_pred(gp, x, y, xt, varargin{:});
  else
    [Ef, Covf] = gpmc_preds(gp, x, y, xt, varargin{:});
  end
end
ntime=size(gp.lik.xtime,1);
if isfield(gp.lik, 'stratificationVariables')
  ind_str=gp.lik.stratificationVariables;
  nf1=ntime.*unique([x(:,ind_str); xt(:,ind_str)], 'rows');
else
  nf1=ntime;
end

Eft1 = Ef(1:nf1,:); Ef(1:nf1,:) = []; Eft2 = Ef;

end


function t_miqps_master(quiet)
%T_MIQPS_MASTER  Tests of MILP/MIQP solvers via MIQPS_MASTER().

%   MP-Opt-Model
%   Copyright (c) 2010-2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MP-Opt-Model.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/mp-opt-model for more info.

if nargin < 1
    quiet = 0;
end

algs = {'DEFAULT', 'CPLEX', 'MOSEK', 'GUROBI', 'GLPK', 'OT'};
names = {'DEFAULT', 'CPLEX', 'MOSEK', 'Gurobi', 'glpk', 'intlin/lin/quadprog'};
check = {@have_miqp_solver, 'cplex', 'mosek', 'gurobi', 'glpk', 'intlinprog'};
does_qp = [0 1 1 1 0 0];
if have_feature('gurobi') || have_feature('cplex') || have_feature('mosek')
    does_qp(1) = 1;
end

n = 48;
nqp = 28;
nmiqp = 6;
t_begin(n*length(algs), quiet);

diff_alg_warn_id = 'optim:linprog:WillRunDiffAlg';
if have_feature('quadprog') && have_feature('quadprog', 'vnum') == 7.005
    s1 = warning('query', diff_alg_warn_id);
    warning('off', diff_alg_warn_id);
end

for k = 1:length(algs)
    if ~isempty(check{k}) && ...
            (ischar(check{k}) && ~have_feature(check{k}) || ...
             isa(check{k}, 'function_handle') && ~check{k}())
        t_skip(n, sprintf('%s not installed', names{k}));
    else
        opt = struct('verbose', 0, 'alg', algs{k});
        mpopt = struct( ...
            'verbose', 0, ...
            'opf', struct( ...
                'violation', 5e-6 ), ...
            'cplex', struct( ...
                'lpmethod', 0, ...
                'qpmethod', 0, ...
                'opt', 0 ), ...
            'mosek', struct( ...
                'lp_alg', 0, ...
                'max_it', 0, ...
                'gap_tol', 0, ...
                'max_time', 0, ...
                'num_threads', 0, ...
                'opt', 0 ) ...
        );
        switch names{k}
            case 'CPLEX'
%               alg = 0;        %% default uses barrier method with NaN bug in lower lim multipliers
                alg = 2;        %% use dual simplex
                mpopt.cplex.lpmethod = alg;
                mpopt.cplex.qpmethod = min([4 alg]);
                opt.cplex_opt = cplex_options([], mpopt);
            case 'MOSEK'
%                 sc = mosek_symbcon;
%                 alg = sc.MSK_OPTIMIZER_DUAL_SIMPLEX;    %% use dual simplex
%                 alg = sc.MSK_OPTIMIZER_INTPNT;          %% use interior point
%                 mpopt.mosek.lp_alg = alg;
                mpopt.mosek.gap_tol = 1e-10;
%                 mpopt.mosek.opts.MSK_DPAR_INTPNT_TOL_PFEAS = 1e-10;
%                 mpopt.mosek.opts.MSK_DPAR_INTPNT_TOL_DFEAS = 1e-10;
%                 mpopt.mosek.opts.MSK_DPAR_INTPNT_TOL_INFEAS = 1e-10;
%                 mpopt.mosek.opts.MSK_DPAR_INTPNT_TOL_REL_GAP = 1e-10;
                vnum = have_feature('mosek', 'vnum');
                if vnum >= 8
%                     mpopt.mosek.opts.MSK_DPAR_INTPNT_QO_TOL_PFEAS = 1e-10;
%                     mpopt.mosek.opts.MSK_DPAR_INTPNT_QO_TOL_DFEAS = 1e-10;
%                     mpopt.mosek.opts.MSK_DPAR_INTPNT_QO_TOL_INFEAS = 1e-10;
%                     mpopt.mosek.opts.MSK_DPAR_INTPNT_QO_TOL_MU_RED = 1e-10;
                    mpopt.mosek.opts.MSK_DPAR_INTPNT_QO_TOL_REL_GAP = 1e-10;
                end
%                 opt.verbose = 3;
                opt.mosek_opt = mosek_options([], mpopt);
        end

        t = sprintf('%s - 3-d LP : ', names{k});
        %% based on example from 'doc linprog'
        c = [-5; -4; -6];
        A = [1 -1  1;
             -3  -2  -4;
             3  2  0];
        l = [-Inf; -42; -Inf];
        u = [20; Inf; 30];
        xmin = [0; 0; 0];
        x0 = [];
        [x, f, s, out, lam] = miqps_master([], c, A, l, u, xmin, [], [], [], opt);
        t_is(s, 1, 12, [t 'success']);
        t_is(x, [0; 15; 3], 6, [t 'x']);
        t_is(f, -78, 6, [t 'f']);
        t_is(lam.mu_l, [0;1.5;0], 9, [t 'lam.mu_l']);
        t_is(lam.mu_u, [0;0;0.5], 9, [t 'lam.mu_u']);
        t_is(lam.lower, [1;0;0], 9, [t 'lam.lower']);
        t_is(lam.upper, zeros(size(x)), 9, [t 'lam.upper']);

        if does_qp(k)
            t = sprintf('%s - unconstrained 3-d quadratic : ', names{k});
            %% from http://www.akiti.ca/QuadProgEx0Constr.html
            H = [5 -2 -1; -2 4 3; -1 3 5];
            c = [2; -35; -47];
            x0 = [0; 0; 0];
            [x, f, s, out, lam] = miqps_master(H, c, [], [], [], [], [], [], [], opt);
            t_is(s, 1, 12, [t 'success']);
            t_is(x, [3; 5; 7], 7, [t 'x']);
            t_is(f, -249, 11, [t 'f']);
            t_ok(isempty(lam.mu_l), [t 'lam.mu_l']);
            t_ok(isempty(lam.mu_u), [t 'lam.mu_u']);
            t_is(lam.lower, zeros(size(x)), 13, [t 'lam.lower']);
            t_is(lam.upper, zeros(size(x)), 13, [t 'lam.upper']);

            t = sprintf('%s - constrained 2-d QP : ', names{k});
            %% example from 'doc quadprog'
            H = [   1   -1;
                    -1  2   ];
            c = [-2; -6];
            A = [   1   1;
                    -1  2;
                    2   1   ];
            l = [];
            u = [2; 2; 3];
            xmin = [0; 0];
            x0 = [];
            [x, f, s, out, lam] = miqps_master(H, c, A, l, u, xmin, [], x0, [], opt);
            t_is(s, 1, 12, [t 'success']);
            t_is(x, [2; 4]/3, 7, [t 'x']);
            t_is(f, -74/9, 6, [t 'f']);
            t_is(lam.mu_l, [0;0;0], 13, [t 'lam.mu_l']);
            t_is(lam.mu_u, [28;4;0]/9, 4, [t 'lam.mu_u']);
            t_is(lam.lower, zeros(size(x)), 7, [t 'lam.lower']);
            t_is(lam.upper, zeros(size(x)), 13, [t 'lam.upper']);

            t = sprintf('%s - constrained 4-d QP : ', names{k});
            %% from https://v8doc.sas.com/sashtml/iml/chap8/sect12.htm
            H = [   1003.1  4.3     6.3     5.9;
                    4.3     2.2     2.1     3.9;
                    6.3     2.1     3.5     4.8;
                    5.9     3.9     4.8     10  ];
            c = zeros(4,1);
            A = [   1       1       1       1;
                    0.17    0.11    0.10    0.18    ];
            l = [1; 0.10];
            u = [1; Inf];
            xmin = zeros(4,1);
            x0 = [1; 0; 0; 1];
            [x, f, s, out, lam] = miqps_master(H, c, A, l, u, xmin, [], x0, [], opt);
            t_is(s, 1, 12, [t 'success']);
            t_is(x, [0; 2.8; 0.2; 0]/3, 5, [t 'x']);
            t_is(f, 3.29/3, 6, [t 'f']);
            t_is(lam.mu_l, [6.58;0]/3, 6, [t 'lam.mu_l']);
            t_is(lam.mu_u, [0;0], 13, [t 'lam.mu_u']);
            t_is(lam.lower, [2.24;0;0;1.7667], 4, [t 'lam.lower']);
            t_is(lam.upper, zeros(size(x)), 13, [t 'lam.upper']);

            t = sprintf('%s - (struct) constrained 4-d QP : ', names{k});
            p = struct('H', H, 'A', A, 'l', l, 'u', u, 'xmin', xmin, 'x0', x0, 'opt', opt);
            [x, f, s, out, lam] = miqps_master(p);
            t_is(s, 1, 12, [t 'success']);
            t_is(x, [0; 2.8; 0.2; 0]/3, 5, [t 'x']);
            t_is(f, 3.29/3, 6, [t 'f']);
            t_is(lam.mu_l, [6.58;0]/3, 6, [t 'lam.mu_l']);
            t_is(lam.mu_u, [0;0], 13, [t 'lam.mu_u']);
            t_is(lam.lower, [2.24;0;0;1.7667], 4, [t 'lam.lower']);
            t_is(lam.upper, zeros(size(x)), 13, [t 'lam.upper']);
        else
            t_skip(nqp, sprintf('%s does not handle QP problems', names{k}));
        end

        t = sprintf('%s - infeasible LP : ', names{k});
        p = struct('A', sparse([1 1]), 'c', [1;1], 'u', -1, 'xmin', [0;0], 'opt', opt);
        [x, f, s, out, lam] = miqps_master(p);
        t_ok(s <= 0, [t 'no success']);

% opt.verbose = 3;
        t = sprintf('%s - 2-d MILP : ', names{k});
        %% from MOSEK 6.0 Guided Tour, section  7.13.1, https://docs.mosek.com/6.0/toolbox/node009.html
        c = [-2; -3];
        A = sparse([195 273; 4 40]);
        u = [1365; 140];
        xmax = [4; Inf];
        vtype = 'I';
        p = struct('c', c, 'A', A, 'u', u, 'xmax', xmax, 'vtype', vtype, 'opt', opt);
        [x, f, s, out, lam] = miqps_master(p);
        t_is(s, 1, 12, [t 'success']);
        t_is(x, [4; 2], 12, [t 'x']);
        t_is(lam.mu_l, [0; 0], 12, [t 'lam.mu_l']);
        t_is(lam.mu_u, [0; 0], 12, [t 'lam.mu_u']);
        t_is(lam.lower, [0; 0], 12, [t 'lam.lower']);
        t_is(lam.upper, [2; 3], 12, [t 'lam.upper']);

        if does_qp(k)
            t = sprintf('%s - 4-d MIQP : ', names{k});
            %% from cplexmiqpex.m, CPLEX_Studio_Academic124/cplex/examples/src/matlab/cplexmiqpex.m
            H = sparse([ 33   6    0    0;
                          6  22   11.5  0;
                          0  11.5 11    0;
                          0   0    0    0]);
            c = [-1 -2 -3 -1]';
            Aineq = [-1  1  1 10;
               1 -3  1  0];
            bineq = [20  30]';
            Aeq   = [0  1  0 -3.5];
            beq   =  0;
            xmin    = [ 0;   0;   0; 2];
            xmax    = [40; Inf; Inf; 3];
            A = sparse([Aeq; Aineq]);
            l = [beq; -Inf; -Inf];
            u = [beq; bineq];
            vtype = 'CCCI';
            p = struct('H', H, 'c', c, 'A', A, 'l', l, 'u', u, ...
                'xmin', xmin, 'xmax', xmax, 'vtype', vtype, 'opt', opt);
            [x, f, s, out, lam] = miqps_master(p);
            t_is(s, 1, 12, [t 'success']);
            t_is(x, [7; 7; 0; 2], 7, [t 'x']);
            t_is(lam.mu_l, [466; 0; 0], 6, [t 'lam.mu_l']);
            t_is(lam.mu_u, [0; 272; 0], 6, [t 'lam.mu_u']);
            t_is(lam.lower, [0; 0; 349.5; 4350], 5, [t 'lam.lower']);
            t_is(lam.upper, [0; 0; 0; 0], 7, [t 'lam.upper']);
        else
            t_skip(nmiqp, sprintf('%s does not handle MIQP problems', names{k}));
        end
% opt.verbose = 0;
    end
end

if have_feature('quadprog') && have_feature('quadprog', 'vnum') == 7.005
    warning(s1.state, diff_alg_warn_id);
end

t_end;

function TorF = have_miqp_solver()
TorF = have_feature('cplex') || have_feature('glpk') || ...
    have_feature('gurobi') || have_feature('intlinprog') || ...
    have_feature('mosek');


function AnimateTrajectory(x,y,z)

ax = newplot([]);
axis(ax,[min(x) max(x) min(y) max(y) min(z) max(z)])
%axis(ax,[0 max(x) 0 max(y) 0 max(z)])
head = line('parent',ax,'color','r','marker','.','markersize',15,'erase','xor', ...
    'xdata',x(1),'ydata',y(1),'zdata',z(1));
body = line('parent',ax,'color','b','linestyle','-','erase','none', ...
    'xdata',[],'ydata',[],'zdata',[]);
box on
%set(gca,'yDir','reverse')
% Grow the body
for i = 1:length(x)
    pause(.01)
    set(head,'xdata',x(i),'ydata',y(i),'zdata',z(i))
    set(body,'xdata',x(1:i),'ydata',y(1:i),'zdata',z(1:i))
end


% collect the training data annotations (from the menpo challenge)
clear
train_data_loc = 'C:\Users\tbaltrus\Documents\menpo_data_orig/';

dataset_locs = {[train_data_loc, '/train/'];};

left_to_frontal_map = [17,28; 18,29; 19,30; 20,31;
                       21,34; 22,32; 23,39; 24,38; 25,37; 26,42; 27,41;
                       28,52; 29,51; 30,50; 31,49; 32,60; 33,59; 34,58;
                       35,63; 36,62; 37,61; 38,68; 39,67];

right_to_frontal_map = [17,28; 18,29; 19,30; 20,31;
                       21,34; 22,36; 23,44; 24,45; 25,46; 26,47; 27,48;
                       28,52; 29,53; 30,54; 31,55; 32,56; 33,57; 34,58;
                       35,63; 36,64; 37,65; 38,66; 39,67];

all_pts_left = [];
all_pts_right = [];
all_pts_orig_left = [];
all_pts_orig_right = [];

for i=1:numel(dataset_locs)
    landmarkLabels = dir([dataset_locs{i} '\*.pts']);
    landmarkImgs = dir([dataset_locs{i} '\*.jpg']);

    for p=1:numel(landmarkLabels)
        landmarks = importdata([dataset_locs{i}, landmarkLabels(p).name], ' ', 3);
        img = imread([dataset_locs{i}, landmarkImgs(p).name]);
        landmarks = landmarks.data;
        landmark_labels = -ones(68,2);

        if(size(landmarks,1) == 39)
            % Determine if the points are clock-wise or counter clock-wise
            % Clock-wise points are facing left, counter-clock-wise right
            sum = 0;
            for k=1:11
                step = (landmarks(k+1,1) - landmarks(k,1)) * (landmarks(k+1,2) + landmarks(k,2));
                sum = sum + step;
            end

            if(sum > 0)
                % First need to resample the face outline as there are 9
                % points in the near-frontal and 10 points in profile for
                % the outline of the face

                outline = iterate_piece_wise(landmarks(1:10,:), 9);
                brow = iterate_piece_wise(landmarks(13:16,:), 5);
                landmark_labels(1:9,:) = outline;
                landmark_labels(18:22,:) = brow;
                landmark_labels(left_to_frontal_map(:,2),:) = landmarks(left_to_frontal_map(:,1),:);

                all_pts_orig_right = cat(3, all_pts_orig_right, landmarks);
                all_pts_right = cat(3, all_pts_right, landmark_labels);
            else
                outline = iterate_piece_wise(landmarks(10:-1:1,:), 9);
                brow = iterate_piece_wise(landmarks(16:-1:13,:), 5);

                landmark_labels(9:17,:) = outline;
                landmark_labels(23:27,:) = brow;

                landmark_labels(right_to_frontal_map(:,2),:) = landmarks(right_to_frontal_map(:,1),:);

                all_pts_orig_left = cat(3, all_pts_orig_left, landmarks);
                all_pts_left = cat(3, all_pts_left, landmark_labels);
            end
        end

        if(mod(p,50) == 0)
            fprintf('%d/%d\n', p, numel(landmarkLabels))
        end
    end
end

%%

save('menpo_68_pts_train_profile', 'all_pts_orig_right', 'all_pts_right', 'all_pts_orig_left', 'all_pts_left');


%-------------------------------------------------------------------------------------------------------------------%
%
% IB2d is an Immersed Boundary Code (IB) for solving fully coupled non-linear
% 	fluid-structure interaction models. This version of the code is based off of
%	Peskin's Immersed Boundary Method Paper in Acta Numerica, 2002.
%
% Author: Nicholas A. Battista
% Email:  nick.battista@unc.edu
% Date Created: September 9th, 2016
% Institution: UNC-CH
%
% This code is capable of creating Lagrangian Structures using:
% 	1. Springs
% 	2. Beams (*torsional springs)
% 	3. Target Points
%	4. Muscle-Model (combined Force-Length-Velocity model, "HIll+(Length-Tension)")
%
% One is able to update those Lagrangian Structure parameters, e.g., spring constants, resting %%	lengths, etc
%
% There are a number of built in Examples, mostly used for teaching purposes.
%
% If you would like us %to add a specific muscle model, please let Nick (nick.battista@unc.edu) know.
%
%--------------------------------------------------------------------------------------------------------------------%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Make_Your_Plates_and_Input_Files()

%
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%
Nx =  256;       % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Lx = 1.0;        % Length of Eulerian Grid in x-Direction
dx = Lx/Nx;      % Grid spatial resolution

%
% Immersed Structure Geometric / Dynamic Parameters %
%
ds = 0.5*dx;              % Lagrangian Pt. Spacing (2x resolution of Eulerian grid)
struct_name = 'plates';   % Name for .vertex, .spring, etc files. (must match what's in 'input2d')

% Call function to construct geometry
[xLag,yLag,dist] = give_Me_Immsersed_Boundary_Geometry(ds,Nx,Lx);

% Plot Geometry to test
plot(xLag,yLag,'r-'); hold on;
plot(xLag,yLag,'*'); hold on;
xlabel('x'); ylabel('y');
axis([0 Lx 0 Lx]);

% Prints .vertex file!
print_Lagrangian_Vertices(xLag,yLag,struct_name);

% Prints .spring file!
k_Spring = 2.5e4;                    % Spring stiffness (does not need to be equal for all springs)
ds_Plates = dist;                    % Spring resting length (does not need to be equal for all springs)
print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Plates,ds,struct_name);

% Prints .beam file!
% k_Beam = 0.5;                      % Beam Stiffness (does not need to be equal for all beams)
% C = compute_Curvatures(xLag,yLag)  % Computes curvature of initial configuration
%print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name);

% Prints .target file!
%k_Target = 1e7;
%print_Lagrangian_Target_Pts(xLag,k_Target,struct_name);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints VERTEX points to a file called 'struct_name'.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag); % Total # of Lag. Pts

    vertex_fid = fopen([struct_name '.vertex'], 'w');

    fprintf(vertex_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        X_v = xLag(s);
        Y_v = yLag(s);
        fprintf(vertex_fid, '%1.16e %1.16e\n', X_v, Y_v);
    end

    fclose(vertex_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints SPRING points to a file called 'struct_name'.spring
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Plates,ds,struct_name)

    N = length(xLag);

    spring_fid = fopen([struct_name '.spring'], 'w');

    fprintf(spring_fid, '%d\n', N-2 + N/2 );    % Print # of springs

    %SPRINGS BETWEEN VERTICES
    for s = 1:N
            if s < N/2
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds);
            elseif s > N/2
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds);
            end
    end

    % SPRINGS ACROSS PLATES
    for s=1:N/2
        fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+N/2, k_Spring, ds_Plates);
    end

    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints TARGET points to a file called 'struct_name'.vertex
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Target_Pts(xLag,k_Target,struct_name)

    N = length(xLag);

    target_fid = fopen([struct_name '.target'], 'w');

    fprintf(target_fid, '%d\n', N );

    %Loops over all Lagrangian Pts.
    for s = 1:N
        fprintf(target_fid, '%d %1.16e\n', s, k_Target);
    end

    fclose(target_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints BEAM (Torsional Spring) points to a file called 'struct_name'.beam
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name)

    % k_Beam: beam stiffness
    % C: beam curvature

    N = length(xLag); % NOTE: Total number of beams = Number of Total Lag Pts. - 2

    beam_fid = fopen([struct_name '.beam'], 'w');

    fprintf(beam_fid, '%d\n', N );

    %spring_force = kappa_spring*ds/(ds^2);

    %BEAMS BETWEEN VERTICES
    for s = 2:N-1
            if  s <= N-1
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, s+1, k_Beam, C(s) );
            else
                %Case s=N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 1,   k_Beam, C(s) );
            end
    end
    fclose(beam_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes "curvature" of starting configuration
%
% NOTE: not curvature in the traditional geometric sense, in the 'discrete'
% sense through cross product.
%
% NOTE: assumes a CLOSED structure
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function C = compute_Curvatures(xLag,yLag)

N = length(xLag);
C = zeros( N );

%Note: needs to be done same order as you print .beam file!
for i=1:N

   % Pts Xp -> Xq -> Xr (same as beam force calc.)

   if ( (i > 1) && (i < N) )

        Xp = xLag(i-1); Xq = xLag(i); Xr = xLag(i+1);
        Yp = yLag(i-1); Yq = yLag(i); Yr = yLag(i+1);

   elseif (i==1)

        Xp = xLag(N); Xq = xLag(i); Xr = xLag(i+1);
        Yp = yLag(N); Yq = yLag(i); Yr = yLag(i+1);

   elseif (i==N)

        Xp = xLag(N-1); Xq = xLag(N); Xr = xLag(1);
        Yp = yLag(N-1); Yq = yLag(N); Yr = yLag(1);

   end

   C(i) = (Xr-Xq)*(Yq-Yp) - (Yr-Yq)*(Xq-Xp); %Cross product btwn vectors

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag,dist] = give_Me_Immsersed_Boundary_Geometry(ds,Nx,L)

% ds: Lagrangian pt. spacing
% Nx: Eulerian grid resolution
% L:  Length of computational domain

yLag = 0:ds:L/5;             % Make plate of length L/10
xLag = zeros(1,length(yLag)); % Make corresponding xPts for Vertical Plate

% Translate points
yLag = yLag + (L/2-L/10);  % Translate them symmetrically

% Make sides
xLag_Left = xLag + (L/2-L/10);
xLag_Right = xLag + (L/2+L/10);

% Combine Pts Into ONE Vector
xLag = [xLag_Left xLag_Right];
yLag = [yLag yLag];

% Distance between plates
dist = (L/2+L/10) - (L/2-L/10);

% Plot the Geometry
% plot(xLag,yLag,'*'); hold on;
% axis([0 L 0 L]);


function [b, mpe] = back_mpe(engine, bfuture, f, ev2, t)

if f.t ~= t
  error('mixed up time stamps')
end
if t==1
  error('should call back1_mpe')
end

maximize = 1;
bnet = bnet_from_engine(engine);
ss = bnet.nnodes_per_slice;

int = engine.interface;
D = engine.in_clq;
C = engine.out_clq;
phiD = marginalize_pot(bfuture.clpot{D}, int, maximize);
phiD = set_domain_pot(phiD, int+ss); % shift to slice 2
phiC = marginalize_pot(f.clpot{C}, int+ss, maximize);
ratio = divide_by_pot(phiD, phiC);
f.clpot{C} = multiply_by_pot(f.clpot{C}, ratio);

[mpe, b.clpot] = find_max_config(engine.jtree_engine, f.clpot, f.seppot, ev2);
mpe = mpe((1:ss)+ss); % extract values for slice 2
for c=1:length(b.clpot)
  [b.clpot{c}, ll(c)] = normalize_pot(b.clpot{c});
end
b.t = t;


function p05_title ( )

%*****************************************************************************80
%
%% P05_TITLE prints a title for problem 05.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    None
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'Problem 05\n' );
  fprintf ( 1, '  Name:       ST04\n' );
  fprintf ( 1, '              Stroud #4, page 26.\n' );
  fprintf ( 1, '  Region:     0 <= X(i) <= 1\n' );
  fprintf ( 1, '  Integrand:  F(X) = 1 / ( 1 + sum ( 2 * X(i) ) )\n' );

  return
end


function iou = evaluateRoomObject3D(sequenceName,gtRoom3D,gtObject3D,outRoom3D,outObject3D,visualization,cutoff,delta)

%% set parameters

% set cutoff distance
if ~exist('cutoff','var')
    cutoff = 5.5;
end

% granuality
if ~exist('delta','var')
    delta  = 0.1;
end

if ~exist('visualization','var')
    visualization = false;
end

%% evaluate the accuracy
rangeX = min(min(outRoom3D(1,:)),min(gtRoom3D(1,:)))-delta:delta:max(max(outRoom3D(1,:)),max(gtRoom3D(1,:)))+delta;
rangeY = min(min(outRoom3D(2,:)),min(gtRoom3D(2,:)))-delta:delta:max(max(outRoom3D(2,:)),max(gtRoom3D(2,:)))+delta;
rangeZ = min(min(outRoom3D(3,:)),min(gtRoom3D(3,:)))-delta:delta:max(max(outRoom3D(3,:)),max(gtRoom3D(3,:)))+delta;
[gridX, gridY, gridZ] = ndgrid(rangeX,rangeY,rangeZ);
gridXYZ = [gridX(:)'; gridY(:)'; gridZ(:)'];
clear gridX gridY gridZ
%plot3(gridXYZ(1,:),gridXYZ(2,:),gridXYZ(3,:),'.');

data=readframe(fullfile('/n/fs/sun3d/data/',sequenceName));
imgSize= size(imread(data.depthpath));

extrinsicsFiles = dirSmart(fullfile('http://sun3d.cs.princeton.edu/data/',sequenceName,'extrinsics/'),'txt');
extrinsicsC2W = permute(reshape(readValuesFromTxt(fullfile('http://sun3d.cs.princeton.edu/data/',sequenceName,'extrinsics',extrinsicsFiles(end).name)),4,3,[]),[2 1 3]);

proj2D = data.K * extrinsicsC2W(1:3,1:3)' * gridXYZ;
proj2D(1:2,:) = proj2D(1:2,:) ./ proj2D([3 3],:);
inCamera = proj2D(3,:) > 1.0 & proj2D(3,:)< cutoff & 0<proj2D(1,:) & proj2D(1,:)<imgSize(2) & 0<proj2D(2,:) & proj2D(2,:)<imgSize(1);
gridXYZ = gridXYZ(:,inCamera);

if visualization

    figure

    %alpha(0.6);
    %title('Prediction Result');

    %figure
    numCorners = size(gtRoom3D,2)/2;
    patch(gtRoom3D(1,1:numCorners),gtRoom3D(2,1:numCorners),gtRoom3D(3,1:numCorners),'g');
    hold on
    patch(gtRoom3D(1,numCorners+1:end),gtRoom3D(2,numCorners+1:end),gtRoom3D(3,numCorners+1:end),'g');
    hold on
    for i=1:numCorners-1
        patch(gtRoom3D(1,[i i+1 numCorners+i+1 numCorners+i]),gtRoom3D(2,[i i+1 numCorners+i+1 numCorners+i]),gtRoom3D(3,[i i+1 numCorners+i+1 numCorners+i]),'g');
        hold on
    end
    patch(gtRoom3D(1,[numCorners 1  numCorners+1 numCorners*2]),gtRoom3D(2,[numCorners 1  numCorners+1 numCorners*2]),gtRoom3D(3,[numCorners 1  numCorners+1 numCorners*2]),'g');
    axis equal

    for j=1:length(gtObject3D.objects)
        if ~isempty(gtObject3D.objects{j})
            plot3(gtObject3D.objects{j}.polygon{1}.X([1:end 1]), repmat(gtObject3D.objects{j}.polygon{1}.Ymin,1,5), gtObject3D.objects{j}.polygon{1}.Z([1:end 1]),'-k','LineWidth',2);
            plot3(gtObject3D.objects{j}.polygon{1}.X([1:end 1]), repmat(gtObject3D.objects{j}.polygon{1}.Ymax,1,5), gtObject3D.objects{j}.polygon{1}.Z([1:end 1]),'-k','LineWidth',2);
            for k=1:4
                plot3(gtObject3D.objects{j}.polygon{1}.X([k k]),[gtObject3D.objects{j}.polygon{1}.Ymin,gtObject3D.objects{j}.polygon{1}.Ymax], gtObject3D.objects{j}.polygon{1}.Z([k k]),'-k','LineWidth',2);
            end
        end
    end
    xlabel('x');
    ylabel('y');
    zlabel('z');

    numCorners = size(outRoom3D,2)/2;
    patch(outRoom3D(1,1:numCorners),outRoom3D(2,1:numCorners),outRoom3D(3,1:numCorners),'k');
    hold on
    patch(outRoom3D(1,numCorners+1:end),outRoom3D(2,numCorners+1:end),outRoom3D(3,numCorners+1:end),'b');
    hold on
    for i=1:numCorners-1
        patch(outRoom3D(1,[i i+1 numCorners+i+1 numCorners+i]),outRoom3D(2,[i i+1 numCorners+i+1 numCorners+i]),outRoom3D(3,[i i+1 numCorners+i+1 numCorners+i]),'r');
        hold on
    end
    patch(outRoom3D(1,[numCorners 1  numCorners+1 numCorners*2]),outRoom3D(2,[numCorners 1  numCorners+1 numCorners*2]),outRoom3D(3,[numCorners 1  numCorners+1 numCorners*2]),'r');
    axis equal

    alpha(0.01);

    for j=1:length(outObject3D.objects)
        if ~isempty(outObject3D.objects{j})
            plot3(outObject3D.objects{j}.polygon{1}.X([1:end 1]), repmat(outObject3D.objects{j}.polygon{1}.Ymin,1,5), outObject3D.objects{j}.polygon{1}.Z([1:end 1]),'-b','LineWidth',2);
            plot3(outObject3D.objects{j}.polygon{1}.X([1:end 1]), repmat(outObject3D.objects{j}.polygon{1}.Ymax,1,5), outObject3D.objects{j}.polygon{1}.Z([1:end 1]),'-b','LineWidth',2);
            for k=1:4
                plot3(outObject3D.objects{j}.polygon{1}.X([k k]),[outObject3D.objects{j}.polygon{1}.Ymin,outObject3D.objects{j}.polygon{1}.Ymax], outObject3D.objects{j}.polygon{1}.Z([k k]),'-b','LineWidth',2);
            end
        end
    end
    xlabel('x');
    ylabel('y');
    zlabel('z');

    %hold on;
    %plot3(gridXYZ(1,:),gridXYZ(2,:),gridXYZ(3,:),'.'); xlabel('x'); ylabel('y'); zlabel('z');
end

numCorners = size(gtRoom3D,2)/2;

%% inside the ground truth
[inon,on] = inpolygon(gridXYZ(1,:),gridXYZ(3,:),gtRoom3D(1,1:numCorners),gtRoom3D(3,1:numCorners));
in = inon&~on;
inGroundTruth = in & min(gtRoom3D(2,:)) < gridXYZ(2,:) & gridXYZ(2,:) < max(gtRoom3D(2,:));

intersectCount = zeros(1,length(inGroundTruth));
for i=find(inGroundTruth)
    [xi,yi] = polyxpoly([0 gridXYZ(1,i)],[0 gridXYZ(3,i)],gtRoom3D(1,1:numCorners),gtRoom3D(3,1:numCorners));
    if ~isempty(xi)
        count = 0;
        for k=1:length(xi)
            disSq = xi(k)^2+yi(k)^2;
            if disSq > 0.5^2
                count=count+1;
            end
        end
        intersectCount(i) = count;
    end
end
inGroundTruth = inGroundTruth & (intersectCount==0);

for j=1:length(gtObject3D.objects)
    if ~isempty(gtObject3D.objects{j})
        [inon, on] = inpolygon(gridXYZ(1,:),gridXYZ(3,:), gtObject3D.objects{j}.polygon{1}.X, gtObject3D.objects{j}.polygon{1}.Z);
        in = inon&~on;
        in = in & gtObject3D.objects{j}.polygon{1}.Ymin < gridXYZ(2,:) & gridXYZ(2,:) < gtObject3D.objects{j}.polygon{1}.Ymax;
        inGroundTruth = inGroundTruth & ~in;
    end
end

if visualization
    hold on;
    plot3(gridXYZ(1,inGroundTruth),gridXYZ(2,inGroundTruth),gridXYZ(3,inGroundTruth),'.b');
end

%% inside the prediction results
[inon,on] = inpolygon(gridXYZ(1,:),gridXYZ(3,:),outRoom3D(1,1:end/2),outRoom3D(3,1:end/2));
in = inon&~on;
inResult = in & min(outRoom3D(2,:)) < gridXYZ(2,:) & gridXYZ(2,:) < max(outRoom3D(2,:));

intersectCount = zeros(1,length(inResult));
for i=find(inResult)
    [xi,yi] = polyxpoly([0 gridXYZ(1,i)],[0 gridXYZ(3,i)],outRoom3D(1,1:end/2),outRoom3D(3,1:end/2));
    if ~isempty(xi)
        count = 0;
        for k=1:length(xi)
            disSq = xi(k)^2+yi(k)^2;
            if disSq > 0.5^2
                count=count+1;
            end
        end
        intersectCount(i) = count;
    end
end
inResult = inResult & (intersectCount==0);

for j=1:length(outObject3D.objects)
    if ~isempty(outObject3D.objects{j})
        [inon, on] = inpolygon(gridXYZ(1,:),gridXYZ(3,:), outObject3D.objects{j}.polygon{1}.X, outObject3D.objects{j}.polygon{1}.Z);
        in = inon&~on;
        in = in & outObject3D.objects{j}.polygon{1}.Ymin < gridXYZ(2,:) & gridXYZ(2,:) < outObject3D.objects{j}.polygon{1}.Ymax;
        inResult = inResult & ~in;
    end
end

if visualization
    hold on;
    plot3(gridXYZ(1,inResult),gridXYZ(2,inResult),gridXYZ(3,inResult),'.k');
end

%% evaluate
intersectionGR = inGroundTruth & inResult;
unionGR = inGroundTruth | inResult;
iou = sum(intersectionGR) / sum(unionGR);


function data = readframeSUNRGBD(thispath,dataRoot,cls,bbmode)
    % example code to read annotation from ".json" file.
    % thispath: full path to the data folder.
    % dataRoot: root directory of all data folder.
    % cls : object category of ground truth to load. If not speficfy, the code will load all ground truth.
    if ~exist('cls','var')
        cls  =[];
    end
    if ~exist('bbmode','var')
        bbmode  ='2Dbb';
    end

    if ~exist('dataRoot','var')||isempty(dataRoot)
        dataRoot = '/n/fs/sun3d/data/';
    end
    sequenceName  = getSequenceName(thispath,dataRoot);
    if ~exist(thispath,'dir')
        data.sequenceName = sequenceName;
        data.valid = 0;
        return;
    end
    indd = find(sequenceName=='/');
    sensorType = sequenceName(indd(1)+1:indd(2)-1);
    % get K
    fID = fopen([thispath '/intrinsics.txt'],'r');
    K = reshape(fscanf(fID,'%f'),[3,3])';
    fclose(fID);

     % get image and depth path
    depthpath = dir([thispath '/depth/' '/*.png']);
    depthname = depthpath(1).name;
    depthpath = [thispath '/depth/' depthpath(1).name];

    rgbpath = dir([thispath '/image/' '/*.jpg']);
    rgbname = rgbpath(1).name;
    rgbpath = [thispath '/image/' rgbpath(1).name];

    if exist(sprintf('%s/annotation3Dfinal/index.json',thispath),'file')
        annoteImage =loadjson(sprintf('%s/annotation3Dfinal/index.json',thispath));
        % get Box
        filename = dir([fullfile(thispath,'extrinsics') '/*.txt']);
        Rtilt = dlmread([fullfile(thispath,'extrinsics') '/' filename(end).name]);
        Rtilt = Rtilt(1:3,1:3);
        anno_extrinsics = Rtilt;
        % convert it into matlab coordinate
        Rtilt = [1 0 0; 0 0 1 ;0 -1 0]*Rtilt*[1 0 0; 0 0 -1 ;0 1 0];

        cnt =1;
        for obji =1:length(annoteImage.objects)
                annoteobject =annoteImage.objects(obji);
                if ~isempty(annoteobject)&&~isempty(annoteobject{1})&&~isempty(annoteobject{1}.polygon)
                    annoteobject =annoteobject{1};
                    box = annoteobject.polygon{1};

                    % class name and label
                    ind = find(annoteobject.name==':');
                    if isempty(ind)
                        classname  = annoteobject.name;
                        labelname ='';
                    else
                        if ismember(annoteobject.name(ind-1),{'_',' '}),
                            clname = annoteobject.name(1:ind-2);
                        else
                            clname = annoteobject.name(1:ind-1);
                        end
                        %[~,classId]= ismember(clname,classNames);
                        classname  = clname;
                        labelname = annoteobject.name(ind+2:end);
                        %[~,label]= ismember(Labelname,labelNames);
                    end
                    if ismember(classname,{'wall','floor','ceiling'})||(~isempty(cls)&&~(sum(ismember(cls,{classname}))>0)),
                        continue;
                    end

                    x =box.X;
                    y =box.Z;
                    vector1 =[x(2)-x(1),y(2)-y(1),0];
                    coeff1 =norm(vector1);
                    vector1 =vector1/norm(vector1);
                    vector2 =[x(3)-x(2),y(3)-y(2),0];
                    coeff2 = norm(vector2);
                    vector2 =vector2/norm(vector2);
                    up = cross(vector1,vector2);
                    vector1 = vector1*up(3)/up(3);
                    vector2 = vector2*up(3)/up(3);
                    zmax =-box.Ymax;
                    zmin =-box.Ymin;
                    centroid2D = [0.5*(x(1)+x(3)); 0.5*(y(1)+y(3))];

                    thisbb.basis = [vector1;vector2; 0 0 1]; % one row is one basis
                    thisbb.coeffs = abs([coeff1, coeff2, zmax-zmin])/2;
                    thisbb.centroid = [centroid2D(1), centroid2D(2), 0.5*(zmin+zmax)];
                    thisbb.classname = classname;
                    thisbb.labelname = labelname;
                    thisbb.sequenceName = sequenceName;
                    orientation = [([0.5*(x(2)+x(1)),0.5*(y(2)+y(1))] - centroid2D(:)'), 0];
                    thisbb.orientation = orientation/norm(orientation);

                    if strcmp(bbmode,'2Dbb'),
                        [bb2d,bb2dDraw] = projectStructBbsTo2d(thisbb,Rtilt,[],K);
                        %gtBb2D = crop2DBB(gtBb2D,427,561);
                        thisbb.gtBb2D = bb2d(1:4);
                    end
                    groundtruth3DBB(cnt) =thisbb;
                    cnt=cnt+1;
                end
        end
        if cnt==1,groundtruth3DBB =[];end
    else
        groundtruth3DBB =[];
        filename = dir([fullfile(thispath,'extrinsics') '/*.txt']);
        Rtilt = dlmread([fullfile(thispath,'extrinsics') '/' filename(end).name]);
        Rtilt = Rtilt(1:3,1:3);
        anno_extrinsics = Rtilt;
        Rtilt = [1 0 0; 0 0 1 ;0 -1 0]*Rtilt*[1 0 0; 0 0 -1 ;0 1 0];

    end
    % read in room
    gtCorner3D =[];
    if exist([thispath '/annotation3Dlayout/index.json'],'file')
       json=loadjson([thispath '/annotation3Dlayout/index.json']);
       for objectID=1:length(json.objects)
           try
                groundTruth = json.objects{objectID}.polygon{1};
                numCorners = length(groundTruth.X);

                gtCorner3D(1,:) = [groundTruth.X groundTruth.X];
                gtCorner3D(2,:) = [repmat(groundTruth.Ymin,[1 numCorners]) repmat(groundTruth.Ymax,[1 numCorners])];
                gtCorner3D(3,:) = [groundTruth.Z groundTruth.Z];
                gtCorner3D = anno_extrinsics'*gtCorner3D;
                gtCorner3D = gtCorner3D([1,3,2],:);
                gtCorner3D(3,:) = -1*gtCorner3D(3,:);
                gtCorner3D = Rtilt*gtCorner3D;
                break;
           catch
           end
       end

    end

    data =struct('sequenceName',sequenceName,'groundtruth3DBB',...
         groundtruth3DBB,'Rtilt',Rtilt,'K',K,...
         'depthpath',depthpath,'rgbpath',rgbpath,'anno_extrinsics',anno_extrinsics,'depthname',depthname,...
         'rgbname',rgbname,'sensorType',sensorType,'valid',1,'gtCorner3D',gtCorner3D);

end


function [Wrot, CC] = get_whitening_matrix_faster(rez)
% based on a subset of the data, compute a channel whitening matrix
% this requires temporal filtering first (gpufilter)

ops = rez.ops;
Nbatch = ops.Nbatch;
twind = ops.twind;
NchanTOT = ops.NchanTOT;
NT = ops.NT;
NTbuff = ops.NTbuff;
chanMap = ops.chanMap;
Nchan = rez.ops.Nchan;
xc = rez.xc;
yc = rez.yc;

% load data into patches, filter, compute covariance
fprintf('Getting channel whitening matrix... \n');

ibatch = 1;
ibatch = ibatch:ops.nSkipCov:Nbatch;

% scrappy progress bar in command window
% allBatches = 1:Nbatch;
pb = progBar(ibatch, 20);

if getOr(ops, 'useMemMapping',0)
    %% Parallelize raw data loading (needs memmapfile implement for parallel read access)
        % slice ops for parallel
        ntbuff = ops.ntbuff; % NOTE: this is just the buffer length, not ".NTbuff" (which == the actual batch + buffer length)
        NT = ops.NT;
        bufferedNT = NT + 2*ntbuff;
        tend = ops.tend;
        twind = ops.twind;
        doCAR = logical(ops.CAR);
        CC = cell(1, numel(ibatch));
        [CC{:}] = deal(zeros(Nchan,  Nchan, 'single')); % we'll estimate the covariance from data batches, then add to this variable

        % set up the parameters of the filter % just one instance of filters
        if isfield(ops,'fslow')&&ops.fslow<ops.fs/2
            [b1, a1] = butter(3, [ops.fshigh/ops.fs,ops.fslow/ops.fs]*2, 'bandpass'); % butterworth filter with only 3 nodes (otherwise it's unstable for float32)
        else
            [b1, a1] = butter(3, ops.fshigh/ops.fs*2, 'high'); % the default is to only do high-pass filtering at 150Hz
        end

        % Memory map the raw data file to m.Data.x of size [nChan, nSamples)
        mmfRaw = dir(ops.fbinary);
        nsamp = mmfRaw.bytes/2/ops.NchanTOT;
        mmfRaw = memmapfile(ops.fbinary, 'Format',{'int16', [ops.NchanTOT, nsamp], 'x'}); % ignore tstart here b/c we'll account for it on each mapped read (using .twind)
        %parmmf = parallel.pool.Constant(mmf);
        tic
        parfor i = 1:length(ibatch)
        % for i = 1:length(ibatch)
            ii = ibatch(i);
            %offset = max(0, twind + 2*NchanTOT*((NT - ntbuff) * (ii-1) - 2*ntbuff));
            % index batch timepoints to read:  [ntbuff + NT + ntbuff]
            twindow = [max(1, twind + NT*(ii-1) - ntbuff): min(tend, twind + NT*(ii) + ntbuff)];

            % read from memory mapped file
            buff = mmfRaw.Data.x(:,twindow);

            % % --- step inside gpufilter.m operations ---%

            % subsample only good channels & transpose for filtering
            buff = single(buff(chanMap,:))';    % buff dims now: [time, channel]

            % --- Demean before padding ---
            % subtract within-channel means from each channel
            buff = buff - mean(buff, 1); % subtract mean of each channel

            % CAR, common average referencing by median
            if doCAR %getOr(ops, 'CAR', 1)
                buff = buff - median(buff, 2); % subtract median across channels
            end

            % Now can pad first & last batches with zeros
            nsampcurr = size(buff,1);
            if nsampcurr<bufferedNT
                if ii~=1
                    % append zeros if end batch
                    buff = [buff; zeros(bufferedNT-nsampcurr, size(buff,2))];
                else
                    % prepend zeros if first batch
                    buff = [zeros(bufferedNT-nsampcurr, size(buff,2)); buff];
                end
            end

            % apply high/low pass filtering
            % next four lines should be equivalent to filtfilt (which cannot be used because it requires float64)
            buff = filter(b1, a1, buff); % causal forward filter
            buff = flipud(buff); % reverse time
            buff = filter(b1, a1, buff); % causal forward filter again
            buff = flipud(buff); % reverse time back

            % % --- end of gpufilter.m operations ---%

            buff = buff(ntbuff+1:end-ntbuff,:);   % remove timepoints used as buffers

            CC{i}        = (buff' * buff)/NT; % sample covariance
            %pb.check(i); % progress bar updates not meaningful when running on parallel pool
        end
        toc
        % scale & combine parallel outputs
        CC = cell2mat(cellfun(@(x) permute(x,[3,1,2]), CC', 'uni',0)); % shift dims of cell so that cell2mat maintains array dimensions
        CC = squeeze(mean(CC));
    %%

else
    % old single-thread method
    fid = fopen(ops.fbinary, 'r');
    tic
    CC = gpuArray.zeros( Nchan,  Nchan, 'single'); % we'll estimate the covariance from data batches, then add to this variable

    for i = 1:length(ibatch) %allBatches
        ii = ibatch(i);
        offset = max(0, twind + 2*NchanTOT*((NT - ops.ntbuff) * (ii-1) - 2*ops.ntbuff));
        fseek(fid, offset, 'bof');
        buff = fread(fid, [NchanTOT NTbuff], '*int16');

        if isempty(buff)
            break;
        end
        nsampcurr = size(buff,2);
        if nsampcurr<NTbuff
            buff(:, nsampcurr+1:NTbuff) = repmat(buff(:,nsampcurr), 1, NTbuff-nsampcurr);
        end

        datr    = gpufilter(buff, ops, chanMap); % apply filters and median subtraction

        CC        = CC + (datr' * datr)/NT; % sample covariance

        pb.check(ii) % update progress bar in command window
    end
    CC = CC / ceil((Nbatch-1)/ops.nSkipCov); % normalize by number of batches
    toc
    fclose(fid);
end

% CC = diag(diag(CC));
% mu = mean(diag(CC));

% CC = gpuArray(eye(size(CC), 'single'));
% CC = CC * mu;

% xp = cat(2, rez.xc, rez.yc);
% CC = mu * (.05 + .95 * kernel2D(xp, xp, 30));

% plot(diag(CC))

if ops.whiteningRange<Inf
    % if there are too many channels, a finite whiteningRange is more robust to noise in the estimation of the covariance
    ops.whiteningRange = min(ops.whiteningRange, Nchan);
    Wrot = whiteningLocal(gather(CC), yc, xc, ops.whiteningRange); % this function performs the same matrix inversions as below, just on subsets of channels around each channel
else
    Wrot = whiteningFromCovariance(CC);
end
Wrot    = ops.scaleproc * Wrot; % scale this from unit variance to int 16 range. The default value of 200 should be fine in most (all?) situations.

fprintf('Channel-whitening matrix computed. \n');


function [ y ] = back_trans( x, TransformSpecifics, reconDIM, n1, n2, n3 )
% used to set up transform handle

% M. Fischer, April 2016

%%

if ~iscell(x) && isreal(x) || iscell(x) && isreal(x{1}) % is this error safe?
    if strcmp(reconDIM,'2D')
        switch TransformSpecifics.TransformName
            case 'dwt'
                y = waverec2(x,TransformSpecifics.waveS,TransformSpecifics.FilterName);
            case 'cplxdt_matlab' % matlab variant:
                x_helper = TransformSpecifics.wt_struct;
%                 for k = 1:TransformSpecifics.Stages+1 % change to array to apply thresholding
%                     x_helper.cfs{k} = x{k}; % cfs{i} contains a 5D array.
%                 end;
                x_helper.cfs = x;
                y = idddtree2(x_helper);
            case 'cplxdt' % NOTE! n1 & n2 must be divisible by 2^J.
                for l = 1:TransformSpecifics.Stages % wavletStages (j)
                    for k = 1:2 % real & imag (i)
                        for j = 1:2 % orientation#1 (d1)
                            for h = 1:3 % orientation#2 (d2)
                                x_helper{1,l}{1,k}{1,j}{1,h} = x{l}(:,:,h,j,k); % contains a 5D array.
                            end;
                        end;
                    end;
                end;
                for j = 1:2 % orientation#1 (d1)
                    for h = 1:2 % orientation#2 (d2)
                        x_helper{1,TransformSpecifics.Stages+1}{1,j}{1,h} = x{TransformSpecifics.Stages+1}(:,:,h,j,1);
                    end;
                end;
                y = icplxdual2D(x_helper, TransformSpecifics.Stages, TransformSpecifics.Fsf, TransformSpecifics.sf);
                y = y(1:n1,1:n2);
            case 'shearlet'
                y = nsst_rec2(x.',TransformSpecifics.shear_f,TransformSpecifics.lpfilt);
                y = y(1:n1,1:n2);
            case 'shearlet_nontight'
                y = SLshearrec2D(x,TransformSpecifics.shearletSystem);
            otherwise
                warning('No correct Transform chosen');
        end;
    elseif strcmp(reconDIM,'3D')
        switch TransformSpecifics.TransformName
            case 'dwt'
                x_helper = TransformSpecifics.WDEC;
                x_helper.dec = x;
                y = waverec3(x_helper);
            case 'cplxdt'
                for l = 1:TransformSpecifics.Stages % wavletStages (j)
                    for k = 1:2 % (m)
                        for j = 1:2 % (n)
                            for h = 1:2 % (p)
                                for g = 1:7 % (d)
                                    x_helper{1,l}{1,k}{1,j}{1,h}{1,g} = x{l}(:,:,:,g,h,j,k); % contains a 7D array.
                                end;
                            end;
                        end;
                    end;
                end;
                for k = 1:2 % (m)
                    for j = 1:2 % (n)
                        for g = 1:2 % (d) not 1:7
                        x_helper{1,TransformSpecifics.Stages+1}{1,k}{1,j}{1,g} = x{TransformSpecifics.Stages+1}(:,:,:,g,1,j,k);
                        end;
                    end;
                end;
                y = icplxdual3D(x_helper, TransformSpecifics.Stages, TransformSpecifics.Fsf, TransformSpecifics.sf);
                y = y(1:n1,1:n2,1:n3);
            case 'shearlet'
                decomp_cells = 3*(4*4+2*2); % (fixed in get_transformSpecifics) dims:3, level:2 ,directions: 4x4 & 2x2
                % hard coded solution: (feel free to replace it)
                y = cell(1+decomp_cells,1);
                i = 1;
                for l = 1:3 % dims
                    k = 1; % (level)
                    for j = 1:4 % (direction)
                        for h = 1:4 % (direction)
                            x_helper.D{l,k}{j,h} = x{i};
                            i = i+1;
                        end;
                    end;
                    k =2;
                    for j = 1:2 % (direction)
                        for h = 1:2 % (direction)
                            x_helper.D{l,k}{j,h} = x{i};
                            i = i+1;
                        end;
                    end;
                end;
                x_helper.A = x{i+1};
                y = InvShearTransform3D(x_helper);
            case 'shearlet_nontight'
                if TransformSpecifics.flagExtend
                    x = img_extend(x,0,0,TransformSpecifics.extend_z);
                end;
                y = SLshearrec3D(x,TransformSpecifics.shearletSystem);
            otherwise
                warning('No correct Transform chosen');
        end;
    else
        error('ReconDim is not supported');
    end;
else % should always be real if operator_frame.m is used.
    error('use operator_frame.m');
end;

end


function PlotAdaptiveContour3D(u, levels, tol)

% function PlotAdaptiveContour3D(u, levels, tol)
% Purpose: adaptively refine the mesh to approximately locate isocontours

Globals3D;

% build interpolation matrix (coarse->fine)
EToVi = [1 5 7 8; 5 2 6 9; 7 6 3 10; 8 9 10 4; 8 5 7 9; 7 5 6 9; 8 9 7 10; 9 6 7 10];
VXi   = [-1  1 -1 -1  0  0 -1 -1  0 -1];
VYi   = [-1 -1  1 -1 -1  0  0 -1 -1  0];
VZi   = [-1 -1 -1  1 -1 -1 -1  0  0  0];

v1 = EToVi(:,1); v2 = EToVi(:,2); v3 = EToVi(:,3); v4 = EToVi(:,4);
ri = 0.5*(-(r+s+t+1)*VXi(v1) + (1+r)*VXi(v2) + (1+s)*VXi(v3) + (1+t)*VXi(v4) );
si = 0.5*(-(r+s+t+1)*VYi(v1) + (1+r)*VYi(v2) + (1+s)*VYi(v3) + (1+t)*VYi(v4) );
ti = 0.5*(-(r+s+t+1)*VZi(v1) + (1+r)*VZi(v2) + (1+s)*VZi(v3) + (1+t)*VZi(v4) );

interp = Vandermonde3D(N, ri(:), si(:), ti(:))*invV;
ri = [-1;1;-1;-1]; si = [-1;-1;1;-1]; ti = [-1;-1;-1;1]; refNp = length(ri);
interp1 = Vandermonde3D(N, ri(:), si(:), ti(:))*invV;

Nlevels = length(levels);

sk = 1;
F = spalloc(Np,Np,1);
for i=0:N % old ordering
  for j=0:N - i
    for k=0:N - i - j
      if(i+j+k<=1), F(sk,sk) = 1.; end;
      sk = sk+1;
    end
  end
end

hold on;
for n=1:Nlevels

  xref = x; yref = y; zref = z; uref = u; Kref = K; Jref = J;

  err = 1;
  while(err > tol)

    level = levels(n);

    umin = min(uref, [], 1);
    umax = max(uref, [], 1);

    refineflag = (umin <= level & umax >= level);

    toref = find( refineflag);
    Nref = length(toref);

    uref = reshape(interp*uref(:,toref), Np, 8*Nref);
    xref = reshape(interp*xref(:,toref), Np, 8*Nref);
    yref = reshape(interp*yref(:,toref), Np, 8*Nref);
    zref = reshape(interp*zref(:,toref), Np, 8*Nref);
    Jref = reshape(interp*Jref(:,toref), Np, 8*Nref)/8;

    Kref = 8*Nref;

    ufilt = V*(F*(invV*uref));
    err = max(max(abs(ufilt-uref)));

  end

  xref = interp1*xref; yref = interp1*yref; zref = interp1*zref; uref = interp1*uref;

  tets = reshape(1:4*Kref, 4, Kref)';

  x1 = xref(1,:)'; y1 = yref(1,:)'; z1 = zref(1,:)'; u1 = uref(1,:)' + 1e-10*rand(Kref,1);
  x2 = xref(2,:)'; y2 = yref(2,:)'; z2 = zref(2,:)'; u2 = uref(2,:)' + 1e-10*rand(Kref,1);
  x3 = xref(3,:)'; y3 = yref(3,:)'; z3 = zref(3,:)'; u3 = uref(3,:)' + 1e-10*rand(Kref,1);
  x4 = xref(4,:)'; y4 = yref(4,:)'; z4 = zref(4,:)'; u4 = uref(4,:)' + 1e-10*rand(Kref,1);

  trix = []; triy = []; triz = []; triu = [];;

  umin = min(uref, [], 1);
  umax = max(uref, [], 1);

  lev = levels(n);

  % find candidate elements
  ks = find(umin<= lev & umax >= lev);

  Nks = length(ks);

  if(Nks>0)

    fc = zeros(Nks,6);

    u1ks = u1(ks); u2ks = u2(ks); u3ks = u3(ks); u4ks = u4(ks);

    % find local coordinate at each of 6 edges
    c1 = (lev-u1ks)./(u2ks-u1ks);  fc(:,1) = (c1>=0 & c1<=1);
    c2 = (lev-u2ks)./(u3ks-u2ks);  fc(:,2) = (c2>=0 & c2<=1);
    c3 = (lev-u3ks)./(u1ks-u3ks);  fc(:,3) = (c3>=0 & c3<=1);
    c4 = (lev-u1ks)./(u4ks-u1ks);  fc(:,4) = (c4>=0 & c4<=1);
    c5 = (lev-u2ks)./(u4ks-u2ks);  fc(:,5) = (c5>=0 & c5<=1);
    c6 = (lev-u3ks)./(u4ks-u3ks);  fc(:,6) = (c6>=0 & c6<=1);

    % find triangle intersection
    tris = find(sum(fc, 2)==3); Ntris = length(tris);  tfc = fc(tris,:); ids = find(tfc');
    ktris = ks(tris);

    % trim list
    tc1 = c1(tris); tc2 = c2(tris); tc3 = c3(tris); tc4 = c4(tris); tc5 = c5(tris); tc6 = c6(tris);

    tx1 = x1(ktris);   ty1 = y1(ktris);   tz1 = z1(ktris);
    tx2 = x2(ktris);   ty2 = y2(ktris);   tz2 = z2(ktris);
    tx3 = x3(ktris);   ty3 = y3(ktris);   tz3 = z3(ktris);
    tx4 = x4(ktris);   ty4 = y4(ktris);   tz4 = z4(ktris);

    xc = zeros(Ntris, 6); yc = zeros(Ntris, 6); zc = zeros(Ntris, 6);
    xc(:,1) = (1-tc1).*tx1 + tc1.*tx2; yc(:,1) = (1-tc1).*ty1 + tc1.*ty2; zc(:,1) = (1-tc1).*tz1 + tc1.*tz2;
    xc(:,2) = (1-tc2).*tx2 + tc2.*tx3; yc(:,2) = (1-tc2).*ty2 + tc2.*ty3; zc(:,2) = (1-tc2).*tz2 + tc2.*tz3;
    xc(:,3) = (1-tc3).*tx3 + tc3.*tx1; yc(:,3) = (1-tc3).*ty3 + tc3.*ty1; zc(:,3) = (1-tc3).*tz3 + tc3.*tz1;
    xc(:,4) = (1-tc4).*tx1 + tc4.*tx4; yc(:,4) = (1-tc4).*ty1 + tc4.*ty4; zc(:,4) = (1-tc4).*tz1 + tc4.*tz4;
    xc(:,5) = (1-tc5).*tx2 + tc5.*tx4; yc(:,5) = (1-tc5).*ty2 + tc5.*ty4; zc(:,5) = (1-tc5).*tz2 + tc5.*tz4;
    xc(:,6) = (1-tc6).*tx3 + tc6.*tx4; yc(:,6) = (1-tc6).*ty3 + tc6.*ty4; zc(:,6) = (1-tc6).*tz3 + tc6.*tz4;
    xc = xc'; yc = yc'; zc = zc';

    ids = reshape(ids, 3, Ntris);

    trix = [trix, xc(ids)];
    triy = [triy, yc(ids)];
    triz = [triz, zc(ids)];
    triu = [triu, lev*ones(size(ids))];

    % find quadrilateral intersection
    quads = find(sum(fc, 2)==4); Nquads = length(quads);  qfc = fc(quads,:); ids = find(qfc');
    kquads = ks(quads);

    % quad list
    qc1 = c1(quads); qc2 = c2(quads); qc3 = c3(quads); qc4 = c4(quads); qc5 = c5(quads); qc6 = c6(quads);

    qx1 = x1(kquads);   qy1 = y1(kquads);   qz1 = z1(kquads);
    qx2 = x2(kquads);   qy2 = y2(kquads);   qz2 = z2(kquads);
    qx3 = x3(kquads);   qy3 = y3(kquads);   qz3 = z3(kquads);
    qx4 = x4(kquads);   qy4 = y4(kquads);   qz4 = z4(kquads);

    xc = zeros(Nquads, 6); yc = zeros(Nquads, 6); zc = zeros(Nquads, 6);
    xc(:,1) = (1-qc1).*qx1 + qc1.*qx2; yc(:,1) = (1-qc1).*qy1 + qc1.*qy2; zc(:,1) = (1-qc1).*qz1 + qc1.*qz2;
    xc(:,2) = (1-qc2).*qx2 + qc2.*qx3; yc(:,2) = (1-qc2).*qy2 + qc2.*qy3; zc(:,2) = (1-qc2).*qz2 + qc2.*qz3;
    xc(:,3) = (1-qc3).*qx3 + qc3.*qx1; yc(:,3) = (1-qc3).*qy3 + qc3.*qy1; zc(:,3) = (1-qc3).*qz3 + qc3.*qz1;
    xc(:,4) = (1-qc4).*qx1 + qc4.*qx4; yc(:,4) = (1-qc4).*qy1 + qc4.*qy4; zc(:,4) = (1-qc4).*qz1 + qc4.*qz4;
    xc(:,5) = (1-qc5).*qx2 + qc5.*qx4; yc(:,5) = (1-qc5).*qy2 + qc5.*qy4; zc(:,5) = (1-qc5).*qz2 + qc5.*qz4;
    xc(:,6) = (1-qc6).*qx3 + qc6.*qx4; yc(:,6) = (1-qc6).*qy3 + qc6.*qy4; zc(:,6) = (1-qc6).*qz3 + qc6.*qz4;
    xc = xc'; yc = yc'; zc = zc';

    ids1 = reshape(ids, 4, Nquads);
    ids2 = ids1([1;4;2;3],:);
    ids3 = ids1([1;3;4;2],:);

    % find length perimeters bounded by 4 nodes
    perims1 = QuadPerimeter3D(xc(ids1),yc(ids1),zc(ids1));
    perims2 = QuadPerimeter3D(xc(ids2),yc(ids2),zc(ids2));
    perims3 = QuadPerimeter3D(xc(ids3),yc(ids3),zc(ids3));

    % locate minimum perimeter quad
    idsA = find(perims1<min(perims2, perims3));
    idsB = find(perims2<min(perims1, perims3));
    idsC = find(perims3<min(perims1, perims2));

    % use only minimum perimeter quads
    ids  = [ids1(:,idsA),ids2(:,idsB),ids3(:,idsC)];

    % divide into two triangles (ok since quad must be convex)
    ids1 = [ids([1;2;3],:),ids([1;3;4],:)];

    % add triangles to list
    trix = [trix, xc(ids1)];
    triy = [triy, yc(ids1)];
    triz = [triz, zc(ids1)];
    triu = [triu, lev*ones(size(ids1))];

  end

  size(trix)

  ha = patch(trix,triy,triz,triu); shading interp;

  set(ha, 'EdgeColor', 'none')
end
hold off;

camlight; material shiny; lighting gouraud; view(3); axis equal;
axis off;

return;


function [f,err] = fmincon_funn(cbData,nRow,x,njdiff,dXjbase,reserved,inParam)

persistent params hash xevaled oldh F
if nargin == 7
    params = inParam;
    hash = randn(1,length(params.linearindicies));
    oldh = randn(1);
    xevaled = zeros(1,length(params.interfacedata.c));
    F = [];
    return
end

x = x(1:length(params.linearindicies));

if x(:)'*hash(:) ~= oldh
    oldh = x(:)'*hash(:);
    xevaled = zeros(1,length(params.interfacedata.c));
    xevaled(params.linearindicies) = x;

    % Experimental support for arbitrary functions
    if ~isempty(params.interfacedata.evalMap)

        pp = params.monomtable(params.nonlinearindicies,:);
        xevaled(params.nonlinearindicies) = prod(repmat(xevaled,length(params.nonlinearindicies),1).^pp,2);

        for i = 1:length(params.interfacedata.evalMap)
            arguments = {params.interfacedata.evalMap{i}.fcn,xevaled(params.interfacedata.evalMap{i}.variableIndex)};
            arguments = {arguments{:},params.interfacedata.evalMap{i}.arg{2:end-1}};
            xevaled(params.interfacedata.evalVariables(i)) = feval(arguments{:});
        end
    end
    xevaled(params.nonlinearindicies) = prod(repmat(xevaled,length(params.nonlinearindicies),1).^params.monomtable(params.nonlinearindicies,:),2);
    xevaled = xevaled(:);
    F =  -params.F_struc*[1;xevaled];
end

if nRow == -1
    f = params.interfacedata.c'*xevaled+xevaled'*params.interfacedata.Q*xevaled;
else
    f = F(nRow + 1);%-params.F_struc(nRow + 1,:)*[1;xevaled];
end
err = 0;


function y = mrdivide(X,Y)
%MRDIVIDE (overloaded)

if (isa(Y,'sdpvar'))
    if Y.dim(1)*Y.dim(2) == 1
        y = X*Y^-1;
        return
    else
        error('Division of matrix variables not possible (maybe you meant ./).')
    end
end

try

    % Quick exit on scalar division
    if X.dim(1)*X.dim(2) == 1 & length(Y)==1
        if isinf(Y)
            y = 0;
        else
            y = X;
            y.basis = y.basis/Y;
        end
        return
    end

    lmi_variables = getvariables(X);
    nv = length(lmi_variables);
    y  = X;
    n = X.dim(1);
    m = X.dim(2);
    if (n==1) & (m==1) % SPECIAL CODE FOR FREAKY MATLAB BUG
        y.basis = sparse(reshape(reshape(full(X.basis(:,1)),n,m)/Y,n*m,1));
        for i = 1:nv
            temp = reshape(full(X.basis(:,i+1)),n,m)/Y;
            y.basis(:,i+1) = sparse(temp(:));
        end;
    else
        y.basis = reshape(reshape(X.basis(:,1),n,m)/Y,n*m,1);
        for i = 1:nv
            temp = reshape(X.basis(:,i+1),n,m)/Y;
            y.basis(:,i+1) = temp(:);
        end;
    end

    y.dim(1) = size(temp,1);
    y.dim(2) = size(temp,2);
catch
    error(lasterr);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%     Intra Database Experiments     %%%%%
%%%%%     SFA (Li et al. TMM 2018)       %%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% written by Dingquan Li
% dingquanli@pku.edu.cn
% IDM, SMS, PKU
% Last update: Sept. 15, 2018

clear;clc

caffe_path = '/home/ldq/caffe/matlab/'; % point to the caffe path
addpath(genpath(caffe_path));

database = 'MLIVE2';
% im_dir, im_ids, im_lists, im_names, index, ref_ids, subjective_scores, subjective_scoresSTD
load(['data/' database 'info']); % point to data info file, im_dir should be re-assigned.
% for i = 1:length(im_names) % Update im_lists when im_dir changes
%     im_lists{i} = [im_dir im_names{i}];
% end
% clear i
if ~exist('results','dir')
    mkdir('results');
end
save_path = ['./results/' database '-reproduce']; % point to save path

%% Feature Extraction
layer_names = {'res3d', 'res4f', 'res5c'};
options = 'none';
for k = 1:length(layer_names)
    layer_name = layer_names{k};
    eval(['[feature1.' layer_name ', feature2.' layer_name ', feature3.' layer_name ...
        '] = SFAfeature(im_lists, layer_name, options);']);
end

%%
switch options
    case 'none'
        Rep = 1;
    case 'flipH'
        Rep = 2;
    case 'clipUL'
        T = 6;
        Rep = T^2;
end

N = length(im_lists);
train_ratio = 0.8;
for t = 1 : size(index, 1)
    fprintf('the %d-th iteration\n',t);

    srocc = zeros(length(layer_names), 3, 5);
    for k = 1:length(layer_names)
        layer_name = layer_names{k};
        im_index = index(t,1:ceil(train_ratio*size(index,2)));
        I = buffer(im_index,5);
        for i = 1:5
            train_im_index = I;
            train_im_index(i,:) = [];
            train_im_index(train_im_index==0) = [];
            test_im_index = I(i,:);
            test_im_index(test_im_index==0) = [];
            train_im_index = cell2mat(arrayfun(@(i)find(ref_ids==train_im_index(i))',...
                1:length(train_im_index),'UniformOutput',false));
            test_im_index = cell2mat(arrayfun(@(i)find(ref_ids==test_im_index(i))',...
                1:length(test_im_index),'UniformOutput',false));
            train_labels = subjective_scores(train_im_index); %#ok<NASGU>
            test_labels = subjective_scores(test_im_index);

            p = 10; % number of components;
            eval(['[~,~,~,~,beta1] = plsregress(feature1.' layer_name '(train_im_index(:)+[0:N:(Rep-1)*N],:),repmat(train_labels,Rep,1),p);']);
            eval(['predict_statistics1 = [ones(length(test_im_index(:)),1) feature1.' layer_name '(test_im_index, :)]*beta1;']);
            eval(['[~,~,~,~,beta2] = plsregress(feature2.' layer_name '(train_im_index(:)+[0:N:(Rep-1)*N],:),repmat(train_labels,Rep,1),p);']);
            eval(['predict_statistics2 = [ones(length(test_im_index(:)),1) feature2.' layer_name '(test_im_index, :)]*beta2;']);
            eval(['[~,~,~,~,beta3] = plsregress(feature3.' layer_name '(train_im_index(:)+[0:N:(Rep-1)*N],:),repmat(train_labels,Rep,1),p);']);
            eval(['predict_statistics3 = [ones(length(test_im_index(:)),1) feature3.' layer_name '(test_im_index, :)]*beta3;']);
        %
            predict_statistics = [predict_statistics1 predict_statistics2  predict_statistics3];

            srocc(k,:,i) = corr(predict_statistics, test_labels, 'type', 'Spearman');
        end
    end

    train_im_index = index(t,1:ceil(train_ratio*size(index,2)));
    train_im_index = cell2mat(arrayfun(@(i)find(ref_ids==train_im_index(i))',...
        1:length(train_im_index),'UniformOutput',false));
    test_im_index = index(t,1+ceil(train_ratio*size(index,2)):size(index,2));
    test_im_index = cell2mat(arrayfun(@(i)find(ref_ids==test_im_index(i))',...
        1:length(test_im_index),'UniformOutput',false));
    train_labels = subjective_scores(train_im_index);
    test_labels = subjective_scores(test_im_index);

    Avesrocc = mean(srocc,3);
    [bestK, bestS] = find(Avesrocc==max(Avesrocc(:)),1,'first'); %

    layer_name = layer_names{bestK};
    p = 10; % number of components;
    clear predict_statistics
    eval(['[~,~,~,~,beta1] = plsregress(feature1.' layer_name '(train_im_index(:)+[0:N:(Rep-1)*N],:),repmat(train_labels,Rep,1),p);']);
    eval(['predict_statistics(:, 1) = [ones(length(test_im_index(:)),1) feature1.' layer_name '(test_im_index, :)]*beta1;']);
    eval(['[~,~,~,~,beta2] = plsregress(feature2.' layer_name '(train_im_index(:)+[0:N:(Rep-1)*N],:),repmat(train_labels,Rep,1),p);']);
    eval(['predict_statistics(:, 2) = [ones(length(test_im_index(:)),1) feature2.' layer_name '(test_im_index, :)]*beta2;']);
    eval(['[~,~,~,~,beta3] = plsregress(feature3.' layer_name '(train_im_index(:)+[0:N:(Rep-1)*N],:),repmat(train_labels,Rep,1),p);']);
    eval(['predict_statistics(:, 3) = [ones(length(test_im_index(:)),1) feature3.' layer_name '(test_im_index, :)]*beta3;']);
    w = Avesrocc(bestK,:)'/sum(Avesrocc(bestK,:));
    objective_scores = predict_statistics*w;
    results.srocc(t) = corr(objective_scores, test_labels, 'type', 'Spearman');
    results.plcc(t) = corr(objective_scores, test_labels);
    results.krocc(t) = corr(objective_scores, test_labels, 'type', 'Kendall');
    results.RMSE(t) = sqrt(mean((objective_scores-test_labels).^2));
    results.OR(t) = mean(abs(objective_scores-test_labels)>2*subjective_scoresSTD(test_im_index));
    median(results.srocc)
end
save(save_path);


#!/usr/bin/octave
% Copyright 2018-2020 Marc René Schädler
%
% This file is part of the mobile hearing aid prototype project
% The the mobile hearing aid prototype project 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.
%
% The mobile hearing aid prototype project 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 the mobile hearing aid prototype project. If not, see http://www.gnu.org/licenses/.

cd(fileparts(mfilename('fullpath')))

close all
clear
clc

graphics_toolkit qt;

thresholds_freqs = [125 250 500 1000 2000 4000 8000 16000];
thresholds_init  = [ 39  71  71   62   60   58   58    36]; % dB SPL at mic
normal_threshold = [ 33  24  13    5   13   15   14    11]; % Estimated

plot_freqs = 1000 .* 2.^(-3:0.5:4);
plot_threshold = interp1(thresholds_freqs,normal_threshold,plot_freqs);
plot_levels = 20:15:110;
plot_colors = lines(length(plot_levels));

set (0, "defaultaxesfontsize", 20);
set (0, "defaulttextfontsize", 20);
h_figure = figure('Position', [ 500 200 1500 1000],
       'NumberTitle', 'off',
       'Name', 'Simple self-fitting GUI',
       'toolbar', 'none',
       'menubar', 'none');
h.thresholds_freqs = thresholds_freqs;
h.plot_freqs = plot_freqs;
h.plot_threshold = plot_threshold;
h.plot_levels = plot_levels;
h.plot_colors = plot_colors;

figure_positions = {[0.075 0.37 0.4 0.5],[0.55 0.37 0.4 0.5]};
position_title = {'L E F T', 'R I G H T'};
for i=1:2
  h.ax(i) = axes ('position', figure_positions{i});
  box('on');
  h_threshold = plot(log(thresholds_freqs), zeros(size(thresholds_freqs)),'-k','linewidth',3);
  hold on;
  title(position_title{i});
  plot(log(plot_freqs), plot_threshold,'--k','linewidth',3);
  xlim(log([min(plot_freqs) max(plot_freqs)]));
  ylim([0 130]);
  set(gca,'xtick',log(plot_freqs(1:2:end)));
  set(gca,'xticklabel',plot_freqs(1:2:end));
  set(gca,'ytick',0:10:100);
  xlabel('Frequency / Hz');
  ylabel('Levels / dB SPL (in device)');
  for j=1:length(plot_levels)
    plot(log(plot_freqs),ones(size(plot_freqs)).*plot_levels(j),'--','color',plot_colors(j,:),'linewidth',3);
  end
  h_amplified = zeros(size(plot_levels));
  for j=1:length(plot_levels)
    h_amplified(j) = plot(log(plot_freqs),ones(size(plot_freqs)).*plot_levels(j),'-','color',plot_colors(j,:),'linewidth',3);
  end
  h.h_activedata{i} = h_amplified;
  h.h_threshold{i} = h_threshold;
end

function s = gt_data2string(gt_data)
  [num_rows, num_cols] = size(gt_data);
  s = '[';
  for i=1:num_rows
    s = [s, '['];
    for j=1:num_cols
      s = [s, sprintf('%.2f',gt_data(i,j))];
      if j < num_cols
        s = [s, ' '];
      else
        s = [s, ']'];
      end
    end
    if i < num_rows
      s = [s, ';'];
    else
      s = [s, ']'];
    end
  end
end

function text2speech(message)
  system(['echo text2speech "',message,'" | nc -w 1 127.0.0.1 33338']);
end

function mhacontrol(command)
  system(['echo mhacontrol "',command,'" | nc -w 1 127.0.0.1 33338']);
end

function mhaplay(filename)
  system(['echo mhaplay "',filename,'" "yes" | nc -w 1 127.0.0.1 33338']);
end

function thresholdnoise(status)
  system(['echo thresholdnoise "',status,'" | nc -w 1 127.0.0.1 33338']);
end

function live(status)
  system(['echo live "',status,'" | nc -w 1 127.0.0.1 33338']);
end

function feedback(duration)
  if isnumeric(duration)
    duration = num2str(duration);
  end
  system(['echo feedback "',duration,'" | nc -w 1 127.0.0.1 33338']);
end

function record(duration)
  system(['echo record "',duration,'" | nc -w 1 127.0.0.1 33338']);
end

function send_gaintable(gt_data)
  system(['echo " | nc -w 1 127.0.0.1 33338']);
end

function update_gaintable (obj)
  tic
  h = guidata (obj);
  offset = 30+30.*(0.5-get(h.offset_slider,'value'));
  rolloff = 1+2.^-(get(h.rolloff_slider,'value').*3);
  marginfactor = get(h.marginfactor_slider,'value');
  center = 50+get(h.center_slider,'value').*40;
  focus = round(get(h.focus_slider,'value').*100)./10;

  thresholds_left = zeros(size(h.thresholds_freqs));
  thresholds_right = zeros(size(h.thresholds_freqs));

  for i=1:length(h.thresholds_freqs)
    thresholds_left(i) = str2num(get(h.thresholds_left{i},'string'));
    thresholds_right(i) = str2num(get(h.thresholds_right{i},'string'));
  end

  set(h.h_threshold{1},'ydata',thresholds_left);
  set(h.h_threshold{2},'ydata',thresholds_right);

  [gt_data, gt_freqs, gt_levels] = prescription_minimalistic(h.thresholds_freqs, thresholds_left, thresholds_right, offset, marginfactor, rolloff, center, focus);
  for i=1:2
    gain = interp2(gt_levels,gt_freqs.',gt_data(1+(i-1).*length(gt_freqs):i.*length(gt_freqs),:),h.plot_levels.',h.plot_freqs,'linear');
    for j=1:length(h.plot_levels)
      set(h.h_activedata{i}(j),'ydata',h.plot_levels(j)+gain(:,j));
    end
  end
  mhacontrol(['mha.transducers.mhachain.altplugs.dynamiccompression.mhachain.dc.gtdata = ',gt_data2string(gt_data)]);
  toc
  drawnow;
end

function update_live(obj)
  h = guidata (obj);
  if get(h.live_checkbox,'value')
    live('on');
  else
    live('off');
  end
end

function update_noise(obj)
  h = guidata (obj);
  if get(h.noise_checkbox,'value')
    thresholdnoise('on');
  else
    thresholdnoise('off');
  end
end

function start_recording()
  record('10');
end

function update_amplification(obj)
  h = guidata (obj);
  if get(h.amplification_checkbox,'value')
    mhacontrol('mha.transducers.mhachain.altplugs.select = dynamiccompression');
  else
    mhacontrol('mha.transducers.mhachain.altplugs.select = identity');
  end
end

function update_calib(obj)
  h = guidata (obj);
  if get(h.calib_checkbox,'value')
	% values taken from current openMHA.cfg
    mhacontrol('mha.transducers.calib_in.fir = [[-0.674757033017875 -0.339128228007168 0.030117856798366 -0.119542483672035 0.131726684555067 -0.071387589553860 0.056561094657502 -0.013365060531109 0.007091265106959 0.006913350591302 -0.002602409570910 0.023338189483743 -0.018652297865493 0.021281416315377 -0.014374905102702 0.015659551668403];[-0.674757033017875 -0.339128228007168 0.030117856798366 -0.119542483672035 0.131726684555067 -0.071387589553860 0.056561094657502 -0.013365060531109 0.007091265106959 0.006913350591302 -0.002602409570910 0.023338189483743 -0.018652297865493 0.021281416315377 -0.014374905102702 0.015659551668403]]');
    mhacontrol('mha.transducers.calib_out.fir = [[-0.723329250680037 1.285750531421244 -1.764370643350338 1.702299222653803 0.569187694811727 -1.301642333817312 0.540112043557634 -0.111972475691304 0.187607100395961 0.532899301077540 -0.733701264924906 0.290587502735630 0.470074681167595 -0.330332208101367 -0.010615612830630 -0.221018479662762 -0.020491963942295 0.385847945936633 -0.198969798086483 -0.123437157204177 0.152968427130900 -0.205009361694624 -0.071974598348291 0.041537565470939 -0.058703551389704 0.006171609199330 -0.037439953306526 -0.032735557865821 0.028903184718236 -0.019431703287974 -0.069837180982837 0.038967963643885 -0.015801874625706 -0.060091628384134 0.057102986709234 -0.043134256115441 -0.016815289718170 0.002879163232524 -0.031134102833679 0.026124175604008 -0.040758325083950 0.022598560658844 0.012982112833010 -0.029909215215110 0.018353193685970 0.012015086827306 0.031537385134343 -0.028980188877489 0.026113358679986 0.017332696993042 -0.018853690496196 0.059665425003585 -0.017281793549857 0.007285675861939 0.010218784848149 0.003173184622963 0.040817408121183 -0.024810702312558 0.007344195504224 0.007639434860116 -0.004797394889051 0.025321400227842 -0.005638659121956 -0.002191206231116];[-0.723329250680037 1.285750531421244 -1.764370643350338 1.702299222653803 0.569187694811727 -1.301642333817312 0.540112043557634 -0.111972475691304 0.187607100395961 0.532899301077540 -0.733701264924906 0.290587502735630 0.470074681167595 -0.330332208101367 -0.010615612830630 -0.221018479662762 -0.020491963942295 0.385847945936633 -0.198969798086483 -0.123437157204177 0.152968427130900 -0.205009361694624 -0.071974598348291 0.041537565470939 -0.058703551389704 0.006171609199330 -0.037439953306526 -0.032735557865821 0.028903184718236 -0.019431703287974 -0.069837180982837 0.038967963643885 -0.015801874625706 -0.060091628384134 0.057102986709234 -0.043134256115441 -0.016815289718170 0.002879163232524 -0.031134102833679 0.026124175604008 -0.040758325083950 0.022598560658844 0.012982112833010 -0.029909215215110 0.018353193685970 0.012015086827306 0.031537385134343 -0.028980188877489 0.026113358679986 0.017332696993042 -0.018853690496196 0.059665425003585 -0.017281793549857 0.007285675861939 0.010218784848149 0.003173184622963 0.040817408121183 -0.024810702312558 0.007344195504224 0.007639434860116 -0.004797394889051 0.025321400227842 -0.005638659121956 -0.002191206231116]]');
  else
    mhacontrol('mha.transducers.calib_in.fir = 1');
    mhacontrol('mha.transducers.calib_out.fir = 1');
  end
end

function measure_feedback(obj)
  feedback(3);
end

function update_playback(obj)
  h = guidata (obj);
  string = get(h.playback_popup,'string');
  value = get(h.playback_popup,'value');
  selection = string{value};
  switch selection
    case 'off'
      filename = '';
    case 'last record'
      filename ='/dev/shm/recording.wav';
    otherwise
      filename = ['/home/pi/hearingaid-prototype/recordings/' selection '.wav'];
  end
  mhaplay(filename);
end

for i=1:length(thresholds_freqs)
  uicontrol ('style', 'text',
    'units', 'normalized',
    'string', num2str(thresholds_freqs(i)) ,
    'horizontalalignment', 'center',
    'position', [0.02+i*0.05 0.95 0.05 0.05]);
  h.thresholds_left{i} = uicontrol ('style', 'edit',
    'units', 'normalized',
    'string', num2str(thresholds_init(i)),
    'callback', @update_gaintable,
    'position', [0.02+i*0.05 0.90 0.05 0.05]);
end

for i=1:length(thresholds_freqs)
  uicontrol ('style', 'text',
    'units', 'normalized',
    'string', num2str(thresholds_freqs(i)) ,
    'horizontalalignment', 'center',
    'position', [0.50+i*0.05 0.95 0.05 0.05]);
  h.thresholds_right{i} = uicontrol ('style', 'edit',
    'units', 'normalized',
    'string', num2str(thresholds_init(i)),
    'callback', @update_gaintable,
    'position', [0.50+i*0.05 0.90 0.05 0.05]);
end

uicontrol ('style', 'text',
  'units', 'normalized',
  'string', 'gain' ,
  'horizontalalignment', 'left',
  'position', [0.05 0.25 0.15 0.05]);

h.offset_slider = uicontrol ('style', 'slider',
  'units', 'normalized',
  'sliderstep', [0.1 0.1],
  'string', 'slider',
  'callback', @update_gaintable,
  'value', 0.5,
  'position', [0.15 0.25 0.65 0.05]);

uicontrol ('style', 'text',
  'units', 'normalized',
  'string', 'compression' ,
  'horizontalalignment', 'left',
  'position', [0.05 0.2 0.15 0.05]);

h.rolloff_slider = uicontrol ('style', 'slider',
  'units', 'normalized',
  'sliderstep', [0.1 0.1],
  'string', 'slider',
  'callback', @update_gaintable,
  'value', 0.5,
  'position', [0.15 0.2 0.65 0.05]);

uicontrol ('style', 'text',
  'units', 'normalized',
  'string', 'boost factor' ,
  'horizontalalignment', 'left',
  'position', [0.05 0.15 0.15 0.05]);

h.marginfactor_slider = uicontrol ('style', 'slider',
  'units', 'normalized',
  'sliderstep', [0.1 0.1],
  'string', 'slider',
  'callback', @update_gaintable,
  'value', 0.5,
  'position', [0.15 0.15 0.65 0.05]);

uicontrol ('style', 'text',
  'units', 'normalized',
  'string', 'boost level' ,
  'horizontalalignment', 'left',
  'position', [0.05 0.1 0.15 0.05]);

h.center_slider = uicontrol ('style', 'slider',
  'units', 'normalized',
  'sliderstep', [0.1 0.1],
  'string', 'slider',
  'callback', @update_gaintable,
  'value', 0.5,
  'position', [0.15 0.1 0.65 0.05]);

uicontrol ('style', 'text',
  'units', 'normalized',
  'string', 'focus' ,
  'horizontalalignment', 'left',
  'position', [0.05 0.05 0.15 0.05]);

h.focus_slider = uicontrol ('style', 'slider',
  'units', 'normalized',
  'sliderstep', [0.1 0.1],
  'string', 'slider',
  'callback', @update_gaintable,
  'value', 0.5,
  'position', [0.15 0.05 0.65 0.05]);

h.live_checkbox = uicontrol ('style', 'checkbox',
  'units', 'normalized',
  'string', 'live',
  'value', 1,
  'callback', @update_live,
  'position', [0.85 0.275 0.05 0.025]);

h.record_button = uicontrol ('style', 'pushbutton',
  'units', 'normalized',
  'string', 'record',
  'callback', @start_recording,
  'position', [0.90 0.275 0.05 0.025]);

uicontrol ('style', 'text',
  'units', 'normalized',
  'string', 'playback' ,
  'horizontalalignment', 'left',
  'position', [0.85 0.25 0.1 0.025]);

h.playback_popup = uicontrol ('style', 'popupmenu',
  'units', 'normalized',
  'string', {'off' 'last record' '35-29-25_fridge' '36-26-19_clock' '53-41-34_footsteps' '55-44-33_handwash' '63-57-37_microwave' '63-57-51_coffeemachine' '64-33-24_keyboardwriting' '64-42-35_foyer' '64-53-46_autobahn' '66-56-48_siren' '68-57-50_drivingnews' '68-59-52_street' '70-61-53_street' '72-57-49_bird' '72-63-60_coffeegrinder' '73-50-42_steeldrum-steps' '73-60-50_cardriveby' '73-63-57_steet-laughter' '75-25-17_phone' '77-54-46_ducks' '78-68-60_street' '78-69-62_lisboa-bride-bird' '79-59-50_bar' '81-54-34_coffeemachine' '81-61-47_traincrossing' '81-66-60_lisboa-plane-landing' '82-53-35_shakeout' '83-69-60_mainstreet' '83-73-65_traffic' '86-75-68_lisboa-bride-train' '88-73-65_lisboa-train' '100-64-37_flute'},
  'callback', @update_playback,
  'position', [0.85 0.225 0.1 0.025]);

h.amplification_checkbox = uicontrol ('style', 'checkbox',
  'units', 'normalized',
  'string', 'amplification',
  'value', 0,
  'callback', @update_amplification,
  'position', [0.85 0.15 0.1 0.025]);

h.noise_checkbox = uicontrol ('style', 'checkbox',
  'units', 'normalized',
  'string', 'threshold noise',
  'value', 0,
  'callback', @update_noise,
  'position', [0.85 0.175 0.1 0.025]);

h.calib_checkbox = uicontrol ('style', 'checkbox',
  'units', 'normalized',
  'string', 'calibration',
  'value', 1,
  'callback', @update_calib,
  'position', [0.85 0.125 0.1 0.025]);

h.feedback_button = uicontrol ('style', 'pushbutton',
  'units', 'normalized',
  'string', 'feedback',
  'callback', @measure_feedback,
  'position', [0.85 0.05 0.1 0.05]);

guidata(gcf, h);
update_calib(gcf);
update_amplification(gcf);
update_gaintable(gcf);
update_noise(gcf);
update_playback(gcf);
update_live(gcf);
waitfor(h_figure);


%
%       HDR Toolbox demo Fusion:
%	   1) Load "Bottles_Small.pfm" HDR image
%	   2) Apply Fusion TMO
%	   3) Show the image without gamma encoding, because it is working in
%	   gamma space
%	   4) Save the tone mapped image as PNG
%
%       Author: Francesco Banterle
%       Copyright 2012-16 (c)
%

clear all;

disp('1) Load the image Bottles_Small.pfm using hdrimread');
img = hdrimread('demos/Bottles_Small.hdr');

disp('2) Apply Fusion Operator by Mertens et al.');
imgTMO = MertensTMO(img);

disp('3) Show the image after fusion, note that there is no need of gamma correction!');
h = figure(1);
set(h,'Name','Exposure fusion by Mertens et al.');
GammaTMO(imgTMO, 1.0, 0.5, 1);

disp('4) Save the tone mapped image as a PNG.');
imwrite(imgTMO, 'demos/output/Bottles_Small_TMO_fusion.png');


classdef PrincipalDirectionComputerIn3D < PrincipalDirectionComputer

    methods (Access = public, Static)

        function obj = PrincipalDirectionComputerIn3D(cParams)
            obj.ndim = 3;
            obj.init(cParams)
        end

    end

    methods (Access = public)

        function compute(obj,tensor)
            s1  = squeeze(tensor(1,1,:));
            s2  = squeeze(tensor(1,2,:));
            s3  = squeeze(tensor(1,3,:));
            s12 = squeeze(tensor(1,4,:));
            s13 = squeeze(tensor(1,5,:));
            s23 = squeeze(tensor(1,6,:));
            eG = obj.eigenComputer;
            for i = 1:obj.ndim
                for j = 1:obj.ndim
                    d = eG.eigenVectorFunction{i,j}(s1,s12,s13,s2,s23,s3);
                    obj.direction(i,j,:) = real(d);
                    obj.assertSmallImaginaryValue(d);
                end
            end
        end

    end

    methods (Access = private, Static)

        function assertSmallImaginaryValue(d)
            imagD = imag(d);
%             assert(norm(imagD(:)) < 1e6) % To be fixed
        end

    end

end


% Calculate border coverage for detected fields
%
% This function calculates firing map border coverage that is further
% used in calculation of a border score.
%
%  USAGE
%   coverage = analyses.borderCoverage(fields, <options>)
%   fields          Array of structures with information about detected fields.
%   <options>       Optional list of property-value pairs (see table below)
%
%   ==============================================================================================
%    Properties    Values
%   ----------------------------------------------------------------------------------------------
%    'searchWidth'  If map is not perfect, but contains NaN values along borders, then
%                   search for border pixels can have NaNs. To mitigate this, we check
%                   searchWidth rows/columns near border and if the closest to the border pixel
%                   equals to NaN, we search for first non-NaN value in searchWidth rows-columns.
%                   This argument is optional and default value is 8.
%    'walls'        Definition of walls along which the border score is calculated. Provided by
%                   a string which contains characters that stand for walls:
%                   T - top wall (we assume the bird-eye view on the arena)
%                   R - right wall
%                   B - bottom wall
%                   L - left wall
%                   Characters are case insensitive. Default value is 'TRBL' meaning that border
%                   score is calculated along all walls. Any combination is possible, e.g.
%                   'R' to calculate along right wall, 'BL' to calculate along two walls, e.t.c.
%   ==============================================================================================
%   coverage        Border coverage, ranges from 0 to 1.
%
function coverage = borderCoverage(fields, varargin)
    coverage = 0;

    inp = inputParser;
    defaultSearchWidth = 8;
    defaultWalls = 'TRBL'; % top, right, bottom, left

    checkSearchWidth = @(x) isnumeric(x) && isscalar(x) && (x > 0);
    checkWalls = @internCheckWalls;

    addRequired(inp, 'fields');
    addParameter(inp, 'searchWidth', defaultSearchWidth, checkSearchWidth);
    addParameter(inp, 'walls', defaultWalls, checkWalls);

    inp.KeepUnmatched = true;
    parse(inp, fields, varargin{:});

    % get parsed results
    walls = inp.Results.walls;
    searchWidth = inp.Results.searchWidth;

    % find out what walls are present
    dict = {'B', 'L', 'R', 'T'}; % already sorted
    u = unique(walls);
    t = ['^' sprintf('%c{0,%d}', [u; histc(walls, u)]) '$'];
    wallsIdx = find(~cellfun('isempty', regexpi(dict, t))); % indices in dict of present walls

    for i=1:length(fields)
        curField = fields(i).map;

        % coverage for left wall
        if any(ismember(wallsIdx, 2))
            aux_map = curField(:, 1:searchWidth);
            [covered, norm] = wall_field(aux_map);
            if (covered/norm > coverage)
                coverage = covered/norm;
            end
        end

        % coverage for right wall
        if any(ismember(wallsIdx, 3))
            aux_map = curField(:, end:-1:end+1-searchWidth);
            [covered, norm] = wall_field(aux_map);
            if (covered/norm > coverage)
                coverage=covered/norm;
            end
        end

        % coverage for bottom wall, since we are dealing with data that came from a camera
        % 'bottom' is actually at the top of the matrix curField.
        if (any(ismember(wallsIdx, 1)))
            aux_map = curField(1:searchWidth, :)';
            [covered, norm] = wall_field(aux_map);
            if (covered/norm > coverage)
                coverage=covered/norm;
            end
        end

        % coverage for top wall
        if (any(ismember(wallsIdx, 4)))
            aux_map = curField(end:-1:end+1-searchWidth, :)';
            [covered, norm] = wall_field(aux_map);
            if (covered/norm > coverage)
                coverage = covered/norm;
            end
        end
    end
end

% 'covered' pixels will have distance to border 0.
% Essentially we need to calculate number of elements,
% that equal to zero and take NaNs into account.
function [covered, norm] = wall_field(map)
    ly = size(map, 1);

    D = bwdist(map);
    nanIndices = find(isnan(map(:, 1)));
    numNans = length(nanIndices);
    for i = 1:numNans
        testRow = nanIndices(i);
        nonNan = find(~isnan(map(testRow, :)), 1, 'first');
        if ~isempty(nonNan)
            numNans = numNans - 1;
            D(testRow, 1) = D(testRow, nonNan);
        end
    end

    norm = ly - numNans;
    covered = nansum(D(:, 1) == 0) - numNans;
end

% check input argument that defines walls
% We need to figure out if argument contains one of the characters
% from the dictionary.
% Rise descriptive exception in case of an error.
function res = internCheckWalls(walls)
    res = true;
    if ~ischar(walls)
        error('BNT:args:notChar', 'Argument is not a string.')
    end
    if length(walls) > 4
        error('BNT:args:length', 'Argument can not exceed 4 characters (default ''TLBR'')');
    end

    dict = {'T', 'R', 'B', 'L'};

    % http://stackoverflow.com/questions/19343339/matlab-find-the-indices-of-a-cell-array-of-strings-with-characters-all-containe
    u = unique(walls);
    t = ['^' sprintf('%c{0,%d}', [u; histc(walls, u)]) '$'];
    s = cellfun(@sort, dict, 'uni', 0);
    idx = find(~cellfun('isempty', regexp(s, t)), 1);
    %res = ~isempty(idx); % if it is not empty, then we have characters from dict in walls
    if isempty(idx)
        error('BNT:args:noValidChars', 'There is no information about the walls in the argument.');
    end
end


function [y]=fort_mvk4(A,x,eps,varargin)

nswp = 10;
kickrank = 5;
rmax = 256;
y0 = [];
verb = 1;
for i=1:2:length(varargin)-1
    switch lower(varargin{i})
        case 'nswp'
            nswp=varargin{i+1};
        case 'rmax'
            rmax=varargin{i+1};
        case 'y0'
            y0=varargin{i+1};
        case 'kickrank'
            kickrank=varargin{i+1};
        case 'verb'
            verb=varargin{i+1};

        otherwise
            error('Unrecognized option: %s\n',varargin{i});
    end
end

rx = x.r;
d = x.d;
tailranks = [false,false];
if (rx(1)~=1)
    e1 = tt_tensor;
    e1.d=1;
    e1.n = [rx(1)];
    e1.r = [1; rx(1)];
    e1.ps = [1; rx(1)^2+1];
    e1.core = reshape(eye(rx(1)), rx(1)^2, 1);
    x = kron(e1, x);

    A = kron(tt_matrix(eye(rx(1))), A);
    tailranks(1)=true;
end;
if (rx(d+1)~=1)
    e1 = tt_tensor;
    e1.d=1;
    e1.n = [rx(d+1)];
    e1.r = [rx(d+1); 1];
    e1.ps = [1; rx(d+1)^2+1];
    e1.core = reshape(eye(rx(d+1)), rx(d+1)^2, 1);
    x = kron(x, e1);

    A = kron(A, tt_matrix(eye(rx(d+1))));
    tailranks(2)=true;
end;

d = x.d;
n = A.n;

if (isempty(y0))
    y0 = tt_rand(n, d, 2);
end;

[ry,cry]=fort_mvk4_mex(int64(d), int64(n), int64(A.m), ...
    int64(x.r), int64(A.r), A.core, x.core, y0.core, int64(y0.r), eps, ...
    int64(rmax), int64(nswp), int64(kickrank), int64(verb));
% fprintf('fort_mvk4 done.\n');

ry = double(ry);
ps = cumsum([1; ry(1:d).*n.*ry(2:d+1)]);
if (tailranks(1))
    cr1 = reshape(cry(1:ps(2)-1), n(1), ry(2));
    cr2 = reshape(cry(ps(2):ps(3)-1), ry(2), n(2)*ry(3));
    cr2 = cr1*cr2;
    cry = [cr2(:); cry(ps(3):ps(d+1)-1)];
    ry = ry(2:d+1);
    ry(1) = n(1);
    n = n(2:d);
    d = d-1;
    ps = cumsum([1; ry(1:d).*n.*ry(2:d+1)]);
end;
if (tailranks(2))
    cr1 = reshape(cry(ps(d-1):ps(d)-1), ry(d-1)*n(d-1), ry(d));
    cr2 = reshape(cry(ps(d):ps(d+1)-1), ry(d), n(d));
    cr2 = cr1*cr2;
    cry = [cry(1:ps(d-1)-1); cr2(:)];
    d = d-1;
    ry = ry(1:d+1);
    ry(d+1) = n(d+1);
    n = n(1:d);
    ps = cumsum([1; ry(1:d).*n.*ry(2:d+1)]);
end;

y = y0;
y.r = ry;
y.core = cry;
y.ps = ps;
y.n = n;
y.d = d;

end


function z = plus( x, y )
    %PLUS Addition of two TT/MPS tensors.
    %   Z = PLUS(X,Y) adds two TT/MPS tensors. The rank of the resulting
    %   tensor is 2*R.
    %
    %   See also MINUS, UMINUS.

    %   TTeMPS Toolbox.
    %   Michael Steinlechner, 2013-2016
    %   Questions and contact: michael.steinlechner@epfl.ch
    %   BSD 2-clause license, see LICENSE.txt

    % add sanity check...
    rx = x.rank;
    ry = y.rank;
    nx = x.size;

    z = TTeMPS( cell(1, x.order) );

    % first core:
    p = size(x.U{1},4);
    tmp = zeros( 1, nx(1), rx(2)+ry(2), p );
    tmp( 1, :, 1:rx(2), : ) = x.U{1};
    tmp( 1, :, rx(2)+1:end, : ) = y.U{1};
    z.U{1} = tmp;

    % central cores:
    for i = 2:x.order-1
        % possibility of block format:
        p = size(x.U{i},4);
        tmp = zeros( rx(i)+ry(i), nx(i), rx(i+1)+ry(i+1), p);
        tmp( 1:rx(i), :, 1:rx(i+1), :) = x.U{i};
        tmp( rx(i)+1:end, :, rx(i+1)+1:end, :) = y.U{i};
        z.U{i} = tmp;
    end

    % last core:
    p = size(x.U{end},4);
    tmp = zeros( rx(end-1)+ry(end-1), nx(end), 1, p );
    tmp( 1:rx(end-1), :, 1, : ) = x.U{end};
    tmp( rx(end-1)+1:end, :, 1, : ) = y.U{end};
    z.U{end} = tmp;
end


function stats = ReconstructPosition(positions,spikes,phases,varargin)

%ReconstructPosition - Bayesian reconstruction of positions from spike trains.
%
% Instantaneous positions are reconstructed using a Bayesian algorithm.
% Instantaneous population firing rates can be estimated either over fixed time
% windows, or over fractions of the theta cycle (or of any other brain rhythm).
% Similarly, positions will be reconstructed either over time or phase windows.
% The model is first trained on a subset of the data, then tested on the rest.
%
% USAGE
%
%    stats = ReconstructPosition(positions,spikes,phases,<options>)
%
%    positions      linear or two-dimensional positions, in [0..1]
%    spikes         list of (t,group,cluster) triplets (obtained via e.g.
%                   GetSpikes, using full output)
%    phases         optional unwrapped phase of the LFP (see Phase)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'training'    time interval over which the model should be trained
%                   (default = first half of the position data)
%     'window'      length of the time or phase window (default = 0.020 s for
%                   time, and pi/3 for phases)
%     'type'        two letters (one for X and one for Y) indicating which
%                   coordinates are linear ('l') and which are circular ('c')
%                   - for 1D data, only one letter is used (default 'll')
%     'nBins'       firing curve or map resolution (default = [200 200])
%    =========================================================================
%
%   OUTPUT
%
%     stats.positions     real position across time or phase windows
%     stats.spikes        cell firing vector across time or phase windows
%     stats.estimations   estimated position across time or phase windows
%     stats.errors        estimation error across time or phase windows
%     stats.average       average estimation error in each phase window
%     stats.windows       time windows (possibly computed from phases)
%     stats.phases        phase windows (empty for fixed time windows)
%

% Copyright (C) 2012-2013 by Michaël Zugaro, (C) 2012 by Karim El Kanbi
%
% 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 3 of the License, or
% (at your option) any later version.

% Defaults
wt = 0.020; % default time window
wp = pi/3; % default phase window
window = [];
nBins = 200;
training = 0.5;
type = '';
nDimensions = 1;

% Check number of parameters
if nargin < 2 || mod(length(varargin),2) ~= 0,
	builtin('error','Incorrect number of parameters (type ''help ReconstructPosition'' for details).');
end

% Optional parameter 'phases'
if nargin == 2,
	phases = [];
elseif nargin >= 3 && ischar(phases),
	varargin = {phases,varargin{:}};
	phases = [];
end

% Check parameter sizes
if ~isdmatrix(positions),
	builtin('error','Incorrect positions (type ''help ReconstructPosition'' for details).');
end
if ~isdmatrix(spikes),
	builtin('error','Incorrect spikes (type ''help ReconstructPosition'' for details).');
end
if size(positions,2) > 3,
	nDimensions = 2;
end
if ~isempty(phases) && ~isdmatrix(phases),
	builtin('error','Incorrect value for property ''phases'' (type ''help ReconstructPosition'' for details).');
end

% Check number of output parameters
if isempty(phases) && nargout == 3,
	builtin('error','Too many output parameters or missing phases (type ''help ReconstructPosition'' for details).');
end

% Parse parameters
for i = 1:2:length(varargin),
	if ~ischar(varargin{i}),
		builtin('error',['Parameter ' num2str(i+2) ' is not a property (type ''help ReconstructPosition'' for details).']);
	end
	switch(lower(varargin{i})),
		case 'training',
			training = varargin{i+1};
			if ~isdvector(training,'>'),
				builtin('error','Incorrect value for property ''training'' (type ''help ReconstructPosition'' for details).');
			end
		case 'window',
			window = varargin{i+1};
			if ~isdscalar(window,'>0'),
				builtin('error','Incorrect value for property ''window'' (type ''help ReconstructPosition'' for details).');
			end
		case 'show',
			show = varargin{i+1};
			if ~isstring_FMAT(show,'on','off'),
				builtin('error','Incorrect value for property ''show'' (type ''help ReconstructPosition'' for details).');
			end
		case 'nBins',
			nBins = varargin{i+1};
			if ~isinteger(nBins),
				builtin('error','Incorrect value for property ''nBins'' (type ''help ReconstructPosition'' for details).');
			end
		case 'type',
			type = lower(varargin{i+1});
			if (nDimensions == 1 && ~isstring_FMAT(type,'cc','cl','lc','ll')) || (nDimensions == 2 && ~isstring_FMAT(type,'ccl','cll','lcl','lll','ccc','clc','lcc','llc')),
				builtin('error','Incorrect value for property ''type'' (type ''help ReconstructPosition'' for details).');
			end
		otherwise,
			builtin('error',['Unknown property ''' num2str(varargin{i}) ''' (type ''help ReconstructPosition'' for details).']);
	end
end

% Defaults
if isempty(window),
	if isempty(phases),
		window = wt;
	else
		window = wp;
		if ~isiscalar((2*pi)/window),
			builtin('error',['Incorrect phase window: not an integer fraction of 2pi (type ''help ReconstructPosition'' for details).']);
		end
	end
end
if isdscalar(training),
	training = [-Inf positions(1,1)+training*(positions(end,1)-positions(1,1))];
else
	if min([positions(1,1) spikes(1,1)]) < training(1) & min([positions(end,1) spikes(end,1)]) > training(2),
		builtin('error',['Spikes or positions occur both before and after training interval (type ''help ReconstructPosition'' for details).']);
	end
end
if isempty(type),
	if nDimensions == 2,
		type = 'lll';
	else
		type = 'll';
	end
end
nBinsX = nBins(1);
if length(nBins) > 2,
	nBinsY = nBins(2);
else
	if nDimensions == 2,
		nBinsY = nBinsX;
	else
		nBinsY = 1;
	end
end

% List units, assign them an ID (number them from 1 to N), and associate these IDs with each spike
% (IDs will be easier to manipulate than (group,cluster) pairs in subsequent computations)
[units,~,i] = unique(spikes(:,2:end),'rows');
nUnits = length(units);
index = 1:nUnits;
id = index(i)';
spikes = [spikes(:,1) id];

% Split data (training vs test)
trainingPositions = Restrict(positions,training);
trainingSpikes = Restrict(spikes,training);
testPositions = positions(~InIntervals(positions,training),:);
testSpikes = spikes(~InIntervals(spikes,training),:);

% TRAINING

% Compute occupancy probability P(x) (i.e. normalized occupancy map)
firstUnit = trainingSpikes(:,2) == 1;
s = trainingSpikes(firstUnit,1);
map = Map(trainingPositions,s,'nbins',nBins,'smooth',5,'type',type);
Px = map.time;
Px = Px ./ sum(Px(:));

% Compute average firing probability lambda for each unit (i.e. firing maps)
lambda(:,:,1) = map.z;
for i = 2:nUnits,
	nextUnit = trainingSpikes(:,2) == i;
	s = trainingSpikes(nextUnit,1);
	map = Map(trainingPositions,s,'nbins',nBins,'smooth',5,'type',type);
	lambda(:,:,i) = map.z;
end

% TEST

% Determine time windows (using unwrapped phases if necessary)
if ~isempty(phases),
	testPhases = phases(~InIntervals(phases,training),:);
	drop = testPhases(:,1) < testPositions(1,1);
	testPhases(drop,:) = [];
	startPhase = ceil(testPhases(1,2)/(2*pi))*2*pi;
	stopPhase = floor(testPhases(end,2)/(2*pi))*2*pi;
	windows = (startPhase:window:stopPhase)';
	stats.phases = windows;
	windows = Interpolate(testPhases(:,[2 1]),windows);
	windows = [windows(1:end-1,2) windows(2:end,2)];
else
	stats.phases = [];
	windows = (testPositions(1,1):window:testPositions(end,1))';
	windows = [windows(1:end-1) windows(2:end)];
end
nWindows = size(windows,1);

stats.estimations = nan(nBinsY,nBinsX,nWindows);
stats.spikes = zeros(nUnits,nWindows);
% Loop over data windows
for i = 1:nWindows,

	% Get spikes for this window
	s = Restrict(testSpikes,windows(i,:));

	if isempty(s),
		% No spikes: set uniform probability
		stats.estimations(:,:,i) = ones(nBinsY,nBinsX,1)/(nBinsX*nBinsY);
		continue;
	end

	% Population spike count vector
	stats.spikes(:,i) = Accumulate(s(:,2),1,nUnits);
	% To avoid 'for' loops, prepare for vector computation:
	% assign a spike count to each position and unit (3D array)
	n = reshape(repmat(stats.spikes(:,i),1,nBinsX*nBinsY)',nBinsY,nBinsX,nUnits);

	% For each cell i, compute P(ni|x) using a Poisson model
	dt = windows(i,2) - windows(i,1);
	Pnix = (dt*lambda).^n./factorial(n).*exp(-dt*lambda);
	% Compute P(n|x) assuming independent probabilities across units (hmm...)
	% i.e. P(n|x) = product over i of P(ni|x)
	Pnx = prod(Pnix,3);

	% Compute P(n) = sum over x of P(n|x)*P(x)
	Pn = sum(sum(Pnx.*Px));

	% Compute P(x|n) = P(n|x)*P(x)/P(n)
	Pxn = Pnx .* Px / Pn;

	% Store result
	stats.estimations(:,:,i) = Pxn;

end
stats.estimations = squeeze(stats.estimations);
stats.windows = windows;

% Estimation error

stats.errors = [];
stats.average = [];
if nDimensions == 1,
	% Bin test positions and compute distance to center
	stats.positions = Interpolate(testPositions,windows(:,1));
	stats.positions(:,2) = Bin(stats.positions(:,2),[0 1],nBinsX);
	dx = (round(nBinsX/2)-stats.positions(:,2))';
	% Shift estimated position by the real distance to center
	stats.errors = CircularShift(stats.estimations(:,1:length(dx)),dx);
	% Average over one or more cycles
	k = 2*pi/window;
	n = floor(size(stats.errors,2)/k)*k;
	stats.average = reshape(stats.errors(:,1:n),nBins,k,[]);
	stats.average = nanmean(stats.average,3);
else
	warning('Computation of estimation error not yet implemented for 2D environments');
end