%% Generate best fit function
start_vy = [-0.8, -0.6, -0.4, -0.2, 0, 0.2, 0.4, 0.6, 0.8];
target_vy = [0];
start_vx = 0.0;%[-0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4];
target_vx = [0];
T = 0.4;
subfolder_name = 'library4';
if ~exist(fullfile('local', subfolder_name, 'transition_fitting'), 'dir')
    mkdir(fullfile('local', subfolder_name, 'transition_fitting'));
end

N_vx = length(start_vx);
N_vy = length(start_vy);
N = 16;
t_all = cell(N_vx, N_vy);
q_all = cell(N_vx, N_vy);
dq_all = cell(N_vx, N_vy);
px_all = cell(N_vx, N_vy);
py_all = cell(N_vx, N_vy);
vx_all = cell(N_vx, N_vy);
vy_all = cell(N_vx, N_vy);
counter = 1;
for i = 1:N_vx
    vx = start_vx(i);
    for j = 1:N_vy
        vy = start_vy(j);
        data_name = fullfile('local', subfolder_name, 'transition', ...
                sprintf('gait_X%0.1f_Y%.1f_TO_X%0.1f_Y%.1f.mat', vx, vy, target_vx, target_vy));
        param = load(data_name);

        t_all{i,j} = [param.gait(1).tspan]./T;%, param.gait(3).tspan];
        q_all{i,j} = [param.gait(1).states.x];%, param. gait(3).states.x];
        dq_all{i,j} = [param.gait(1).states.dx];%, param. gait(3).states.dx];
        px_all{i,j} = [param.gait(1).states.x(1,:)];%ones(size(t_all{i,j}))*vx;
        py_all{i,j} = [param.gait(1).states.x(2,:)];%ones(size(t_all{i,j}))*vy;
        vx_all{i,j} = [param.gait(1).states.dx(1,:)];%ones(size(t_all{i,j}))*vx;
        vy_all{i,j} = [param.gait(1).states.dx(2,:)];%ones(size(t_all{i,j}))*vy;

    end
end

inpt = [horzcat(t_all{:})
%     horzcat(px_all{:})
%     horzcat(py_all{:})
%     horzcat(vx_all{:})
    horzcat(vy_all{:})];
q_outpt = horzcat(q_all{:});
dq_outpt = horzcat(dq_all{:});
%%
for i = 1:N
    q_outpt_specific = q_outpt(i, :);
    dq_outpt_specific = dq_outpt(i, :);
    opt.neuralFitting(q_outpt_specific, inpt, 5, fullfile('local', subfolder_name, 'transition_fitting', sprintf('sagittal_library_q%i', i)));
    opt.neuralFitting(dq_outpt_specific, inpt, 5, fullfile('local', subfolder_name, 'transition_fitting', sprintf('sagittal_library_dq%i', i)));
end
addpath(fullfile('local', subfolder_name, 'transition_fitting'));
%% Plot features
% joint angle
joint_names = {    'BasePosX'
    'BasePosY'
    'BasePosZ'
    'BaseRotX'
    'BaseRotY'
    'BaseRotZ'
    'qHRight'
    'qARight'
    'qBRight'
    'fourBarARight'
    'fourBarBRight'
    'qHLeft'
    'qALeft'
    'qBLeft'
    'fourBarALeft'
    'fourBarBLeft'};
for n = 1:N
    f = figure(1000+n);
    f.Name = joint_names{n};
    set(f, 'WindowStyle', 'docked');

    ax = axes(f);
    hold(ax);

    scatter3(ax, inpt(1,:),inpt(2,:),q_outpt(n,:), 'r');

    [S, V] = meshgrid(0:0.01:1, -2:0.04:2);
    L = zeros(size(S));
    for i = 1:size(S, 1)
        for j = 1:size(S, 2)
            L(i, j) = feval(sprintf('sagittal_library_q%i', n), ([S(i, j); V(i, j)]));
            %L(i, j) = l(n);
        end
    end

    surface(ax, S, V, L);
end
%% joint velocity
for n = 1:N
    f = figure(2000+n);
    f.Name = joint_names{n};
    set(f, 'WindowStyle', 'docked');

    ax = axes(f);
    hold(ax);

    scatter3(ax, inpt(1,:),inpt(2,:),dq_outpt(n,:), 'r');

    [S, V] = meshgrid(0:0.01:1, -2:0.01:2);
    L = zeros(size(S));
    for i = 1:size(S, 1)
        for j = 1:size(S, 2)
            L(i, j) = feval(sprintf('sagittal_library_dq%i', n), ([S(i, j); V(i, j)]));
        end
    end

    surface(ax, S, V, L);
end


function [g, kern] = pathKernDiagGradient(kern, x, covDiag)

% PATHKERNDIAGGRADIENT Compute the gradient of the PATH kernel's diagonal wrt parameters.
% FORMAT
% DESC computes the gradient of functions of the diagonal of the
% path kernel matrix with respect to the parameters of the kernel. The
% parameters' gradients are returned in the order given by the
% pathKernExtractParam command.
% ARG kern : the kernel structure for which the gradients are
% computed.
% ARG x : the input data for which the gradient is being computed.
% ARG factors : partial derivatives of the function of interest with
% respect to the diagonal elements of the kernel.
% RETURN g : gradients of the relevant function with respect to each
% of the parameters. Ordering should match the ordering given in
% pathKernExtractParam.
% RETURN kern : the updated kernel structure
%
% SEEALSO : pathKernParamInit, pathDiagGradient, pathKernExtractParam, pathKernGradient
%
% COPYRIGHT : Andrea Baisero, Carl Henrik Ek, 2013

% SHEFFIELDML

maxl=max(cellfun(@(x)size(x,1),x));
kern=pathKernUpdateWMat(kern,maxl);

num=length(x);

gwd=zeros(1,num);
gwhv=zeros(1,num);
ggk=zeros(num,kern.gkern.nParams);
for i=1:num
    li=size(x{i},1);
    w=kern.wmat(1:li,1:li);
    w=.5*(w+w(end:-1:1,end:-1:1));
    dwd=kern.dwdmat(1:li,1:li);
    dwd=.5*(dwd+dwd(end:-1:1,end:-1:1));
    dwhv=kern.dwhvmat(1:li,1:li);
    dwhv=.5*(dwhv+dwhv(end:-1:1,end:-1:1));

    gk=kernCompute(kern.gkern,x{i});
    gwd(i)=sum(sum(dwd.*gk));
    gwhv(i)=sum(sum(dwhv.*gk));
    ggk(i,:)=kernGradient(kern.gkern,x{i},w);
end

g(1,1)=sum(covDiag.*gwd);
g(1,2)=sum(covDiag.*gwhv);
for i=1:kern.gkern.nParams
    g(1,i+2)=sum(covDiag.*ggk(:,i));
end


function scr = merge(param1,param2)
% scr = merge(scr_obj_array)
% scr = merge(scr1,scr2)
% Merges Scores objects.  The function can be called in two ways: with
% two Scores objects or with an object array of Scores.
% Inputs:
%   Either:
%   scr1: A Scores object.
%   scr2: Another Scores object.
%   Or:
%   scr_obj_array: An array of Scores objects.
% Outputs:
%   scr: A Scores object that contains the information from the
%     input Scores objects.

assert(nargin<3)
if nargin==2
    scr1 = param1;
    scr2 = param2;
    assert(isa(scr1,'Scores'))
    assert(isa(scr2,'Scores'))
    assert(scr1.validate())
    assert(scr2.validate())

    scr_obj_arr = Scores.empty(2,0);
    scr_obj_arr(1) = scr1;
    scr_obj_arr(2) = scr2;
    scr = Scores.merge(scr_obj_arr);

else
    % the output scr must have all models and segment in the input
    % scrs (only once) and the union of all the scoremasks.
    % It is an error if two of the input Scores objects have a
    % score for the same trial.
    assert(nargin==1)
    obj_array = param1;
    assert(isa(obj_array,'Scores'))
    numscrs = length(obj_array);
    scr = Scores();
    for ii=1:numscrs
	scr_new = Scores();
	scr1 = scr;
	scr2 = obj_array(ii);

	% create new scr with empty matrices
	scr_new.modelset = union(scr1.modelset, scr2.modelset);
	scr_new.segset = union(scr1.segset, scr2.segset);

	% expand scr1 matrices
	scoremat_1 = zeros(length(scr_new.modelset), length(scr_new.segset));
	scoremask_1 = false(length(scr_new.modelset), length(scr_new.segset));
	[dummy,model_index_a,model_index_b] = intersect(scr_new.modelset,scr1.modelset);
	[dummy,seg_index_a,seg_index_b] = intersect(scr_new.segset,scr1.segset);
	scoremat_1(model_index_a,seg_index_a) = scr1.scoremat(model_index_b,seg_index_b);
	scoremask_1(model_index_a,seg_index_a) = scr1.scoremask(model_index_b,seg_index_b);

	% expand scr2 matrices
	scoremat_2 = zeros(length(scr_new.modelset), length(scr_new.segset));
	scoremask_2 = false(length(scr_new.modelset), length(scr_new.segset));
	[dummy,model_index_a,model_index_b] = intersect(scr_new.modelset,scr2.modelset);
	[dummy,seg_index_a,seg_index_b] = intersect(scr_new.segset,scr2.segset);
	scoremat_2(model_index_a,seg_index_a) = scr2.scoremat(model_index_b,seg_index_b);
	scoremask_2(model_index_a,seg_index_a) = scr2.scoremask(model_index_b,seg_index_b);

	% check for clashes
	assert(~any(scoremask_1(:)&scoremask_2(:)))

	% merge masks
	scoremat = scoremat_1 + scoremat_2;
	scoremask = scoremask_1 | scoremask_2;

	% build new scr
	scr_new.scoremat = scoremat;
	scr_new.scoremask = scoremask;
	scr = scr_new;
    end
end

assert(scr.validate())


% This script stores in a file the standard configuration of RP for using
% just one segmentation in LAB colorspace. The parameters were trained with
% the VOC 2007 dataset. Further details can be found in the paper:
%
%"S. Manen, M. Guillaumin, and L. Van Gool. Prime Object Proposals with
%Randomized Prim's Algorithm. In ICCV, 2013."
function generateRPConfig(rppath)
	%% Parameter specification:
	params.approxFinalNBoxes = 10000;                                    %Approximate number of proposals
	params.rSeedForRun = -1;                                            %Random seed to be used (-1 to generate it with a hashing function)
	params.q = 10;                                                      %Parameter to eliminate near duplicates (raise it to eliminate more duplicates)

	% LAB segmentation
	params.segmentations{1}.colorspace = 'LAB';                         %Colorspace: 'RGB', 'LAB', 'opponent', 'rg', 'HSV'

	% --> Segmentation parameters:
	params.segmentations{1}.superpixels.sigma = 0.8;
	params.segmentations{1}.superpixels.c = 100;
	params.segmentations{1}.superpixels.min_size = 100;
	% --> Parameters trained from VOC07:

	%   --> Feature weights:
	params.segmentations{1}.simWeights.wBias = 3.0017;
	params.segmentations{1}.simWeights.wCommonBorder = -1.0029;
	params.segmentations{1}.simWeights.wLABColorHist = -2.6864;
	params.segmentations{1}.simWeights.wSizePer = -2.3655;

	%   --> Size term alpha, as explained in the paper sec. 4.2.
	%       It is quantized to contain exactly 2^16 elements for speed
	%       purposes.
	params.segmentations{1}.alpha = dlmread(fullfile(rppath, 'config', 'alpha/alpha_voc07.dat'));
	params.segmentations{1}.verbose = false;                            %Set to true to display more information during execution

	%% Save parameters:
	save(fullfile(rppath, 'config', 'rp.mat'),'params');
end


function metric = metric_euclidean(varargin)
%METRIC_EUCLIDEAN An euclidean metric function
%
%  Description
%    METRIC = METRIC_EUCLIDEAN('PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    creates a an euclidean metric function structure in which the
%    named parameters have the specified values. Either
%    'components' or 'deltadist' has to be specified. Any
%    unspecified parameters are set to default values.
%
%    METRIC = METRIC_EUCLIDEAN(METRIC,'PARAM1',VALUE1,'PARAM2,VALUE2,...)
%    modify a metric function structure with the named parameters
%    altered with the specified values.
%
%    Parameters for Euclidean metric function [default]
%       components        - cell array of vectors specifying which
%                           inputs are grouped together with a same
%                           scaling parameter. For example, the
%                           component specification {[1 2] [3]}
%                           means that distance between 3
%                           dimensional vectors computed as
%                           r = (r_1^2 + r_2^2 )/l_1 + r_3^2/l_2,
%                           where r_i are distance along component
%                           i, and l_1 and l_2 are lengthscales for
%                           corresponding component sets. If
%                           'components' is not specified, but
%                           'deltadist' is specified, then default
%                           is {1 ... length(deltadist)}
%       deltadist         - indicator vector telling which component sets
%                           are handled using the delta distance
%                           (0 if x=x', and 1 otherwise). Default is
%                           false for all component sets.
%       lengthScale       - lengthscales for each input component set
%                           Default is 1 for each set
%       lengthScale_prior - prior for lengthScales [prior_unif]
%
%  See also
%    GP_SET, GPCF_SEXP
%
% Copyright (c) 2008 Jouni Hartikainen
% Copyright (c) 2008 Jarno Vanhatalo
% Copyright (c) 2010 Aki Vehtari

% 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 = 'METRIC_EUCLIDEAN';
  ip.addOptional('metric', [], @isstruct);
  ip.addParamValue('components',[], @(x) isempty(x) || iscell(x));
  ip.addParamValue('deltadist',[], @(x) isvector(x) || isempty(x));
  ip.addParamValue('lengthScale',[], @(x) isvector(x) && all(x>0));
  ip.addParamValue('lengthScale_prior',prior_unif, ...
                   @(x) isstruct(x) || isempty(x));
  ip.parse(varargin{:});
  metric=ip.Results.metric;

  if isempty(metric)
    % Initialize a Gaussian process
    init=true;
  else
    % Modify a Gaussian process
    if ~isfield(metric,'type') && isequal(metric.type,'metric_euclidean')
      error('First argument does not seem to be a metric structure')
    end
    init=false;
  end

  if init
    % Type
    metric.type = 'metric_euclidean';
  end

  % Components
  if init || ~ismember('components',ip.UsingDefaults)
    metric.components = ip.Results.components;
  end
  % Deltadist
  if init || ~ismember('deltadist',ip.UsingDefaults)
    metric.deltadist = ip.Results.deltadist;
  end
  % Components+Deltadist check and defaults
  if isempty(metric.components) && isempty(metric.deltadist)
    error('Either ''components'' or ''deltadist'' has to be specified')
  elseif isempty(metric.components)
    metric.components=num2cell(1:length(metric.components));
  elseif isempty(metric.deltadist)
    metric.deltadist = false(1,length(metric.components));
  end
  % Lengthscale
  if init || ~ismember('lengthScale',ip.UsingDefaults)
    metric.lengthScale = ip.Results.lengthScale;
    if isempty(metric.lengthScale)
      metric.lengthScale = repmat(1,1,length(metric.components));
    end
  end
  % Prior for lengthscale
  if init || ~ismember('lengthScale_prior',ip.UsingDefaults)
    metric.p=[];
    metric.p.lengthScale = ip.Results.lengthScale_prior;
  end

  if init
    % Set the function handles to the subfunctions
    metric.fh.pak       = @metric_euclidean_pak;
    metric.fh.unpak     = @metric_euclidean_unpak;
    metric.fh.lp        = @metric_euclidean_lp;
    metric.fh.lpg       = @metric_euclidean_lpg;
    metric.fh.dist      = @metric_euclidean_dist;
    metric.fh.distg     = @metric_euclidean_distg;
    metric.fh.ginput    = @metric_euclidean_ginput;
    metric.fh.recappend = @metric_euclidean_recappend;
  end

end

function [w s] = metric_euclidean_pak(metric)
%METRIC_EUCLIDEAN_PAK  Combine GP covariance function
%                      parameters into one vector.
%
%  Description
%    W = METRIC_EUCLIDEAN_PAK(GPCF) takes a covariance function
%    structure GPCF and combines the covariance function
%    parameters and their hyperparameters into a single row
%    vector W and takes a logarithm of the covariance function
%    parameters.
%
%       w = [ log(metric.lengthScale(:))
%             (hyperparameters of metric.lengthScale)]'
%
%  See also
%    METRIC_EUCLIDEAN_UNPAK

  w = []; s = {};
  if ~isempty(metric.p.lengthScale)
    w = log(metric.lengthScale);
    if numel(metric.lengthScale)>1
      s = [s; sprintf('log(metric.lengthScale x %d)',numel(metric.lengthScale))];
    else
      s = [s; 'log(metric.lengthScale)'];
    end
    % Hyperparameters of lengthScale
    [wh sh] = metric.p.lengthScale.fh.pak(metric.p.lengthScale);
    w = [w wh];
    s = [s; sh];
  end

end

function [metric, w] = metric_euclidean_unpak(metric, w)
%METRIC_EUCLIDEAN_UNPAK  Separate metric parameter vector into components
%
%  Description
%    METRIC, W] = METRIC_EUCLIDEAN_UNPAK(METRIC, W) takes a
%    metric structure GPCF and a parameter vector W, and returns
%    a covariance function structure identical to the input,
%    except that the covariance parameters have been set to the
%    values in W. Deletes the values set to GPCF from W and
%    returns the modified W.
%
%    The covariance function parameters are transformed via exp
%    before setting them into the structure.
%
%  See also
%    METRIC_EUCLIDEAN_PAK
%

  if ~isempty(metric.p.lengthScale)
    i2=length(metric.lengthScale);
    i1=1;
    metric.lengthScale = exp(w(i1:i2));
    w = w(i2+1:end);

    % Hyperparameters of lengthScale
    [p, w] = metric.p.lengthScale.fh.unpak(metric.p.lengthScale, w);
    metric.p.lengthScale = p;
  end
end

function lp = metric_euclidean_lp(metric)
%METRIC_EUCLIDEAN_LP  Evaluate the log prior of metric parameters
%
%  Description
%    LP = METRIC_EUCLIDEAN_LP(METRIC) takes a metric structure
%    METRIC and returns log(p(th)), where th collects the
%    parameters.
%
%  See also
%    METRIC_EUCLIDEAN_PAK, METRIC_EUCLIDEAN_UNPAK, METRIC_EUCLIDEAN_G, GP_E
%

% Evaluate the prior contribution to the error. The parameters that
% are sampled are from space W = log(w) where w is all the "real" samples.
% On the other hand errors are evaluated in the W-space so we need take
% into account also the  Jakobian of transformation W -> w = exp(W).
% See Gelman et al. (2013), Bayesian Data Analysis, third edition, p. 21.
  if ~isempty(metric.p.lengthScale)
    lp = metric.p.lengthScale.fh.lp(metric.lengthScale, metric.p.lengthScale) + sum(log(metric.lengthScale));
  else
    lp=0;
  end

end

function lpg = metric_euclidean_lpg(metric)
%METRIC_EUCLIDEAN_LPG  d log(prior)/dth of the metric parameters th
%
%  Description
%    LPG = METRIC_EUCLIDEAN_LPG(METRIC) takes a likelihood
%    structure METRIC and returns d log(p(th))/dth, where th
%    collects the parameters.
%
%  See also
%    METRIC_EUCLIDEAN_PAK, METRIC_EUCLIDEAN_UNPAK, METRIC_EUCLIDEAN, GP_E
%

% Evaluate the prior contribution of gradient with respect to lengthScale
  if ~isempty(metric.p.lengthScale)
    i1=1;
    lll = length(metric.lengthScale);
    lpgs = metric.p.lengthScale.fh.lpg(metric.lengthScale, metric.p.lengthScale);
    lpg(i1:i1-1+lll) = lpgs(1:lll).*metric.lengthScale + 1;
    lpg = [lpg lpgs(lll+1:end)];
  end
end

function gdist  = metric_euclidean_distg(metric, x, x2, mask)
%METRIC_EUCLIDEAN_DISTG  Evaluate the gradient of the metric function
%
%  Description
%    DISTG = METRIC_EUCLIDEAN_DISTG(METRIC, X) takes a metric
%    structure METRIC together with a matrix X of input
%    vectors and return the gradient matrices GDIST and
%    GPRIOR_DIST for each parameter.
%
%    DISTG = METRIC_EUCLIDEAN_DISTG(METRIC, X, X2) forms the
%    gradient matrices between two input vectors X and X2.
%
%    DISTG = METRIC_EUCLIDEAN_DISTG(METRIC, X, X2, MASK) forms
%    the gradients for masked covariances matrices used in sparse
%    approximations.
%
%  See also
%    METRIC_EUCLIDEAN_PAK, METRIC_EUCLIDEAN_UNPAK, METRIC_EUCLIDEAN, GP_E
%

  gdist=[];
  components = metric.components;

  n = size(x,1);
  m = length(components);
  i1=0;i2=1;

  % NOTE! Here we have already taken into account that the parameters
  % are transformed through log() and thus dK/dlog(p) = p * dK/dp

  if ~isempty(metric.p.lengthScale)
    if nargin <= 3
      if nargin == 2
        x2 = x;
      end
      ii1=0;

      dist  =  0;
      distc = cell(1,m);
      % Compute the distances for each component set
      for i=1:m
        if length(metric.lengthScale)==1
          s=1./metric.lengthScale.^2;
        else
          s=1./metric.lengthScale(i).^2;
        end
        distc{i} = 0;
        for j = 1:length(components{i})
          if metric.deltadist(i)
            distc{i} = distc{i} + double(bsxfun(@ne,x(:,components{i}(j)),x2(:,components{i}(j))'));
          else
            distc{i} = distc{i} + bsxfun(@minus,x(:,components{i}(j)),x2(:,components{i}(j))').^2;
          end
        end
        distc{i} = distc{i}.*s;
        % Accumulate to the total distance
        dist = dist + distc{i};
      end
      dist = sqrt(dist);
      % Loop through component sets
      if length(metric.lengthScale)==1
        D = -distc{1};
        D(dist~=0) = D(dist~=0)./dist(dist~=0);
        ii1 = ii1+1;
        gdist{ii1} = D;
      else
        for i=1:m
          D = -distc{i};
          ind = dist~=0;
          D(ind) = D(ind)./dist(ind);
          ii1 = ii1+1;
          gdist{ii1} = D;
        end
      end
% $$$         elseif nargin == 3
% $$$             if size(x,2) ~= size(x2,2)
% $$$                 error('metric_euclidean -> _ghyper: The number of columns in x and x2 has to be the same. ')
% $$$             end
    elseif nargin == 4
      gdist = cell(1,length(metric.lengthScale));
    end

    % Evaluate the prior contribution of gradient with respect to lengthScale
    if ~isempty(metric.p.lengthScale)
      i1=1;
      lll = length(metric.lengthScale);
      gg = -metric.p.lengthScale.fh.lpg(metric.lengthScale, metric.p.lengthScale);
      gprior(i1:i1-1+lll) = gg(1:lll).*metric.lengthScale - 1;
      gprior = [gprior gg(lll+1:end)];
    end
  end
end

function dist = metric_euclidean_dist(metric, x1, x2)
%METRIC_EUCLIDEAN_DIST  Compute the euclidean distence between
%                       one or two matrices.
%
%  Description
%    DIST = METRIC_EUCLIDEAN_DIST(METRIC, X) takes a metric
%    structure METRIC together with a matrix X of input
%    vectors and calculates the euclidean distance matrix DIST.
%
%    DIST = METRIC_EUCLIDEAN_DIST(METRIC, X1, X2) takes a
%    metric structure METRIC together with a matrices X1 and
%    X2 of input vectors and calculates the euclidean distance
%    matrix DIST.
%
%  See also
%    METRIC_EUCLIDEAN_PAK, METRIC_EUCLIDEAN_UNPAK, METRIC_EUCLIDEAN, GP_E
%
  if (nargin == 2 || isempty(x2))
    % use fast c-code for self-distance
    x2=x1;
    % force deltadist to be logical for simplified c-code
    metric.deltadist=logical(metric.deltadist);
    dist = dist_euclidean(metric,x1);
    if ~any(isnan(dist))
      % if c-code was available, result is not NaN
      return
    end
  end

  [n1,m1]=size(x1);
  [n2,m2]=size(x2);

  if m1~=m2
    error('the number of columns of X1 and X2 has to be same')
  end

  components = metric.components;
  m = length(components);
  dist  =  0;

  s=1./metric.lengthScale.^2;
  if m>numel(s)
    s=repmat(s,1,m);
  end
  for i=1:m
    for j = 1:length(components{i})
      if metric.deltadist(i)
        dist = dist + s(i).*double(bsxfun(@ne,x1(:,components{i}(j)),x2(:,components{i}(j))'));
      else
        dist = dist + s(i).*bsxfun(@minus,x1(:,components{i}(j)),x2(:,components{i}(j))').^2;
      end
    end
  end
  dist=sqrt(dist); % euclidean distance

end

function [ginput, gprior_input]  = metric_euclidean_ginput(metric, x1, x2)
%METRIC_EUCLIDEAN_GINPUT  Compute the gradient of the
%                         euclidean distance function with
%                         respect to input. [n, m]=size(x);
  ii1 = 0;
  components = metric.components;

  if nargin == 2 || isempty(x2)
    x2=x1;
  end

  [n1,m1]=size(x1);
  [n2,m2]=size(x2);

  if m1~=m2
    error('the number of columns of X1 and X2 has to be same')
  end

  s = 1./metric.lengthScale.^2;
  dist = 0;
  for i=1:length(components)
    for j = 1:length(components{i})
      if metric.deltadist(i)
        dist = dist + s(i).*double(bsxfun(@ne,x1(:,components{i}(j)),x2(:,components{i}(j))'));
      else
        dist = dist + s(i).*bsxfun(@minus,x1(:,components{i}(j)),x2(:,components{i}(j))').^2;
      end
    end
  end
  dist = sqrt(dist);

  for i=1:m1
    for j = 1:n1
      DK = zeros(n1,n2);
      for k = 1:length(components)
        if ismember(i,components{k})
          if metric.deltadist(i)
            DK(j,:) = DK(j,:)+s(k).*double(bsxfun(@ne,x1(j,i),x2(:,i)'));
          else
            DK(j,:) = DK(j,:)+s(k).*bsxfun(@minus,x1(j,i),x2(:,i)');
          end
        end
      end
      if nargin == 2
        DK = DK + DK';
      end
      DK(dist~=0) = DK(dist~=0)./dist(dist~=0);

      ii1 = ii1 + 1;
      ginput{ii1} = DK;
      gprior_input(ii1) = 0;
    end
  end
  %size(ginput)
  %ginput

end

function recmetric = metric_euclidean_recappend(recmetric, ri, metric)
%RECAPPEND  Record append
%
%  Description
%    RECMETRIC = METRIC_EUCLIDEAN_RECAPPEND(RECMETRIC, RI,
%    METRIC) takes old metric function record RECMETRIC, record
%    index RI and metric function structure. Appends the
%    parameters of METRIC to the RECMETRIC in the ri'th place.
%
%  See also
%    GP_MC and GP_MC -> RECAPPEND

% Initialize record
  if nargin == 2
    recmetric.type = 'metric_euclidean';
    metric.components = recmetric.components;

    % Initialize parameters
    recmetric.lengthScale = [];

    % Set the function handles
    recmetric.fh.pak       = @metric_euclidean_pak;
    recmetric.fh.unpak     = @metric_euclidean_unpak;
    recmetric.fh.lp        = @metric_euclidean_lp;
    recmetric.fh.lpg       = @metric_euclidean_lpg;
    recmetric.fh.dist      = @metric_euclidean_dist;
    recmetric.fh.distg     = @metric_euclidean_distg;
    recmetric.fh.ginput    = @metric_euclidean_ginput;
    recmetric.fh.recappend = @metric_euclidean_recappend;
    return
  end
  mp = metric.p;

  % record parameters
  if ~isempty(metric.lengthScale)
    recmetric.lengthScale(ri,:)=metric.lengthScale;
    recmetric.p.lengthScale = metric.p.lengthScale.fh.recappend(recmetric.p.lengthScale, ri, metric.p.lengthScale);
  elseif ri==1
    recmetric.lengthScale=[];
  end

end


function subpak_test04 ( )

%*****************************************************************************80
%
%% TEST04 tests AXIS_LIMITS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  xmin = 67.3;
  xmax = 114.7;
  ndivs = 6;

  [ pxmin, pxmax, pxdiv, nticks ] = axis_limits ( xmin, xmax, ndivs );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST04\n' );
  fprintf ( 1, '  AXIS_LIMITS adjusts plot limits to "nicer" values.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Input XMIN =    %f\n', xmin );
  fprintf ( 1, '  Input XMAX =    %f\n', xmax );
  fprintf ( 1, '  Input NDIVS =   %d\n', ndivs );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Output PXMIN =  %f\n', pxmin );
  fprintf ( 1, '  Output PXMAX =  %f\n', pxmax );
  fprintf ( 1, '  Output PXDIV =  %f\n', pxdiv );
  fprintf ( 1, '  Output NTICKS = %d\n', nticks );

  xmin = -26.0;
  xmax = +26.0;
  ndivs = 10;

  [ pxmin, pxmax, pxdiv, nticks ] = axis_limits ( xmin, xmax, ndivs );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Input XMIN =    %f\n', xmin );
  fprintf ( 1, '  Input XMAX =    %f\n', xmax );
  fprintf ( 1, '  Input NDIVS =   %d\n', ndivs );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Output PXMIN =  %f\n', pxmin );
  fprintf ( 1, '  Output PXMAX =  %f\n', pxmax );
  fprintf ( 1, '  Output PXDIV =  %f\n', pxdiv );
  fprintf ( 1, '  Output NTICKS = %d\n', nticks );

  return
end


function [indexSelect, infoChange] = ivmSelectPoint(model, add);

% IVMSELECTPOINT Choose a point for inclusion or removal.
% FORMAT
% DESC identifies the next point for inclusion or removal.
% ARG model : IVM structure for which the next point is being
% selected.
% ARG add : flag which indicates whether or not we are adding a
% point. If we are not adding we are assumed to be removing a point
% (default is true).
%
% SEEALSO : ivmOptimiseIvm, ivmSelectPoints, ivmCreate
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2007

% IVM

if nargin < 2
  % If add is 1, then we are including a point.
  add = true;
end

switch model.selectionCriterion
 case 'random'
  if add
    indexSelect = ceil(rand(1)*length(model.J));
    infoChange = -.5*sum(log2(1-model.varSigma(indexSelect, :).* ...
                              model.nu(indexSelect, :)), 2);
  else
    indexSelect = ceil(rand(1)*length(model.I));
    infoChange = -.5*sum(log2(1-model.varSigma(indexSelect, :).* ...
                          model.beta(indexSelect, :)+1e-300), 2);
  end
 case 'entropy'
  delta = ivmComputeInfoChange(model, add);
  [infoChange, indexSelect] = max(delta);
  numSelect = sum(delta==infoChange);
  if numSelect>1
    index1 = find(delta==infoChange);
    index1Select = ceil(rand(1)*numSelect);
    indexSelect = index1(index1Select);
  end
 case 'rentropy'
  % entropy with first point random
  if length(model.I)
    % if point is already selected select another.
    delta = ivmComputeInfoChange(model, add);
    [infoChange, indexSelect] = max(delta);
  else
    % otherwise select one randomly
    if add
      indexSelect = ceil(rand(1)*length(model.J));
      infoChange = -.5*sum(log2(1-model.varSigma(indexSelect, :)* ...
                                model.nu(indexSelect, :)), 2);
    else
      indexSelect = ceil(rand(1)*length(model.I));
      infoChange = -.5*sum(log2(1-model.varSigma(indexSelect, :)* ...
                                model.beta(indexSelect, :)+1e-300), 2);
    end
  end
end


function [net, info] = cnn_mnist_tt(varargin)
% CNN_MNIST_TT  Demonstrated TensorNet on MNIST.

% Fix the random seed.
rng(0);

opts.expDir = fullfile('data','mnist-baseline') ;
[opts, varargin] = vl_argparse(opts, varargin) ;

opts.dataDir = fullfile('data','mnist') ;
opts.imdbPath = fullfile(opts.expDir, 'imdb.mat');
opts.train.batchSize = 100 ;
opts.train.numEpochs = 100 ;
opts.train.continue = true ;
opts.train.gpus = [] ;
opts.train.learningRate = logspace(-2, -5, 45) ;
opts.train.expDir = opts.expDir ;
opts = vl_argparse(opts, varargin) ;

% --------------------------------------------------------------------
%                                                         Prepare data
% --------------------------------------------------------------------

if exist(opts.imdbPath, 'file')
  imdb = load(opts.imdbPath) ;
else
  imdb = getMnistImdb(opts) ;
  mkdir(opts.expDir) ;
  save(opts.imdbPath, '-struct', 'imdb') ;
end

net = cnn_mnist_init() ;

% --------------------------------------------------------------------
%                                                                Train
% --------------------------------------------------------------------

[net, info] = cnn_train(net, imdb, @getBatch, ...
    opts.train, ...
    'val', find(imdb.images.set == 3)) ;

% --------------------------------------------------------------------
function [im, labels] = getBatch(imdb, batch)
% --------------------------------------------------------------------
im = imdb.images.data(:,:,:,batch) ;
labels = imdb.images.labels(1,batch) ;

% --------------------------------------------------------------------
function imdb = getMnistImdb(opts)
% --------------------------------------------------------------------
% Prepare the imdb structure, returns image data with images of size 32 x 32
% and the mean image subtracted.
files = {'train-images-idx3-ubyte', ...
         'train-labels-idx1-ubyte', ...
         't10k-images-idx3-ubyte', ...
         't10k-labels-idx1-ubyte'} ;

if ~exist(opts.dataDir, 'dir')
  mkdir(opts.dataDir) ;
end

for i=1:4
  if ~exist(fullfile(opts.dataDir, files{i}), 'file')
    url = sprintf('http://yann.lecun.com/exdb/mnist/%s.gz',files{i}) ;
    fprintf('downloading %s\n', url) ;
    gunzip(url, opts.dataDir) ;
  end
end

f=fopen(fullfile(opts.dataDir, 'train-images-idx3-ubyte'),'r') ;
x1=fread(f,inf,'uint8');
fclose(f) ;
x1=permute(reshape(x1(17:end),28,28,60e3),[2 1 3]) ;

f=fopen(fullfile(opts.dataDir, 't10k-images-idx3-ubyte'),'r') ;
x2=fread(f,inf,'uint8');
fclose(f) ;
x2=permute(reshape(x2(17:end),28,28,10e3),[2 1 3]) ;

f=fopen(fullfile(opts.dataDir, 'train-labels-idx1-ubyte'),'r') ;
y1=fread(f,inf,'uint8');
fclose(f) ;
y1=double(y1(9:end)')+1 ;

f=fopen(fullfile(opts.dataDir, 't10k-labels-idx1-ubyte'),'r') ;
y2=fread(f,inf,'uint8');
fclose(f) ;
y2=double(y2(9:end)')+1 ;

set = [ones(1,numel(y1)) 3*ones(1,numel(y2))] ;
dataSmall = single(reshape(cat(3, x1, x2),28,28,1,[])) ;
% Fill the image with zeros on the border to resize it to 32 x 32.
data = zeros(32, 32, 1, size(dataSmall, 4)) ;
data(3:30, 3:30, :, :) = dataSmall ;
dataMean = mean(data(:,:,:,set == 1), 4) ;
data = bsxfun(@minus, data, dataMean) ;

imdb.images.data = data ;
imdb.images.data_mean = dataMean ;
imdb.images.labels = cat(2, y1, y2) ;
imdb.images.set = set ;
imdb.meta.sets = {'train', 'val', 'test'} ;
imdb.meta.classes = arrayfun(@(x)sprintf('%d',x),0:9,'uniformoutput',false) ;


function modelPlane = sr_extract_plane(imgPath, imgName, opt)

% SR_EXTRACT_PLANE:
%
% Extract plane model from an image
%
% The code is adapted and modified from the paper
%
% Jia-Bin Huang, Sing Bing Kang, Narendra Ahuja, Johannes Kopf
% Image Completion using Planar Structure Guidance
% ACM Transactions on Graphics (Proceedings of SIGGRAPH 2014).
%
% Output: modelPlane
% The model plane has the following data structure
%
% modelPlane
% modelPlane.vpData:                        Vanishing point data
% modelPlane.numPlane                       Number of planes
% modelPlane.plane{indPlane}.vLine          The vanishing line of the plane
% modelPlane.plane{indPlane}.imgPlaneProb;  The planar location density
% modelPlane.plane{indPlane}.sourceVP       Which two VPs form the plane
% modelPlane.plane{indPlane}.rotPar(vpInd)  Rotation parameters for aligning
%                                           two sets of lines with the x-axis                                           x-axis
% modelPlane.plane{indPlane}.postProb       Posteior probability of the plane

% =========================================================================
% Vanishing point detection
% =========================================================================

vpFilePath = 'cache\vpdetection';
%vpFilePath = [fileparts(fileparts(mfilename('fullpath'))), '\cache\vpdetection'];
vpFileName  = [imgName(1:end-4), '-vanishingpoints.txt'];

recomputeFlag = 1;
if(~exist(fullfile(vpFilePath, 'text', vpFileName), 'file') || recomputeFlag)
    vpExeFile = 'source\EdgeDetectTest.exe';
    vpDetectCMD = [vpExeFile, ' -indir ', imgPath, ' -infile ', imgName, ' -outdir ', vpFilePath];
    system(vpDetectCMD);
end
% Read vanishing point data
vpData = sr_read_vpdata(fullfile(vpFilePath, 'text', vpFileName));

img = imread(fullfile(imgPath, imgName));

% =========================================================================
% Plane localization
% =========================================================================

modelPlane = sr_detect_plane_from_vp(vpData, img, opt);
% figure(1), imshow(modelPlane.postProb(:,:,1:3));

visVPFlag = 0;
if(visVPFlag)
    [vpVis, planeVis] = vis_vp(img, vpData, modelPlane.postProb(:,:,1:3));
end
end

function [vpVis, planeVis] = vis_vp(img, vpData, postProb)

img = im2double(img);

[imgH, imgW, nCh] = size(img);
vpVis = zeros(imgH, imgW, nCh);
planeVis = zeros(imgH, imgW, nCh);
whiteImg = ones(imgH, imgW, nCh);
% blackImg = zeros()
alphaW = 0.5;

imgW = img*(1 - alphaW) + whiteImg*(alphaW);

% Plot VP
h1 = figure(1);
imshow(imgW); hold on;
lineWidth = 4;
for vpInd = 1: vpData.numVP
    vpCurr = vpData.vp{vpInd};
    for lineInd = 1: vpCurr.numLines
        lineCur = vpCurr.lines(lineInd,:);
        lineCur = lineCur + 1;
        if(vpInd == 1)
            plot(lineCur([1,3]), lineCur([2,4]), 'r', 'LineWidth', lineWidth);
        elseif(vpInd == 2)
            plot(lineCur([1,3]), lineCur([2,4]), 'g', 'LineWidth', lineWidth);
        elseif(vpInd == 3)
            plot(lineCur([1,3]), lineCur([2,4]), 'b', 'LineWidth', lineWidth);
        end
    end
end
hold off;
print(h1, '-dpng', fullfile('paper\planar_struct_SR_CVPR2015\figures\plane', 'vpdetection.png'));

% Plot posterior
alphaP = 0.9;
imgP = (1 - alphaP)*img + alphaP*postProb;
figure(2); imshow(imgP);
imwrite(imgP, fullfile('paper\planar_struct_SR_CVPR2015\figures\plane', 'planedetection.png'));

end
function vpData = sr_read_vpdata(fileName)

% SC_READ_VPDATA: read the data from vanishing point detection algorithm
% Input:
%   - fileName: the txt file containing the pre-computed vanishing point
%   detection code
% Output:
%   - vpData
%   The data structure of vpData
%       - vpData.numVP: number of detected vanishing points
%       - vpData.vp{i}.pos: the vanishing point position in the homogenous coordiante
%       - vpData.vp{i}.score: the score of the vanishing point
%       - vpData.vp{i}.numLines: number of lines supporting the vanishing point
%       - vpData.vp{i}.lines{j}.p1: (x1, y1): starting position
%       - vpData.vp{i}.lines{j}.p2: (x2, y2): ending position
%       - vpData.vp{i}.lines{j}.length: length of the line segment

vpData = [];

% Read data
fid = fopen(fileName);

%% Parse VP positions
temp = fscanf(fid, '%s ', [1 5]);
numVP = 0;
readVPFlag = 1;
VP = [];
while(readVPFlag)
    numVP = numVP + 1;
    vpCurr = fscanf(fid, '%g %g %g %g %g', [5 1]);
    if(~isempty(vpCurr))
        VP(:,numVP) = vpCurr;
    else
        temp = fscanf(fid, '%s ', [1 6]);
        readVPFlag = 0;
    end
end
VP = VP';

vpData.numVP = size(VP, 1);

% Save VP position data
for i = 1: vpData.numVP
    vpData.vp{i}.pos = VP(i, 1:3);
    vpData.vp{i}.score = VP(i, 4);
    vpData.vp{i}.numLines = VP(i, 5);
end

%% Parse each set of line segments for the corresponding VP
for i = 1: vpData.numVP
    numLine = fscanf(fid, '%d ', [1 1]);
    lines = fscanf(fid, '%g %g %g %g %g', [5 numLine]);
    vpData.vp{i}.lines = lines';
end

fclose(fid);
end

function modelPlane = sr_detect_plane_from_vp(vpData, img, opt)

% SC_DETECT_PLANE_FROM_VP: simple plane detection algorithm
% Input:
%     - vpData: vanishing point data
%     - img: input image
%     - mask: hole mask
% Output:
%     - modelPlane

%%

modelPlane = [];

% === Setting up ===
[imgH, imgW, ch] = size(img);
HfilterX = fspecial('gaussian', [1, opt.filterSize], opt.filterSigma);
HfilterY = HfilterX';
% fspecial('gaussian', opt.filterSize, opt.filterSigma);

img = im2double(img);

% === Supporting lines spatial support estimation ===
shapeInserter = vision.ShapeInserter('Shape', 'Lines','BorderColor', 'White');

for i = 1: vpData.numVP
    % The support lines
    imgLines = zeros(imgH, imgW);
    imgLines = step(shapeInserter, imgLines, int16(round(vpData.vp{i}.lines(:,1:4))));
    % Spatial density estimation via blurring
    imgLinesPosMap = imgLines;
    for k = 1:opt.numFilterIter
        imgLinesPosMap = imfilter(imgLinesPosMap, HfilterX, 'conv', 'replicate');
    end
    for k = 1:opt.numFilterIter
        imgLinesPosMap = imfilter(imgLinesPosMap, HfilterY, 'conv', 'replicate');
    end

    % Save results
    modelPlane.vp{i}.imgLines = imgLines;
    modelPlane.vp{i}.imgLinesPosMap = imgLinesPosMap;
end

% === Estimate plane support and plane parameters ===
numPlane = (vpData.numVP)*(vpData.numVP-1)/2;
% Initialize plane data
modelPlane.plane = cell(numPlane, 1);

indPlane = 1;
% A pair of vanishing points forms a plane hypothesis
for i = 1: vpData.numVP - 1
    for j = i+1: vpData.numVP
        % Compute the vanishing line
        modelPlane.plane{indPlane}.vLine = vLineFromTwoVP(vpData.vp{i}.pos, vpData.vp{j}.pos);
        % Element-wise product of two support line density
        modelPlane.plane{indPlane}.imgPlaneProb = modelPlane.vp{i}.imgLinesPosMap.*modelPlane.vp{j}.imgLinesPosMap; % Product of two probability maps

        %         modelPlane.plane{indPlane}.imgPlaneProb(mask) = 1e-10;
        modelPlane.plane{indPlane}.sourceVP = [i, j];

        indPlane = indPlane + 1;
    end
end

% === Compute rectified rotation parameters ===

for i = 1: numPlane
    for vpInd = 1: 2

        linesCurr = vpData.vp{modelPlane.plane{i}.sourceVP(vpInd)}.lines;
        invalidLineInd = linesCurr(:,5) == 0;
        linesCurr = linesCurr(~invalidLineInd,:);
        numLines = size(linesCurr, 1);

        vLineCurr = modelPlane.plane{i}.vLine;

        % Rectified homography
        H = eye(3);
        H(3,:) = vLineCurr;

    end
end

% === Add a fronto-parallel plane ===

modelPlane.plane{indPlane}.vLine = [0 0 1];
modelPlane.plane{indPlane}.imgPlaneProb = opt.fpPlaneProb*ones(imgH, imgW);
modelPlane.plane{indPlane}.score = sum(modelPlane.plane{indPlane}.imgPlaneProb(:));

numPlane = numPlane + 1;

modelPlane.numPlane = numPlane;

% === Compute posterior probability ===

planeProb = zeros(imgH, imgW, numPlane);
for i = 1 : numPlane
    planeProb(:,:,i) = modelPlane.plane{i}.imgPlaneProb;
end
planeProbSum = sum(planeProb, 3);
planeProb = bsxfun(@rdivide, planeProb, planeProbSum);
planeProb = planeProb + 0.1; % blur the posterior map
planeProb = bsxfun(@rdivide, planeProb, sum(planeProb, 3));

modelPlane.postProb = planeProb;

end

function vLine = vLineFromTwoVP(vp1, vp2)

A = cat(1, vp1, vp2);

[U S V] = svd(A, 0);
vLine = V(:,end);
vLine = vLine/vLine(3); % [h7, h8, 1]

end


function test_failed = test_libltfat_normalize(varargin)
test_failed = 0;

fprintf(' ===============  %s ================ \n',upper(mfilename));

definput.flags.complexity={'double','single'};
[flags]=ltfatarghelper({},definput,varargin);
dataPtr = [flags.complexity, 'Ptr'];

[~,~,enuminfo]=libltfatprotofile;
Cenumnorms = enuminfo.ltfat_norm_t;

Larr = [1,9,11,110];
normpairs = {{'null',Cenumnorms.LTFAT_NORM_NULL},...
             {'1', Cenumnorms.LTFAT_NORM_1},...
             {'2', Cenumnorms.LTFAT_NORM_2}};

for do_complex = 0:1
    complexstring = '';
    if do_complex, complexstring = 'complex'; end

    funname = makelibraryname('normalize',flags.complexity,do_complex);

    for L = Larr
        for npId = 1:numel(normpairs)
            normpair = normpairs{npId};

            if do_complex
                z = cast(randn(L,1)+1i*randn(L,1),flags.complexity);
                zi = complex2interleaved(z);
                zout = randn(size(zi),flags.complexity);

                ziPtr = libpointer(dataPtr,zi);
                zoutPtr = libpointer(dataPtr,zout);
            else
                z = cast(randn(L,1),flags.complexity);
                zi = z;
                zout = randn(size(zi),flags.complexity);

                ziPtr = libpointer(dataPtr,zi);
                zoutPtr = libpointer(dataPtr,zout);
            end

            trueres = normalize(z,normpair{1});

            status = calllib('libltfat',funname, ziPtr,L,normpair{2},zoutPtr);

            if do_complex
                res = norm(trueres - interleaved2complex(zoutPtr.Value));
            else
                res = norm(trueres - zoutPtr.Value);
            end

            [test_failed,fail]=ltfatdiditfail(res-status,test_failed);
            fprintf(['NORMALIZE OP L:%3i, norm: %s %s %s %s %s\n'],L,normpair{1},flags.complexity,complexstring,ltfatstatusstring(status),fail);

            status = calllib('libltfat',funname, ziPtr,L,normpair{2},ziPtr);

            if do_complex
                res = norm(trueres - interleaved2complex(ziPtr.Value));
            else
                res = norm(trueres - ziPtr.Value);
            end

            [test_failed,fail]=ltfatdiditfail(res-status,test_failed);
            fprintf(['NORMALIZE IP L:%3i, norm: %s %s %s %s %s\n'],L,normpair{1},flags.complexity,complexstring,ltfatstatusstring(status),fail);
        end
    end
end


function [ y2, m2, d2, f2 ] = ymdf_next_islamic ( y1, m1, d1, f1 )

%*****************************************************************************80
%
%% YMDF_NEXT_ISLAMIC returns the Islamic YMDF date of the next day.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y1, M1, D1, real F1,
%    the YMDF date.
%
%    Output, integer Y2, M2, D2, real F2,
%    tomorrow's YMDF date.
%
  y2 = y1;
  m2 = m1;
  d2 = d1 + 1;
  f2 = f1;

  [ y2, m2, d2 ] = day_carry_islamic ( y2, m2, d2 );

  return
end


%CDATS Support routine for checking datasets
%
% [B,C,LABLIST,P] = CDATS(A,REDUCE)
%
% INPUT
%   A        Dataset or double
%   REDUCE   0/1, Reduce A to labeled samples (1) or not (0, default), optional.
%
% OUTPUT
%   B        Dataset
%   C        Number of classes
%   LABLIST  Label list of A
%   P        Priors
%
% DESCRIPTION
% This routine supports dataset checking and conversion for mappings
% and density estimators. If A is double it is converted to a one-class
% dataset with all labels set to 1. The same holds if A is an entirely
% unlabeled dataset. The label list of A is returned in LABLIST, but
% for multi-class datasets LABLIST is set to 1. C is the number of classes
% in A. The priors are returned in P (length C), the dataset itself in B.
% If REDUCE is 1, all unlabeled objects are removed.
%
% If A has soft labels, B = A.
% If A does not have labels but targets: C = 1, LABLIST = [], P = 1.
%
% SEE ALSO (PRTools Guide)
% DATASETS, MAPPINGS

function [a,c,lablist,p] = cdats(a,red)

	if (nargin < 2), red = 0; end

	if (isdataset(a) & islabtype(a,'targets'))
		c = 1; lablist = []; p = 1;
	elseif (isdataset(a) & islabtype(a,'soft'))
		c = getsize(a,3);
		lablist = getlablist(a);
		if nargout > 3, p = getprior(a); end
	else
		if ~isvaldfile(a)
			a = prdataset(a);
      % we decided to allow doubles instead of datasets where possible.
      % However, if the user calls cdats we assume he needs a dataset
      % definitely.
		end
		c = getsize(a,3);
		if nargout > 3 % avoid unnecessary warnings
			p = getprior(a);
		end
		if (c == 0)
			a = setlabels(a,1);
			p = 1;
			c = 1;
			lablist = 1;
			prwarning(4,'Dataset unlabeled: all objects will be used')
		elseif (c == 1)
			p = 1;
			if (red == 1)
				a = seldat(a); % remove unlabeled objects
			end
			lablist = getlablist(a);
		else
			prwarning(4,'Multiclass dataset will be combined using priors')
			if (red == 1)
				a = seldat(a); % remove unlabeled objects
			end
			lablist = 1;
		end
		isvaldfile(a,1,1); % at least one object per class
	end;
return


% If all is well, all of these scripts should run without errors.

% bnets
cg1
cg2
discrete1
fa1
gaussian1
gaussian2
if exist('@gibbs_sampling_inf_engine/private/compute_posterior','file')
  % only exists if installC has been run
  gibbs_test1
end
learn1
lw1
mfa1
mixexp1
mixexp2
mixexp3
mog1
mpe1
mpe2
qmr1
qmr2
sample1
softev1
softmax1
sprinkler1

% belief propagation
belprop_polytree_discrete
belprop_polytree_gauss % alag
belprop_polytree_cg
belprop_loop1_discrete
belprop_loop1_gauss
belprop_loopy_discrete
belprop_loopy_gauss
belprop_loopy_cg % like cg1

% factor graphs
%fg1   failed since marginals were not exact

fg2
fg3
fg_mrf1
fg_mrf2

% Structure learning
bic1
cooper_yoo
k2demo1
mcmc1
model_select1
pc1
%pc2   failed due to numerical problems in KPMstats/cond_indep_fisher_z

% limids
asia_dt1
id1
oil1
pigs1

% dbns
arhmm1
bat1
bkff1
chmm1
dhmm1
filter_test1
ghmm1
kalman1
kjaerulff1
loopy_dbn1
mhmm1
mildew1
reveal1
viterbi1
water1

% HHMMs
abcd_hhmm
sample_square_hhmm_discrete
%learn_square_hhmm_cts
sample_motif_hhmm

%sparse jtree engine & ndx 2TBN engine
if exist('@jtree_sparse_inf_engine/init_pot','file')
  % only exists if installC has been run
  discrete2
  discrete3
  filter_test1
  water2
end

%find . -path '*.m' -exec wc -l {} \; | ~/count.pl

% we cannot use tic;toc to time test_BNT, since functions within this script
% reset the tic;toc timer. Hence we use the following:
%clock0=clock; cpu0 = cputime; test_BNT; cpu=cputime-cpu0; elapsed=etime(clock, clock0)


function [h, varargout] = nethess_weighted(w, net, x, t, eso_w, varargin)
%NETHESS Evaluate network Hessian
%
%	Description
%
%	H = NETHESS(W, NET, X, T) takes a weight vector W and a network data
%	structure NET, together with the matrix X of input vectors and the
%	matrix T of target vectors, and returns the value of the Hessian
%	evaluated at W.
%
%	[E, VARARGOUT] = NETHESS(W, NET, X, T, VARARGIN) also returns any
%	additional return values from the network Hessian function, and
%	passes additional arguments to that function.
%
%	See also
%	NETERR, NETGRAD, NETOPT
%

%	Copyright (c) Ian T Nabney (1996-9)

hess_str = [net.type, 'hess_weighted'];

net = netunpak(net, w);

[s{1:nargout}] = feval(hess_str, net, x, t, eso_w, varargin{:});
h = s{1};
for i = 2:nargout
  varargout{i-1} = s{i};
end


function au_qmr_ilu_m_i(A,mc,max_it_qmr,tol_qmr,tol_ilu,info_qmr)
%
%  Generates the data used in 'au_qmr_ilu_m'. Data are stored in global
%  variables.
%
%  Moreover, it defines the parameters mc, max_it_qmr, tol_qmr,
%  tol_ilu, and info_qmr, which are needed later in 'au_qmr_ilu_l_i' and
%  'au_qmr_ilu_s_i'.
%
%  Calling sequence:
%
%    au_qmr_ilu_m_i(A,mc,max_it_qmr,tol_qmr,tol_ilu,info_qmr)
%
%  Input:
%
%    A          real matrix;
%    mc         (= 'M' or 'C') Optimize for memory ('M') or computation ('C')
%               when shifted systems are solved by 'au_qmr_ilu_l/s' later.
%               Optional (default value: 'M');
%    max_it_qmr maximal number of QMR steps allowed in 'au_qmr_ilu_l/s'.
%               Corresponds to parameter 'MAXIT' in MATLAB function 'qmr'.
%               Optional (default value: min([n-1,100]));
%    tol_qmr    stopping tolerance (w.r.t. normalized residual) for QMR
%               iteration in 'au_qmr_ilu_l/s'. Corresponds to parameter 'TOL'
%               in MATLAB function 'qmr'. Optional (default value: 1e-14);
%    tol_ilu    dropping tolerance for ILU preconditioner. Corresponds to
%               parameter 'DROPTOL' in MATLAB function 'luinc'. Optional
%               (default value: 1e-2);
%    info_qmr   (= 0, 1, ...) If 1, warnings are printed when parameter FLAG
%               of MATLAB function 'qmr' is not equal to zero in QMR
%               iteration in 'au_qmr_ilu_l/s'. If 0, no warnings are
%               printed. Optional (default value: 1).
%
%  Remark:
%
%    If mc = 'M', the incomplete LU factors for A+p(i)*I are computed
%    any time 'au_qmr_ilu_s' is called, but they are not stored as global
%    variables to save memory. That means they are in general computed
%    several times, which results in additional computational cost. If
%    'mc = C', the incomplete LU factors are computed once by calling
%    'au_qmr_ilu_s_i' and stored as global variables, which results is
%    an increased memory demand.
%
%
%  LYAPACK 1.0 (Thilo Penzl, August 1999)

na = nargin;

if na<1 | na>6
  error('Wrong number of input arguments.');
end

if nargin < 2, mc = []; end
if nargin < 3, max_it_qmr = []; end
if nargin < 4, tol_qmr = []; end
if nargin < 5, tol_ilu = []; end
if nargin < 6, info_qmr = []; end

if ~length(mc), mc = 'M'; end
if ~length(max_it_qmr), max_it_qmr = min([size(A,1)-1,100]); end
if ~length(tol_qmr), tol_qmr = 1e-14; end
if ~length(tol_ilu), tol_ilu = 1e-2; end
if ~length(info_qmr), info_qmr = 1; end

global LP_A LP_MC LP_MAX_IT_QMR LP_TOL_QMR LP_TOL_ILU LP_INFO_QMR

LP_A = A;
LP_MC = mc;
LP_MAX_IT_QMR = max_it_qmr;
LP_TOL_QMR = tol_qmr;
LP_TOL_ILU = tol_ilu;
LP_INFO_QMR = info_qmr;


%-------------------------------------------------------------------------------------------------------------------%
%
% 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: May 27th, 2015
% 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 RUBBERBAND-EXAMPLE geometry and prints associated input files
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Dear_KC()

%
% Grid Parameters (MAKE SURE MATCHES IN input2d !!!)
%
Nx =  32;        % # of Eulerian Grid Pts. in x-Direction (MUST BE EVEN!!!)
Ny =  32;        % # of Eulerian Grid Pts. in y-Direction (MUST BE EVEN!!!)
Lx = 1.0;        % Length of Eulerian Grid in x-Direction
Ly = 1.0;        % Length of Eulerian Grid in y-Direction
ds = Lx/(2*Nx);  % Spatial step!

% Immersed Structure Geometric / Dynamic Parameters %
N = 2*Nx;        % Number of Lagrangian Pts. (2x resolution of Eulerian grid)
ds_Rest = 0;     % Resting length of springs
struct_name = 'dear_KC'; % Name for .vertex, .spring, etc files.

% Line on top and bottom of geometry
ds2 = 0.125*ds;
xLine = Lx/20:ds2:0.95*Lx;
yLineB = 0.025*Lx*ones(1,length(xLine));
yLineT= 0.975*Lx*ones(1,length(xLine));

xSq = [xLine xLine yLineB yLineT];
ySq = [yLineB yLineT xLine xLine];

% Call function to construct geometry
[x1,y1] = give_Me_Immsersed_Boundary_Geometry_1(Lx,Nx,ds);
x1 = [x1 xSq];
y1 = [y1 ySq];

% Call function to construct geometry
[x2,y2] = give_Me_Immsersed_Boundary_Geometry_2(Lx,Nx,ds,x1);
x2 = [x2 xSq];
y2 = [y2 ySq];

% Call function to construct geometry
[x3,y3] = give_Me_Immsersed_Boundary_Geometry_3(Lx,Nx,ds);
x3 = [x3 xSq];
y3 = [y3 ySq];

% Plot Geometry to test BEFORE taking out pts.
figure(1)
plot(x1,y1,'r*'); hold on;
axis([0 Lx 0 Ly]);

figure(2)
plot(x2,y2,'b*'); hold on;
axis([0 Lx 0 Ly]);

figure(3)
plot(x3,y3,'k*'); hold on;
axis([0 Lx 0 Ly]);

% Print files to .txt files
please_Print_Vertices_To_File(x1,y1,x2,y2,x3,y3)

% Prints .vertex file!
print_Lagrangian_Vertices(x1,y1,struct_name);

% Prints .spring file!
%k_Spring = 1e7;
%print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Rest,struct_name);

% Prints .beam file!
%k_Beam = 0.5; C = 0.0;
%print_Lagrangian_Beams(xLag,yLag,k_Beam,C,struct_name);

% Prints .target file!
k_Target = 1e6;
print_Lagrangian_Target_Pts(x1,k_Target,struct_name);

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

function print_Lagrangian_Vertices(xLag,yLag,struct_name)

    N = length(xLag);

    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 TARGET points to a file called struct.target
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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
        if s > 284
            k_Target = 2.5e9;
        end
        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.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);
            else
                %Case s=N
                fprintf(beam_fid, '%d %d %d %1.16e %1.16e\n',s-1, s, 1,   k_Beam, C);
            end
    end
    fclose(beam_fid);

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

function print_Lagrangian_Springs(xLag,yLag,k_Spring,ds_Rest,struct_name)

    N = length(xLag);

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

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

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

    %SPRINGS BETWEEN VERTICES
    for s = 1:N
            if s < N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, s+1, k_Spring, ds_Rest);
            else
                %Case s=N
                fprintf(spring_fid, '%d %d %1.16e %1.16e\n', s, 1,   k_Spring, ds_Rest);
            end
    end
    fclose(spring_fid);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for PHASE 1
%           msg: "Hi KC!!"
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry_1(Lx,Nx,ds)

% The immsersed structure message #1: Hi Nghieng!! %
len = Lx/6.5;

xC = 0;%-Lx/4;
yC = 0;%-Lx/2;
[xH,yH] = give_Me_The_Letter_Please_2(ds,len,'H',xC,yC); % (0,0)
[xi1,yi1] = give_Me_The_Letter_Please_2(ds,len,'i',xC,yC-0.05);
%
[xN,yN] = give_Me_The_Letter_Please_2(ds,len,'N',xC,yC);
[xg1,yg1] = give_Me_The_Letter_Please_2(ds,len,'g',xC,yC);
[xh,yh] = give_Me_The_Letter_Please_2(ds,len,'h',xC,yC);
[xi2,yi2] = give_Me_The_Letter_Please_2(ds,len,'i',xC,yC-0.05);
[xe,ye] = give_Me_The_Letter_Please_2(ds,len,'e',xC,yC);
[xn,yn] = give_Me_The_Letter_Please_2(ds,len,'n',xC,yC);
[xg2,yg2] = give_Me_The_Letter_Please_2(ds,len,'g',xC,yC);

% No room in domain for !'s
%[xEx1,yEx1] = give_Me_The_Letter_Please_2(ds,1.2*len,'!',xC-0.55,yC);
%[xEx2,yEx2] = give_Me_The_Letter_Please_2(ds,1.2*len,'!',xC-0.59,yC);

% HI
xH = xH + 0.45;   yH = yH + 0.7;
xi1 = xi1 + 0.55; yi1 = yi1 + 0.65;

% Nghieng
xN =  xN + 0.15;     yN = yN + 0.375;
xg1 = xg1 + 0.325;   yg1 = yg1 + 0.375;
xh =  xh + 0.435;      yh = yh + 0.375;
xi2 = xi2 + 0.515;     yi2 = yi2 + 0.325;
xe = xe + 0.59;       ye = ye + 0.375;
xn = xn + 0.7;       yn = yn + 0.375;
xg2 = xg2 + 0.8;     yg2 = yg2 + 0.375;

%
% COMBINE GEOMETRY
%
xLag = [xH xi1 xN xg1 xh xi2 xe xn xg2];
yLag = [yH yi1 yN yg1 yh yi2 ye yn yg2];

%
% Test Plot
%
% figure(1)
% plot(xH,yH,'.'); hold on;
% plot(xi1,yi1,'.'); hold on;
% plot(xN,yN,'.'); hold on;
% plot(xg1,yg1,'.'); hold on;
% plot(xh,yh,'.'); hold on;
% plot(xi2,yi2,'.'); hold on;
% plot(xe,ye,'.'); hold on;
% plot(xn,yn,'.'); hold on;
% plot(xg2,yg2,'.'); hold on;

%
% Construct Horizontal Lines for Added Pts. in Domain
%
xRow = (0:ds:Lx/3) + 1/3;
yRow = ones(size(xRow));

xRow2 = (0:ds:Lx/6+ds) + 0.41666;
yRow2 = ones(size(xRow2));

xRow_T1 = xRow; yRow_T1 = 0.925*yRow;
xRow_T2 = xRow2; yRow_T2 = 0.85*yRow2;

xRow_B1 = [xRow xRow(end)+ds]; yRow_B1 = 0.075*[yRow 1];
xRow_B2 = xRow2; yRow_B2 = 0.15*yRow2;

%
% Combine Letters + Horizontal Lines
%
xLag = [xLag xRow_T1 xRow_T2 xRow_B1 xRow_B2];
yLag = [yLag yRow_T1 yRow_T2 yRow_B1 yRow_B2];

%
% Test Plot 2
%
% figure(11)
% plot(xLag,yLag,'.'); hold on;
% axis([0 1 0 1]);

%
% CHECK TO MAKE SURE EQUAL NUMBER OF MOVING LAGS AS OTHER PHASES
% HERE: 284
%
%size(xLag)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for PHASE 2
%           msg: "Would you like to ..."
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry_2(Lx,Nx,ds,x1)

% The immsersed structure message #2: Woud you like to... %
len = Lx/8;

xC = -Lx/6;
yC = -2*Lx/3;
[xW,yW] = give_Me_The_Letter_Please(ds,len,'W',xC,yC);
[xo,yo] = give_Me_The_Letter_Please(ds,len,'o',xC-0.1,yC);
[xu,yu] = give_Me_The_Letter_Please(ds,len,'u',xC-0.18,yC);
[xl,yl] = give_Me_The_Letter_Please(ds,len,'l',xC-0.27,yC);
[xd,yd] = give_Me_The_Letter_Please(ds,1.2*len,'d',xC-0.29,yC);

[xy,yy] = give_Me_The_Letter_Please(ds,1.2*len,'y',xC-0.47,yC);
[xo2,yo2] = give_Me_The_Letter_Please(ds,len,'o',xC-0.55,yC);
[xu2,yu2] = give_Me_The_Letter_Please(ds,len,'u',xC-0.63,yC);

xC = -Lx/4;
yC = -1*Lx/4 - Lx/6;
[xl2,yl2] = give_Me_The_Letter_Please(ds,len,'l',xC,yC);
[xi,yi] = give_Me_The_Letter_Please(ds,len,'i',xC-0.01,yC);
[xk,yk] = give_Me_The_Letter_Please(ds,len,'k',xC-0.07,yC);
[xe,ye] = give_Me_The_Letter_Please(ds,len,'e',xC-0.13,yC);

[xt,yt] = give_Me_The_Letter_Please(ds,len,'t',xC-0.27,yC);
[xo3,yo3] = give_Me_The_Letter_Please(ds,len,'o',xC-0.32,yC);

yC = -1*Lx/5 - Lx/6;
[xo4,yo4] = give_Me_The_Letter_Please(ds/3,len/6,'o',xC-0.44,yC);
[xo5,yo5] = give_Me_The_Letter_Please(ds/3,len/6,'o',xC-0.48,yC);
[xo6,yo6] = give_Me_The_Letter_Please(ds/3,len/6,'o',xC-0.52,yC);

xLag = [xW xo xu xl xd xy xo2 xu2 xl2 xi xk xe xt xo3 xo4 xo5 xo6];
yLag = [yW yo yu yl yd yy yo2 yu2 yl2 yi yk ye yt yo3 yo4 yo5 yo6];

%x1Len = length(x1)
%xLagLen=length(xLag)
%PtsLeft = abs( length(xLag) - length(x1) )
%ds1 = (Lx/2.5)/( PtsLeft/2 - 1 );

xBL = 0:ds:Lx/2.5; xBL = xBL + 3/10*Lx;
yTL = 0.84*ones(1,length(xBL));
yBL = 0.18*ones(1,length(xBL));

xLag = [xLag xBL xBL];
yLag = [yLag yTL yBL];

% plot(xW,yW,'r*'); hold on;
% plot(xo,yo,'r*'); hold on;
% plot(xu,yu,'r*'); hold on;
% plot(xl,yl,'r*'); hold on;
% plot(xd,yd,'r*'); hold on;
%
% plot(xy,yy,'r*'); hold on;
% plot(xo2,yo2,'r*'); hold on;
% plot(xu2,yu2,'r*'); hold on;
%
% plot(xl2,yl2,'r*'); hold on;
% plot(xi,yi,'r*'); hold on;
% plot(xk,yk,'r*'); hold on;
% plot(xe,ye,'r*'); hold on;
%
% plot(xt,yt,'r*'); hold on;
% plot(xo3,yo3,'r*'); hold on;
%
% plot(xo4,yo4,'r*'); hold on;
% plot(xo5,yo5,'r*'); hold on;
% plot(xo6,yo6,'r*'); hold on;
% axis([0 Lx 0 Lx]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for PHASE 2
%           msg: "Would you like to ..."
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry_3(Lx,Nx,ds)

% The immsersed structure message #3: ...go on a date with me? %
len = Lx/9;

xC = -Lx/8;
yC = -3*Lx/4;
shift = 0.2;

[xo4,yo4] = give_Me_The_Letter_Please(ds/3,len/6,'o',xC,yC+0.09);
[xo5,yo5] = give_Me_The_Letter_Please(ds/3,len/6,'o',xC-0.04,yC+0.09);
[xo6,yo6] = give_Me_The_Letter_Please(ds/3,len/6,'o',xC-0.08,yC+0.09);

[xg,yg] = give_Me_The_Letter_Please(ds,len,'g',xC-shift,yC);
[xo,yo] = give_Me_The_Letter_Please(ds,len,'o',xC-0.1-shift,yC);

[xo2,yo2] = give_Me_The_Letter_Please(ds,1.2*len,'o',xC-0.25-shift,yC);
[xn,yn] = give_Me_The_Letter_Please(ds,1.2*len,'n',xC-0.34-shift,yC);

[xa,ya] = give_Me_The_Letter_Please(ds,len,'a',xC-0.48-shift,yC);

xC = -Lx/5;
yC = -Lx/2;
[xd,yd] = give_Me_The_Letter_Please(ds,len,'d',xC,yC);
[xa2,ya2] = give_Me_The_Letter_Please(ds,len,'a',xC-0.08,yC);
[xt,yt] = give_Me_The_Letter_Please(ds,len,'t',xC-0.16,yC);
[xe,ye] = give_Me_The_Letter_Please(ds,len,'e',xC-0.22,yC);

[xw,yw] = give_Me_The_Letter_Please(ds,1.1*len,'w',xC-0.37,yC);
[xi,yi] = give_Me_The_Letter_Please(ds,len,'i',xC-0.45,yC);
[xt2,yt2] = give_Me_The_Letter_Please(ds,len,'t',xC-0.505,yC);
[xh,yh] = give_Me_The_Letter_Please(ds,len,'h',xC-0.58,yC);

xC = -Lx/2.5;
yC = -Lx/4;
[xm,ym] = give_Me_The_Letter_Please(ds,1.1*len,'m',xC,yC);
[xe2,ye2] = give_Me_The_Letter_Please(ds,len,'e',xC-0.12,yC);
[xQU,yQU] = give_Me_The_Letter_Please(ds,1.2*len,'?',xC-0.22,yC);

xLag = [xo4 xo5 xo6 xg xo xo2 xn xa xd xa2 xt xe xw xi xt2 xh xm xe2 xQU];
yLag = [yo4 yo5 yo6 yg yo yo2 yn ya yd ya2 yt ye yw yi yt2 yh ym ye2 yQU];

% plot(xo4,yo4,'r*'); hold on;
% plot(xo5,yo5,'r*'); hold on;
% plot(xo6,yo6,'r*'); hold on;
%
% plot(xg,yg,'r*'); hold on;
% plot(xo,yo,'r*'); hold on;
%
% plot(xo2,yo2,'r*'); hold on;
% plot(xn,yn,'r*'); hold on;
%
% plot(xa,ya,'r*'); hold on;
%
% plot(xd,yd,'r*'); hold on;
% plot(xa2,ya2,'r*'); hold on;
% plot(xt,yt,'r*'); hold on;
% plot(xe,ye,'r*'); hold on;
%
% plot(xw,yw,'r*'); hold on;
% plot(xi,yi,'r*'); hold on;
% plot(xt2,yt2,'r*'); hold on;
% plot(xh,yh,'r*'); hold on;
%
% plot(xm,ym,'r*'); hold on;
% plot(xe2,ye2,'r*'); hold on;
% plot(xQU,yQU,'r*'); hold on;
%
% axis([0 Lx 0 Lx]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%s%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for the Heart
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry_Heart(Lx,Nx,frac,Nb)

% The immsersed structure is a heart %
ds = 2*pi/(Nb-1);
tVec = 0:ds:2*pi;

for i=1:length(tVec)
    t = tVec(i);
    xLag(i)	=	16*sin(t)^3;
    yLag(i)	=	13*cos(t)-5*cos(2*t)-2*cos(3*t)-cos(4*t);
end

xLag = frac*xLag + 0.5;
yLag = frac*yLag + 0.5;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: creates the Lagrangian structure geometry for PHASE 2
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [xLag,yLag] = give_Me_Immsersed_Boundary_Geometry_4(Lx,Nx,frac,Nb)

% The immsersed structure is a heart %
ds = 2*pi/(Nb-1);
tVec = 0:ds:2*pi;
for i=1:length(tVec)
    t = tVec(i);
    r(i) = 1 - sin(t);
    xLag(i)	=	r(i)*cos(t);
    yLag(i)	=	r(i)*sin(t);
end

xLag = frac*xLag;
yLag = frac*yLag;

minY = min(yLag);
yLag = yLag - 3*minY/8;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: take out Lagrangian Pts.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x1,y1,x2,y2] = please_Take_Out_Points(x1,y1,x2,y2)

n1 = 50;
n2 = 70;

x1 = [x1(1:n1) x1(n2:end)];
y1 = [y1(1:n1) y1(n2:end)];

x2 = [x2(1:n1) x2(n2:end)];
y2 = [y2(1:n1) y2(n2:end)];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: prints all Vertices to File
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function please_Print_Vertices_To_File(X1,Y1,X2,Y2,X3,Y3)

fileID = fopen('All_Positions.txt','w');
for j=1:length(X1)
    fprintf(fileID,'%1.16e %1.16e %1.16e %1.16e %1.16e %1.16e %1.16e %1.16e\n', X1(j),Y1(j),X2(j),Y2(j),X3(j),Y3(j),X3(j),Y3(j) );
end
fclose(fileID);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: give me the letter!!!!!
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [x,y] = give_Me_The_Letter_Please(ds,len,letter,xC,yC)

if strcmp(letter,'a')
    r = len/4.1;
    dt = ds/r;
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       xA(i) = r*cos(theta(i)) - xC;
       yA(i) = r*sin(theta(i)) - yC - len/4;
    end

    yLine = [-r:ds:r r]; yLine = yLine - yC - len/4;
    xLine = r*ones(1,length(yLine)) - (xC-ds/6);

    x = [xA xLine];
    y = [yA yLine];

elseif strcmp(letter,'C')

    r = len/2;
        dt = ds/r;
    theta = pi/2:dt:pi-0.25*dt;
    theta = [theta pi -theta];
    for i=1:length(theta)
       x(i) = 3*r/4*cos(theta(i)) - xC;
       y(i) = r*sin(theta(i)) - yC;
    end

    xTB = 0:3*ds/4:len/6;
    xTB = xTB - xC + 0.01;
    yT = r*ones(1,length(xTB))-yC;
    yB = -r*ones(1,length(xTB))-yC;

    x = [xTB x xTB];
    y = [yT y yB];

    length(x)

elseif strcmp(letter,'d')

    r = len/4.1;
    dt = ds/r;
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       xA(i) = r*cos(theta(i)) - xC;
       yA(i) = r*sin(theta(i)) - yC - len/4;
    end

    yLine = [-len/2:ds:len/2.25 r]; yLine = yLine - yC;
    xLine = r*ones(1,length(yLine)) - (xC-ds/6);

    x = [xA xLine];
    y = [yA yLine];

 elseif strcmp(letter,'e')

    r = len/4.1;
    yV = [-r:ds:r r];
    xH = yV - xC;
    yH = zeros(1,length(xH)) - yC;
    yH2 = yH - len/4;
    yH3 = yH - len/2;

    yV = yV - yC - len/4;
    xV = r*ones(1,length(yV));

    yV3 = 0:-ds:-len/4;
    yV3 = yV3 - yC;
    xV3 = r*ones(1,length(yV3));

    x = [-xV-(xC-ds/6) xH xV3-(xC-ds/6) xH xH];
    y = [yV yH yV3 yH2 yH3];

elseif strcmp(letter,'g')

    r = len/4.1;
    dt = ds/r;
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       xA(i) = r*cos(theta(i)) - xC;
       yA(i) = r*sin(theta(i)) - yC - len/4;
    end

    yLine = [-len/2:ds:len/2.25 r]; yLine = yLine - yC - len/3;
    xLine = r*ones(1,length(yLine)) - (xC-ds/6);

    yV = [-r:ds:r r];
    xH = yV - xC;
    yH = zeros(1,length(xH)) - yC - 0.85*len;

    x = [xA xLine xH];
    y = [yA yLine yH];

elseif strcmp(letter,'h')

   yLine = -len/2:ds:0-ds/4;
   yLine = [yLine 0 -yLine];
   yLine1 = yLine - yC;
   xLine1 = 1/2*len/2*ones(1,length(yLine1));

   yLine2 = -len/2:ds:0-ds/4;
   yLine2 = [yLine2 0];
   yLine2 = yLine2 - yC;
   xLine2 = 1/2*len/2*ones(1,length(yLine2));

   xH = (-1/2*len/2+ds/2:ds:1/2*len/2-ds/4);
   xH = xH - xC;
   yH = zeros(1,length(xH)) - yC;

   x = [-xLine1-xC xH xLine2-xC];
   y = [yLine1 yH yLine2];

elseif strcmp(letter,'H')
   yLine = -len/2:ds:0-ds/4;
   yLine = [yLine 0 -yLine];
   yLine = yLine - yC;
   xLine = 1/2*len/2*ones(1,length(yLine));
   xH = (-1/2*len/2+ds/2:ds:1/2*len/2-ds/4);
   xH = xH - xC;
   yH = zeros(1,length(xH)) - yC;

   x = [-xLine-xC xH xLine-xC];
   y = [yLine yH yLine];

elseif strcmp(letter,'i')

   yLine = -len/2:ds:0-ds/4;
   yLine = yLine - yC;
   xLine = zeros(1,length(yLine)) - xC;

    r = len/21;
    dt = ds/(2*r);
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       xDot(i) = r*cos(theta(i)) - xC;
       yDot(i) = r*sin(theta(i)) - yC + len/7;
    end

    x = [xLine xDot];
    y = [yLine yDot];

elseif strcmp(letter,'k')

   yLine = -len/2:ds:len/2;
   yLine = yLine - yC;
   xLine = zeros(1,length(yLine)) -xC-len/3.9;

   XD = 0:ds:len/sqrt(2);
   YD = zeros(1,length(XD));

   %rotate!
   for i=1:length(XD)
      xDt(i) = XD(i)*cos(pi/4)-YD(i)*sin(pi/4);
      yDt(i) = XD(i)*sin(pi/4)+YD(i)*cos(pi/4);

      xDb(i) = XD(i)*cos(pi/4)-YD(i)*sin(-pi/4);
      yDb(i) = XD(i)*sin(-pi/4)+YD(i)*cos(pi/4);
   end
   xDt = xDt - xC - len/4;
   yDt = yDt - yC;

   xDb = xDb - xC - len/4;
   yDb = yDb - yC;

   x = [xLine(1:ceil(0.75*end)) xDt(1:ceil(2*end/3)) xDb(1:ceil(2*end/3))];
   y = [yLine(1:ceil(0.75*end)) yDt(1:ceil(2*end/3))-len/8 yDb(1:ceil(2*end/3))-len/5.5];

elseif strcmp(letter,'K')

   yLine = -len/2:ds:len/2;
   yLine = yLine - yC;
   xLine = zeros(1,length(yLine)) -xC-len/3.9;

   XD = 0:ds:len/sqrt(2);
   YD = zeros(1,length(XD));

   %rotate!
   for i=1:length(XD)
      xDt(i) = XD(i)*cos(pi/4)-YD(i)*sin(pi/4);
      yDt(i) = XD(i)*sin(pi/4)+YD(i)*cos(pi/4);

      xDb(i) = XD(i)*cos(pi/4)-YD(i)*sin(-pi/4);
      yDb(i) = XD(i)*sin(-pi/4)+YD(i)*cos(pi/4);
   end
   xDt = xDt - xC - len/4.5;
   yDt = yDt - yC;

   xDb = xDb - xC - len/5.5;
   yDb = yDb - yC;

   x = [xLine xDt xDb];
   y = [yLine yDt yDb];

elseif strcmp(letter,'l')

   yLine = -len/2:ds:len/2.25;
   yLine = yLine - yC;
   xLine = 1/2*len/2*ones(1,length(yLine));

   x = -xLine-xC;
   y = yLine;

elseif strcmp(letter,'m')

    r = len/2;
    xH = -r:ds:r;  xH = xH - xC;
    yH = zeros(1,length(xH)) - yC;
    yV = 0:-ds:-len/2; yV = yV - yC;
    xV = -r*ones(1,length(yV)) - xC;
    xV2 = zeros(1,length(yV)) - xC;
    xV3 = r*ones(1,length(yV)) - xC;

    yVS = 0:ds:len/12;
    yVS = yVS - yC;
    xVS = -r*ones(1,length(yVS)) - xC;

    x = [xVS xH xV xV2 xV3];
    y = [yVS yH yV yV yV];

elseif strcmp(letter,'n')

    r = len/4.1;
    yV = [-r:ds:r r];
    xH = yV - xC;
    yH = zeros(1,length(xH)) - yC;

    yV = yV - yC - len/4;
    xV = r*ones(1,length(yV));

    yV2 = 0:ds:len/12;
    yV2 = yV2 - yC;
    xV2 = r*ones(1,length(yV2));

    x = [-xV-(xC-ds/6) -xV2-(xC-ds/6) xH xV-(xC-ds/6)];
    y = [yV yV2 yH yV];

elseif strcmp(letter,'o')

    r = len/4.1;
    dt = ds/r;
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       x(i) = r*cos(theta(i)) - xC;
       y(i) = r*sin(theta(i)) - yC - len/4;
    end

 elseif strcmp(letter,'r')

    r = len/4.1;
    yV = [-r:ds:r r];
    xH = yV - xC;
    yH = zeros(1,length(xH)) - yC;

    yV = yV - yC - len/4;
    xV = r*ones(1,length(yV));

    yV2 = 0:ds:len/12;
    yV2 = yV2 - yC;
    xV2 = r*ones(1,length(yV2));

    yV3 = 0:-ds:-len/10;
    yV3 = yV3 - yC;
    xV3 = r*ones(1,length(yV3));

    x = [-xV-(xC-ds/6) -xV2-(xC-ds/6) xH xV3-(xC-ds/6)];
    y = [yV yV2 yH yV3];

elseif strcmp(letter,'t')

   yLine = -len/2:ds:len/2.25;
   yLine = yLine - yC;
   xLine = zeros(1,length(yLine));

   xH = -len/4:ds:len/4;
   xH = xH-xC;
   yH = zeros(1,length(xH)) - yC + len/9;

   x = [xLine-xC xH];
   y = [yLine yH];

elseif strcmp(letter,'u')

    r = len/4.1;
    yV = [-r:ds:r r];
    xH = yV - xC;
    yH = zeros(1,length(xH)) - yC - len/2;

    yV = yV - yC - len/4;
    xV = r*ones(1,length(yV));

    x = [-xV-(xC-ds/6) xH xV-(xC-ds/6)];
    y = [yV yH yV];

elseif strcmp(letter,'v')

   XD = 0:ds:sqrt(5)*len/4;
   YD = zeros(1,length(XD));

   %rotate!
   ang = atan(2);
   for i=1:length(XD)
      xDr(i) = XD(i)*cos(ang)-YD(i)*sin(ang);
      yDr(i) = XD(i)*sin(ang)+YD(i)*cos(ang);

      xDL(i) = XD(i)*cos(pi-ang)-YD(i)*sin(pi-ang);
      yDL(i) = XD(i)*sin(pi-ang)+YD(i)*cos(pi-ang);
   end
   xDr = xDr - xC;
   yDr = yDr - yC - len/2;

   xDL = xDL - xC;
   yDL = yDL - yC - len/2;

   x = [xDL xDr(2:end)];
   y = [yDL yDr(2:end)];

elseif strcmp(letter,'w')

   XD = 0:ds:sqrt(5)*len/4;
   YD = zeros(1,length(XD));

   %rotate!
   ang = atan(2);
   for i=1:length(XD)
      xDr(i) = XD(i)*cos(ang)-YD(i)*sin(ang);
      yDr(i) = XD(i)*sin(ang)+YD(i)*cos(ang);

      xDL(i) = XD(i)*cos(pi-ang)-YD(i)*sin(pi-ang);
      yDL(i) = XD(i)*sin(pi-ang)+YD(i)*cos(pi-ang);
   end
   xDr = xDr - xC - len/4;
   yDr = yDr - yC - len/2;

   xDL = xDL - xC - len/4;
   yDL = yDL - yC - len/2;

   x = [xDL xDr(2:end) xDL+len/2 xDr(2:end)+len/2];
   y = [yDL yDr(2:end) yDL yDr(2:end)];

elseif strcmp(letter,'W')

   XD = 0:ds:sqrt(5)*len/2;
   YD = zeros(1,length(XD));

   %rotate!
   ang = atan(2);
   for i=1:length(XD)
      xDr(i) = XD(i)*cos(ang)-YD(i)*sin(ang);
      yDr(i) = XD(i)*sin(ang)+YD(i)*cos(ang);

      xDL(i) = XD(i)*cos(pi-ang)-YD(i)*sin(pi-ang);
      yDL(i) = XD(i)*sin(pi-ang)+YD(i)*cos(pi-ang);
   end
   xDr = xDr - xC - len/4;
   yDr = yDr - yC - len/2;

   xDL = xDL - xC - len/4;
   yDL = yDL - yC - len/2;

   x = [xDL xDr(2:floor(end/2)) xDL(1:floor(end/2))+len/2 xDr(2:end)+len/2];
   y = [yDL yDr(2:floor(end/2)) yDL(1:floor(end/2))       yDr(2:end)];

elseif strcmp(letter,'y')

   XD = 0:ds:sqrt(5)*len/4;
   YD = zeros(1,length(XD));

   %rotate!
   ang = atan(2);
   for i=1:length(XD)
      xDr(i) = XD(i)*cos(ang)-YD(i)*sin(ang);
      yDr(i) = XD(i)*sin(ang)+YD(i)*cos(ang);

      xDL(i) = XD(i)*cos(pi-ang)-YD(i)*sin(pi-ang);
      yDL(i) = XD(i)*sin(pi-ang)+YD(i)*cos(pi-ang);

      xB(i) = XD(i)*cos(pi+ang)-YD(i)*sin(pi+ang);
      yB(i) = XD(i)*sin(pi+ang)+YD(i)*cos(pi+ang);
   end
   xDr = xDr - xC;
   yDr = yDr - yC - len/2;

   xDL = xDL - xC;
   yDL = yDL - yC - len/2;

   xB = xB - xC;
   yB = yB - yC - len/2;

   x = [xDL xDr(2:end) xB(2:end)];
   y = [yDL yDr(2:end) yB(2:end)];

elseif strcmp(letter,'!')

   yLine = -len/4:ds:len/2;
   yLine = yLine - yC;
   xLine = zeros(1,length(yLine)) - xC;

    r = len/21;
    dt = ds/(2*r);
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       xDot(i) = r*cos(theta(i)) - xC;
       yDot(i) = r*sin(theta(i)) - yC - len/2.1;
    end

    x = [xLine xDot];
    y = [yLine yDot];

elseif strcmp(letter,'?')

    %xT = 0:ds:len/8-ds; xT = xT - xC - len/4;
    %yT = zeros(1,length(xT)) - yC + len/2;

    r = len/4;
    dt = ds/r;
    theta = 1.25*pi/2:-dt:-pi/2;
    for i=1:length(theta)
       xL(i) = 1.5*r*cos(theta(i)) - xC - len/8;
       yL(i) = r*sin(theta(i)) - yC + len/4;
    end

    yV = -ds:-ds:-len/3.25;
    xV = zeros(1,length(yV)) - xC - len/8;

    r = len/21;
    dt = ds/(2*r);
    theta = 0:dt:2*pi;
    for i=1:length(theta)
       xDot(i) = r*cos(theta(i)) - xC -len/8;
       yDot(i) = r*sin(theta(i)) - yC - len/2.1;
    end

    x = [xL xV xDot];
    y = [yL yV yDot];

end


function sys = dual(X)
%DUAL Extract dual variable
%
%   Z = DUAL(F)     Returns the dual variable for the constraint F
%
%   See also SOLVESDP, DUALIZE

sys = dual(lmi(X));


function varargout = interp_ungridded(pos_from, pos_to, varargin)

% INTERP_UNGRIDDED computes an interpolation matrix for two clouds of 3-D points
%
% To get the interpolated data, use as
%   [valto] = interp_ungridded(pos_from, pos_to, 'data', valfrom, ...)
% or to get the interpolation matrix itself, use as
%   [interpmat, distmat] = interp_ungridded(pos_from, pos_to, ...)
% where
%   pos_from  Nx3 matrix with the vertex positions
%   pos_to    Mx3 matrix with the vertex positions onto which the data should be interpolated
%
% Optional arguments are specified in key-value pairs and can be
%    data         = NxK matrix with functional data
%    distmat      = NxM matrix with precomputed distances
%    projmethod   = 'nearest', 'sphere_avg', 'sphere_weighteddistance', 'smudge'
%    triout       = triangulation for the second set of vertices
%    sphereradius = scalar
%    power        = scalar, power parameter as in the Inverse Distance Weighting function proposed by Shepard (default = 1).

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

if nargin<3
  ft_error('Not enough input arguments.');
end

% get the optional arguments
projmethod   = ft_getopt(varargin, 'projmethod');     % required
sphereradius = ft_getopt(varargin, 'sphereradius');   % required for some projection methods
powerparam   = ft_getopt(varargin, 'power', 1);
distmat      = ft_getopt(varargin, 'distmat');        % will be computed if needed and not present
triout       = ft_getopt(varargin, 'triout');
dat          = ft_getopt(varargin, 'data');           % functional data defined at pos_from
inside       = ft_getopt(varargin, 'inside');

hasdat    = ~isempty(dat);
hasinside = ~isempty(inside);
npos_from = size(pos_from, 1);
npos_to   = size(pos_to, 1);
dimres    = sqrt(sum((pos_to(2,:)-pos_to(1,:)).^2,2));

if hasinside
  % convert to boolean vector
  tmp         = false(npos_from,1);
  tmp(inside) = true;
  inside      = tmp;
  clear tmp;
else
  inside      = true(npos_from,1);
end

if isempty(distmat)
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % compute a distance matrix
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  switch projmethod
    case 'nearest'
      if ~isempty(sphereradius)
        ft_warning('sphereradius is not used for projmethod ''nearest''');
      end

      % determine the nearest voxel for each surface point
      ind     = find_nearest(pos_to, pos_from, 5);
      sel     = ind>0;
      indx    = 1:npos_to;
      distmat = sparse(indx(sel), ind(sel), ones(size(ind(sel))), npos_to, npos_from);

    case {'sphere_avg', 'sphere_weighteddistance'}
      if isempty(sphereradius)
        ft_error('sphereradius should be specified');
      end

      % compute the distance between voxels and each surface point
      dpos_fromsq = sum(pos_from.^2,2); % squared distance to origin
      dpos_tosq   = sum(pos_to.^2,2); % squared distance to origin
      maxnpnt = double(npos_to*ceil(4/3*pi*(sphereradius/max(dimres))^3)); % initial estimate of nonzero entries
      maxnpnt = min(maxnpnt, npos_to*npos_from);
      val   = nan(maxnpnt, 1);
      indx1 = nan(maxnpnt, 1);
      indx2 = nan(maxnpnt, 1);
      cnt = 1;
      for j = 1:npos_to

        % d   = dpos_tosq(j) + dpos_fromsq - 2 * pos_from * pos_to(j,:)';
        % sel = find(d<sphereradius.^2);
        % the following lines are equivalent to the previous 2 but uses fewer flops
        d    = sqrt(dpos_fromsq - 2 * pos_from * pos_to(j,:)' + dpos_tosq(j));
        sel  = find(d < sphereradius & inside);

        nsel = numel(sel);
        if nsel>0
          indx1(cnt:(cnt+nsel-1)) = j(ones(nsel,1));
          indx2(cnt:(cnt+nsel-1)) = sel(:);
          val(cnt:(cnt+nsel-1))   = d(sel) + eps('double');
          cnt = cnt + nsel;
        end
      end
      indx1(isnan(indx1)) = [];
      indx2(isnan(indx2)) = [];
      val(isnan(val))     = [];
      distmat = sparse(indx1, indx2, val, npos_to, npos_from);

    case 'smudge'
      if isempty(triout)
        ft_error('the ''smudge'' method needs a triangle definition');
      end

      [datin, loc] = ismember(pos_to, pos_from, 'rows');  % note that small numerical errors can cause them to be different
      [datout, S1] = smudge(datin, triout, 6);            % FIXME 6 is number of iterations, improve here

      sel = find(datin);
      S2  = sparse(sel(:), loc(datin), ones(npos_from,1), npos_to, npos_from);
      distmat = S1 * S2;

    otherwise
      ft_error('unsupported projection method');
  end % case projmethod
end % if isempty distmat

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% do something with the distance matrix
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch projmethod
  case 'nearest'
    projmat         = distmat;

  case 'sphere_avg'
    projmat         = distmat;
    [ind1, ind2]    = find(projmat);
    nnz             = full(sum(spones(projmat),2));
    for k = 1:length(ind1)
      projmat(ind1(k),ind2(k)) = 1./nnz(ind1(k));
    end

  case 'sphere_weighteddistance'
    projmat         = distmat;
    [ind1, ind2, d] = find(projmat);
    projmat         = sparse(ind1, ind2, d.^-powerparam, npos_to, npos_from);
    [ind1, ind2, d] = find(projmat);
    normnz          = full(sum(projmat, 2));
    projmat         = sparse(ind1, ind2, d./normnz(ind1), npos_to, npos_from);

  case 'smudge'
    projmat = distmat;

  otherwise
    ft_error('unsupported projection method');
end  % case projmethod

if hasdat
  % return the interpolated values
  varargout{1} = projmat * dat;
else
  % return the interpolation and the distance matrix
  varargout{1} = projmat;
  varargout{2} = distmat;
end


function [G]=gsp_logo()
%GSP_LOGO Initialize a graph with the GSP logo
%   Usage:  G = gsp_logo();
%
%   Output parameters:
%         G     : Graph structure.
%
%   'gsp_logo()' initializes a graph structure containing the GSP logo
%
%   Example:::
%
%          G = gsp_logo();
%          gsp_plot_graph(G);
%

% Author : Johan Paratte
% Test:

load logogsp

G.W = W;
G.coords = coords;
G.info.idx_g = idx_g;
G.info.idx_s = idx_s;
G.info.idx_p = idx_p;
%G.plotting.vertex_color = [200 136/255.0 204/255.0];
%G.plotting.edge_color = [0 136/255.0 204/255.0];
G.plotting.vertex_size = 20;
G.limits = [0 640 -400 0];
G = gsp_graph_default_parameters(G);

end


function feats =concatenate_feat(frame_feat, nprev, nnext)
% Copyright (c) 2014-present University of Illinois at Urbana-Champaign
% All rights reserved.
%
% Developed by:     Po-Sen Huang, Paris Smaragdis
%                   Department of Electrical and Computer Engineering
%                   Department of Computer Science
%
% concatenate nprev + self + nnext frames together

    [nframes, ndim] = size(frame_feat);
    nDim= (nprev+nnext+1)*ndim;

    % duplicate the beginning and the end of frame
    begin_frame = repmat(frame_feat (1, :), nprev, 1);
    end_frame = repmat(frame_feat ( end, : ), nnext, 1) ;

    frame_feat2 = [begin_frame; frame_feat; end_frame];

    % original frame list
    ndx= nprev+1: nprev+nframes;

    % frame list + nprev + nnext frames
    ndx_merge= repmat( ndx, nprev+nnext+1, 1) + ...
               repmat( (0:nprev+nnext)'-nprev, 1, nframes) ;

    ndx_merge=ndx_merge(:);

    % merge them together
    feats=reshape( frame_feat2(ndx_merge,:)', nDim, nframes)';
end


function [nodes2, edges2] = grRemoveNodes(nodes, edges, rmNodes)
%GRREMOVENODES Remove several nodes in a graph
%
%   usage:
%   [NODES2 EDGES2] = grRemoveNodes(NODES, EDGES, NODES2REMOVE)
%   remove the nodes with indices NODE2REMOVE from array NODES, and also
%   remove edges containing the nodes NODE2REMOVE.
%
%   Example
%     nodes = [...
%         10 10; 20 10; 30 10; ...
%         10 20; 20 20; 30 20];
%     edges = [...
%         1 2; 1 4; 1 5; ...
%         2 3; 2 5; 2 6; ...
%         3 6; 4 5; 5 6];
%     toRemove = [3 4];
%     [nodes2 edges2] = grRemoveNodes(nodes, edges, toRemove);
%     drawGraph(nodes2, edges2);
%     axis equal; axis([0 40 0 30]);
%
%   See also
%     grRemoveEdges
%

%   -----
%   author: David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 13/08/2003.
%

%   HISTORY
%   10/02/2004 doc
%   07/03/2014 rewrite using clearer algorithm

%% edges processing

% remove all edges connected to one of the nodes to remove
edges2 = edges(~any(ismember(edges, rmNodes), 2), :);

% change edges information, due to the node index shift
for i = 1:length(rmNodes)
    inds = edges2 > (rmNodes(i) - i + 1);
    edges2(inds) = edges2(inds) - 1;
end

%% nodes processing

% number of nodes
N   = size(nodes, 1);
NR  = length(rmNodes);
N2  = N-NR;

% allocate memory
nodes2 = zeros(N2, 2);

% process the first node
nodes2(1:rmNodes(1)-1,:) = nodes(1:rmNodes(1)-1,:);

for i = 2:NR
    inds = rmNodes(i-1)+1:rmNodes(i)-1;
    if isempty(inds)
        continue;
    end
    nodes2(inds - i + 1, :) = nodes(inds, :);
end

% process the last node
nodes2(rmNodes(NR)-NR+1:N2, :) = nodes(rmNodes(NR)+1:N, :);


classdef HuberLoss < dagnn.Loss

  properties
    sigma = 1.
  end

  methods
    function outputs = forward(obj, inputs, params)
      outputs{1} = vl_nnhuberloss(inputs{1}, inputs{2}, ...
                       'instanceWeights', inputs{3}, ...
                       obj.opts{:}) ;
      n = obj.numAveraged ;
      m = n + size(inputs{1}, 4) ;
      obj.average = (n * obj.average + gather(outputs{1})) / m ;
      obj.numAveraged = m ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
      derInputs{1} = vl_nnhuberloss(inputs{1}, inputs{2}, derOutputs{1}, ...
                                'instanceWeights', inputs{3}, ...
                                obj.opts{:}) ;
      derInputs{2} = [] ;
      derInputs{3} = [] ;
      derParams = {} ;
    end

    function obj = HuberLoss(varargin)
      obj.load(varargin) ;
      obj.sigma = obj.sigma ;
    end
  end
end


function asa183_test03 ( )

%*****************************************************************************80
%
%% ASA183_TEST03 tests R8_RANDOM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ASA183_TEST03\n' );
  fprintf ( 1, '  Show how the seeds used by R8_RANDOM,\n' );
  fprintf ( 1, '  which change on each step, can be reset to\n' );
  fprintf ( 1, '  restore any part of the sequence.\n' );

  s1_save = 12345;
  s2_save = 34567;
  s3_save = 56789;

  s1 = s1_save;
  s2 = s2_save;
  s3 = s3_save;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Begin sequence with following seeds:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  S1 = %d\n', s1 );
  fprintf ( 1, '  S2 = %d\n', s2 );
  fprintf ( 1, '  S3 = %d\n', s3 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '         I      R                 S1        S2        S3\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    [ r, s1, s2, s3 ] = r8_random ( s1, s2, s3 );
    fprintf ( 1, '  %8d  %14f  %8d  %8d  %8d\n', i, r, s1, s2, s3 );

    if ( i == 5 )
      s1_save = s1;
      s2_save = s2;
      s3_save = s3;
    end

  end

  s1 = s1_save;
  s2 = s2_save;
  s3 = s3_save;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Restart the sequence, using the seeds\n' );
  fprintf ( 1, '  produced after step 5:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '         I      R                 S1        S2        S3\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    [ r, s1, s2, s3 ] = r8_random ( s1, s2, s3 );
    fprintf ( 1, '  %8d  %14f  %8d  %8d  %8d\n', i, r, s1, s2, s3 );

  end

  return
end


% this script tests 1st Order Statistics features between CERR and pyradiomics on a Wavelet filetered image.
%
% RKP, 03/22/2018

%% Load image
firstOrderParamFileName = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing','tests_for_cerr','test_first_order_radiomics_extraction_settings.json');
cerrFileName = fullfile(fileparts(fileparts(getCERRPath)),...
    'Unit_Testing','data_for_cerr_tests','CERR_plans','head_neck_ex1_20may03.mat.bz2');

planC = loadPlanC(cerrFileName,tempdir);
indexS = planC{end};

paramS = getRadiomicsParamTemplate(firstOrderParamFileName);
strNum = getMatchingIndex(paramS.structuresC{1},{planC{indexS.structures}.structureName});
scanNum = getStructureAssociatedScan(strNum,planC);

%wavType = 'coif1';
%scanType = 'Original';
scanType = 'Wavelet';
%dirString = 'HLH';
dirString = paramS.imageType.Wavelets.Direction.val;

%% Calculate features using CERR

firstOrderS = calcGlobalRadiomicsFeatures...
            (scanNum, strNum, paramS, planC);
firstOrderS = firstOrderS.(['Wavelets_coif_1_',dirString]).firstOrderS;
cerrFirstOrderV = [firstOrderS.energy, firstOrderS.totalEnergy, firstOrderS.interQuartileRange, ...
    firstOrderS.kurtosis+3, firstOrderS.max, firstOrderS.mean, firstOrderS.meanAbsDev, ...
    firstOrderS.median, firstOrderS.medianAbsDev, firstOrderS.min, ...
    firstOrderS.P10, firstOrderS.P90, firstOrderS.interQuartileRange, ...
    firstOrderS.robustMeanAbsDev, firstOrderS.rms, firstOrderS.skewness, ...
    firstOrderS.std, firstOrderS.var, firstOrderS.entropy];

% %% Calculate features using pyradiomics
%
% testM = single(planC{indexS.scan}(scanNum).scanArray) - ...
%     single(planC{indexS.scan}(scanNum).scanInfo(1).CTOffset);
% mask3M = zeros(size(testM),'logical');
% [rasterSegments, planC, isError] = getRasterSegments(strNum,planC);
% [maskBoundBox3M, uniqueSlices] = rasterToMask(rasterSegments, scanNum, planC);
% mask3M(:,:,uniqueSlices) = maskBoundBox3M;
% dx = planC{indexS.scan}(scanNum).scanInfo(1).grid1Units;
% dy = planC{indexS.scan}(scanNum).scanInfo(1).grid1Units;
% dz = mode(diff([planC{indexS.scan}(scanNum).scanInfo(:).zValue]));
% pixelSize = [dx dy dz]*10;
% teststruct = PyradWrapper(testM, mask3M, pixelSize, scanType, dirString);
%
% pyradFirstorderNamC = {'Energy', 'TotalEnergy','InterquartileRange','Kurtosis',...
%     'Maximum', 'Mean','MeanAbsoluteDeviation','Median','medianAbsDev',...
%     'Minimum','10Percentile','90Percentile','InterquartileRange',...
%     'RobustMeanAbsoluteDeviation','RootMeanSquared','Skewness',...
%     'StandardDeviation','Variance','Entropy'};
%
% pyradFirstorderNamC = strcat(['wavelet', '_', dirString, '_firstorder_'],pyradFirstorderNamC);
% %pyradFirstorderNamC = strcat(['original', '_firstorder_'],pyradFirstorderNamC);
%
% pyRadFirstOrderV = [];
% for i = 1:length(pyradFirstorderNamC)
%     if isfield(teststruct,pyradFirstorderNamC{i})
%         pyRadFirstOrderV(i) = teststruct.(pyradFirstorderNamC{i});
%     else
%         pyRadFirstOrderV(i) = NaN;
%     end
% end
%
% %% Compare
% diffFirstOrderV = (cerrFirstOrderV - pyRadFirstOrderV) ./ cerrFirstOrderV * 100

%% Compare with previously calculated values of pyradiomics first order features
saved_pyRadFirstOrderV = [87498183.4756395,1049978201.70767,30.4772882461548,16.3069672735214,836.854187011719,-0.191955631352121,48.4171827693622,0.0908048748970032,NaN,-718.366882324219,-73.1680297851562,64.9023208618165,30.4772882461548,16.4187532424825,96.2495122333083,0.546840923205712,NaN,9263.93175818536,3.17432864851443];
diffFirstOrderV = (cerrFirstOrderV - saved_pyRadFirstOrderV) ./ cerrFirstOrderV * 100


function sleep_stage = ECG_sleep_staging_prediction(ECGdata,fs,class)
% sleep stage prediction from ECG signal by model trained from SHHSv1 DB
%
% input:
%   ECGdata: ECG data
%   fs: sampling frequency of ECG
%   class: sleep staging type: class = 1, four types classification
%                              class = 4, two types classification
%
% output:
%   sleep_stage:  = 1: Wake,
%                   2: REM sleep,
%                   3: NREM light sleep,
%                   4: NREM deep sleep, when class = 1
%   sleep_stage:  = 1: NREM sleep,
%                   2: Wake+REM, when class = 4

classesn=[4 3 3 2 2];
numClasses=classesn(class);

% best parameters
filterSize=6;
balanced=1;
selectn = 3;

% 70 (10, sdnn), 73(14,avgsqi), 78(20, lfhf), 80(ac),81(dc),85(28, sampEn)
selects={[1:86],[1:69 71 72 74:77 79 82:84 86],[51:86],[51:69 71 72 74:77 79 82:84 86]};
used=selects{selectn};

load(['SHHS_ECG_model_' num2str(filterSize) '_' num2str(class) '_' num2str(balanced) '_' num2str(selectn) '_for_prediction'],'C','ka','parameters','hyperparameters','features_mean','features_std');

% calculate features

features=ECG_sleep_staging_features_extraction(ECGdata,fs);

if ~isempty(features)
    doTraining=false;

    dlXTest=features;

    for l=1:length(features_mean)
            dlXTest(l,:)=bsxfun(@minus, dlXTest(l,:),features_mean(l));
            dlXTest(l,:)=bsxfun(@rdivide, dlXTest(l,:),features_std(l));
    end

    dlXTest=dlarray(dlXTest(used,:));
%     dlYTest = dlarray(s.Ydata_all);

    dimension = size(dlXTest,1);

    dlXTest=reshape(dlXTest,dimension,1,[]);
    dlYPred = model(dlXTest,parameters,hyperparameters,doTraining);
    dlYPred = softmax(dlYPred,'DataFormat','CBT');

%     YPred = onehotdecode(dlYPred,classes,1);
%     YTest = onehotdecode(dlYTest,classes,1);
%
%     if isempty(predictions2)
%         predictions2=YPred(:);
%     else
%         predictions2 = [predictions2(:); YPred(:)];
%     end
%     yy=YPred == YTest;
%     predCorr2 = [predCorr2(:); yy(:)];

    [~,YPred]=max(dlYPred);
    YPred=extractdata(YPred);
    sleep_stage=YPred(:);

%     YTest=extractdata(dlYTest);
%     YTest=YTest(:);
%     YTest=YTest+1;
%
%     numFilters
%     C = confusionmat(YPred,YTest,'order',[numClasses:-1:1])
%     ka = kappa(C)
else
    sleep_stage = [];
end