function segment_length = p05_boundary_segment_length ( segment_index, h )

%*****************************************************************************80
%
%% P05_BOUNDARY_SEGMENT_LENGTH returns boundary segment lengths in problem 05.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer SEGMENT_INDEX, the index of one of the boundary segments.
%
%    Input, real H, the suggested spacing between points.
%
%    Output, integer SEGMENT_LENGTH, the number of points in the segment.
%
  center1 = [  0.0, 0.0 ];
  center2 = [ -0.4, 0.0 ];
  r1 = 1.00;
  r2 = 0.55;

  if ( h <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'P05_BOUNDARY_SEGMENT_LENGTH - Fatal error!\n' );
    fprintf ( 1, '  Nonpositive H = %f\n', h );
    error ( 'P05_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );
  end

  if ( segment_index == 1 )

    n = round ( ( pi * ( r1  + r2 ) ...
      + ( ( center2(1) - r2 ) - ( center1(1) - r1 ) ) ...
      + ( ( center1(1) + r1 ) - ( center2(1) + r2 ) ) ) / h );
    n = max ( n, 21 );
    segment_length = n;

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'P05_BOUNDARY_SEGMENT_LENGTH - Fatal error!\n' );
    fprintf ( 1, '  Illegal SEGMENT_INDEX = %d\n', segment_index );
    error ( 'P05_BOUNDARY_SEGMENT_LENGTH - Fatal error!' );

  end

  return
end


function cost = rcrossvalidatelssvm(model,Y, L, omega, estfct,combinefct)
%%%%%%%%%%%%%%%%%%%%%
% INTERNAL FUNCTION %
%%%%%%%%%%%%%%%%%%%%%
% Estimate the model performance of a model with l-fold robust crossvalidation

% Copyright (c) 2011,  KULeuven-ESAT-SCD, License & help @
% http://www.esat.kuleuven.be/sista/lssvmlab

%
% initialisation and defaults
%
%if size(X,1)~=size(Y,1), error('X and Y have different number of datapoints'); end
[nb_data,y_dim] = size(Y);
d = size(model.xtrain,2);
% LS-SVMlab
eval('model = initlssvm(model{:});',' ');
model.status = 'changed';

eval('L;','L=min(round(sqrt(size(model.xfull,1))),10);');
eval('estfct;','estfct=''mse'';');
eval('combinefct;','combinefct=''mean'';');

py = Y;
[~,Y] = postlssvm(model,[],Y);

gams = model.gamcsa;
eval('sig2s = model.kernel_parscsa;','sig2s=[];')
eval('deltas = model.deltacsa;','deltas=[];')
%
%initialize: no incremental  memory allocation
%
costs = zeros(L,length(gams));
block_size = floor(nb_data/L);

% check whether there are more than one gamma or sigma
for j =1:numel(gams)
    if strcmp(model.kernel_type,'RBF_kernel') || strcmp(model.kernel_type,'RBF4_kernel')
        model = changelssvm(changelssvm(model,'gam',gams(j)),'kernel_pars',sig2s(j));
        eval('model.delta=deltas(j);','model.delta=[];')
    elseif strcmp(model.kernel_type,'lin_kernel')
        model = changelssvm(model,'gam',gams(j));
        eval('model.delta=deltas(j);','model.delta=[];')
    elseif strcmp(model.kernel_type,'poly_kernel')
        model = changelssvm(changelssvm(model,'gam',gams(j)),'kernel_pars',[sig2s(1,j);sig2s(2,j)]);
        eval('model.delta=deltas(j);','model.delta=[];')
    else
        model = changelssvm(changelssvm(model,'gam',gams(j)),'kernel_pars',[sig2s(1,j);sig2s(2,j);sig2s(3,j)]);
        eval('model.delta=deltas(j);','model.delta=[];')
    end

    S = ones(nb_data,1);
    Atot = kernel_matrix2(omega,model.kernel_type,model.kernel_pars,d)+eye(nb_data)./model.gam;
    %
    %
    % start loop over l validations
    %
    for l = 1:L,

        % divide in data and validation set, trainings data set is a copy
        % of permutated_data, validation set is just a logical index
        if l==L,
            train = 1:block_size*(l-1);
            validation = block_size*(l-1)+1:nb_data;
        else
            train = [1:block_size*(l-1) block_size*l+1:nb_data];
            validation = block_size*(l-1)+1:block_size*l;
        end

        A = [0 S(train)';S(train) Atot(train,train)];
        b = [0;py(train)];

        % Solve linear system
        sol = linsolve(A,b,struct('SYM',true));

        % Determine residuals ek
        ek = sol(2:end)./model.gam;
        g = model.gam;
        %for i=2:size(A,1), A(i,i) = A(i,i) - 1/g; end
        A = A-eye(size(train,2)+1)./g; A(1,1)=0;
        Ah = A;
        %
        % robust estimation of the variance
        %
        for k = 1:30
            vare = 1.483*median(abs((ek)-median(ek)));
            alphaold = sol(2:end);
            %
            % robust re-estimation of the alpha's and the b
            %
            cases = reshape((ek./vare),1,size(ek,1));
            W = g*weightingscheme(cases,model.weights,model.delta);

            for t=1:size(train,2), A(t+1,t+1) = A(t+1,t+1)+1./W(t); end

            sol = linsolve(A,b,struct('SYM',true));

            ek = sol(2:end)./W';
            A = Ah;
            if norm(abs(alphaold-sol(2:end)),'fro')<=1e-3,
                %fprintf('\n Converged after %.0f iteration(s)', k);
                k = inf;
            end
            model.status = 'changed';
        end

        % regression
        % Simulate system on validation data
        yh = Atot(train,validation)'*sol(2:end) + ones(numel(validation),1)*sol(1);
        [~,yh] = postlssvm(model,[],yh);
        z = yh - Y(validation,:);
        eval('costs(l,j) = feval(estfct,z);')
    end
end

cost = feval(combinefct, costs);


function [rglm,yclean] = spm_rglm (y,X,m,priors,verbose)
% Fit a Robust GLM
% FORMAT [rglm,yclean] = spm_rglm (y,X,m,priors,verbose)
%
% The noise is modelled with a Mixture of Zero-Mean Gaussians
%
% y          [N x 1] data vector
% X          [N x p] design matrix
% m          Number of mixture components
% priors     .alpha      [1 x 1] weight precision (default=0.001)
%            .mean_err   [m x 1] vector of mean error SD
%            .std_err    [m x 1] vector of dev of error SD
% verbose    0/1 to printout inner workings (default=0)
%
% rglm       Returned model
% yclean     'Clean' data
%
% -------------------------------------------------------
% The fields in rglm are:
%
% m                The number of error components
% fm               The negative free energy
% loops            Number of iterations used
%
%                  In the field priors:
%
% lambda_0         Dirichlet parameters for mixing coeffs
% b_0,c_0          Gamma parameters for precisions
%
%                  In the field posts:
%
% lambda           Dirichlet parameters  for mixing coeffs
% b,c              Gamma parameters for precisions
% w_mean           Mean estimated regression coefficients
% w_cov            Covariance of regression coefficients
% pi               mixing coefficients (lambda/sum(lambda))
% variances        variances (1./(b.*c))
% gamma            the responsilities of each noise component
%___________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_rglm.m 1276 2008-03-28 18:29:19Z guillaume $

if nargin < 4 | isempty(priors)
    mean_alpha=0.001;
    % Set variance priors
    b_0=1000*ones(m,1);
    c_0=0.001*ones(m,1);
else
    mean_alpha=priors.alpha;
    for mm=1:m,
        b_0(mm)=(priors.std_err(mm)^2)/priors.mean_err(mm);
        c_0(mm)=priors.mean_err(mm)/b_0(mm);
    end
end

if (m==1)
    rglm=spm_glm(y,X,mean_alpha);
    return
end

if nargin < 5 | isempty(verbose)
    verbose=0;
end

y=y(:);
N=length(y);
p=size(X,2);
spX=issparse(X);

% Initialise regression coefficients to maximum likelihood solution
if spX
    iX=inv(X'*X);
    w_mean=iX*X'*y;
else
    iX=inv(X'*X);
    w_mean = pinv(X)*y;
end
y_pred = full(X*w_mean);
err=y-y_pred;
v=mean((y-y_pred).^2);
w_cov = v*iX;

% Set mixing priors
lambda_0=5;

% Cluster on absolute difference from mean
zmix=spm_kmeans1(abs(err-mean(err)),m);
% Posterior for mixers
lambda=100*zmix.pi+lambda_0;
zmean=[zmix.m].^2;
% Posterior for precisions
var_precision=1/std(zmean)^2;
for s=1:m,
      % Set so that b*c=precision and  b^2*c=var_precision
      precision=1/zmean(s);
      b(s)=var_precision/precision;
      c(s)=(precision^2)/var_precision;
end

if verbose
    disp('Init');
    for s=1:m,
        disp(sprintf('State %d mix=%1.2f var=%1.2f',s,lambda(s)/sum(lambda),1/(b(s)*c(s))));
    end
end

lik=[];
tol=0.0001;
max_loops=100;
WLOOPS=5;
for loops=1:max_loops,

    % E-step
    lambda_tot=sum(lambda);
    ypred=full(X*w_mean);
    ypred2=ypred.^2;
    y2=y.^2;
    xt=X';
    y_err=full(sum(xt.*(w_cov*xt)));

    tv=y2-2*ypred.*y+y_err'+ypred2;
    tv=tv';
    for s=1:m,
        log_tilde_pi(s)=psi(lambda(s))-psi(lambda_tot);
        log_tilde_beta(s)=psi(c(s))+log(b(s));
        tilde_pi(s)=exp(log_tilde_pi(s));
        tilde_beta(s)=exp(log_tilde_beta(s));
        mean_beta(s)=c(s)*b(s);
        tilde_var(s,:)=tv;
        gamma(s,:)=tilde_pi(s)*(tilde_beta(s)^0.5)*exp(-0.5*mean_beta(s)*tv);
    end
    gamma_n=sum(gamma);
    for s=1:m,
        if mean(gamma_n) > eps
            % If component still exists
            gamma(s,:)=gamma(s,:)./gamma_n;
        end
    end

    % M-step
    % Part I
    for s=1:m,
        pi_bar(s)=mean(gamma(s,:));
        N_bar(s)=N*pi_bar(s);
        mean_bar(s)=mean(gamma(s,:)'.*y);
        dg=sparse(1:N,1:N,gamma(s,:));
        x_bar(s,:)=mean(dg*X);
        var_bar_mu(s)=mean(gamma(s,:).*tilde_var(s,:));
    end

    % CALCULATE THE FREE ENERGY
    avg_likelihood=sum(N_bar.*(log_tilde_pi+0.5*log_tilde_beta));
    fit=-0.5*N*sum(mean_beta.*var_bar_mu);
    % Ensure that 0 log 0 = 0
    ent_s=sum(sum(-gamma.*log(gamma+eps)));
    avg_likelihood=avg_likelihood+fit+ent_s;
    lambda_p=lambda_0*ones(1,m);
    kl_dir=spm_kl_dirichlet(lambda,lambda_p,log_tilde_pi);
    kl_gamm=0;
    for s=1:m,
        kl_gamm=kl_gamm+spm_kl_gamma(b(s),c(s),b_0(s),c_0(s));
    end
    kl_weights=spm_kl_normald(w_mean,w_cov,zeros(1,p),(1/mean_alpha)*eye(p));
    fm= avg_likelihood - kl_dir - kl_gamm - kl_weights;

    % Convergence criterion
    oldlik=lik;
    lik=fm;
    if (mod(loops-1,WLOOPS)==0)
        if (loops>1)
            if abs((lik-oldlik)/lik) < tol
                break;
            end
        end
    end

    % M-Step: Part II
    for s=1:m,
        % Mixers
        lambda(s)=N_bar(s)+lambda_0;
        % Precisions
        b(s)=1/(0.5*N*var_bar_mu(s)+1/b_0(s));
        c(s)=0.5*N_bar(s)+c_0(s);
        mean_beta(s)=c(s)*b(s);
    end

    if mod(loops,WLOOPS)==0
        % Regression coefficients
        cc=sparse(p,p);
        cw=zeros(p,1);
        for s=1:m,
            dg=sparse(1:N,1:N,gamma(s,:));
            cc=cc+mean_beta(s)*X'*dg*X;
            cw=cw+mean_beta(s)*X'*dg*y;
        end
        cc=cc+mean_alpha*speye(p);
        w_cov=inv(cc);
        w_mean=w_cov*cw;
    end

    if verbose
        disp(sprintf('It=%d, L_AV =%1.2f, KL Mix=%1.2f, KL Prec=%1.2f, KL-W=%1.2f, Fm=%1.2f',loops,avg_likelihood,kl_dir,kl_gamm,kl_weights,fm));
        disp(' ');
        disp(sprintf('Iteration number=%d',loops));
        for s=1:m,
            var=1/(b(s)*c(s));
            disp(sprintf('State %d pi_bar=%1.2f var=%1.2f',s,pi_bar(s),var));
        end
    end

end

% Put variables into data structure
rglm.m=m;
rglm.fm=fm;
rglm.kl_dir=kl_dir;
rglm.kl_gamm=kl_gamm;
rglm.kl_w=kl_weights;
rglm.mean_alpha=mean_alpha;
rglm.priors.lambda_0=lambda_0;
rglm.priors.c_0=c_0;
rglm.priors.b_0=b_0;
for k=1:m,
  rglm.posts.lambda(k)=lambda(k);
end
for k=1:m,
  rglm.posts.c(k)=c(k);
  rglm.posts.b(k)=b(k);
end
rglm.posts.w_mean=w_mean;
rglm.posts.w_cov=w_cov;

for k=1:m,
  rglm.posts.pi(k)=lambda(k)/sum(lambda);
end
rglm.posts.variances=1./(b.*c);
rglm.posts.gamma=gamma;
rglm.loops=loops;

% Get 'clean' data
if m > 1
    [tmp,outlier_class]=min(rglm.posts.pi);
    e=y-ypred;
    outlier_error=rglm.posts.gamma(outlier_class,:)'.*e;
    yclean=ypred+e-outlier_error;
else
    yclean=y;
end


function [y,ysup] = erf_rnd(x,rnd)
% input x real non-negative column vector
% rnd  -1  y = lower bound for erf(x)
%       1  y = upper bound for erf(x)
%      []  [y,ysup] inclusion of erf(x)
% rounding may be altered after leaving erf_rnd
%

% written  05/30/13     S.M. Rump
%

  x2 = hex2num('4017744f8f74e94b');     % ~5.86: erf(x)>pred(1) for x>=x2

  y = x;
  if isempty(rnd)
    ysup = x;
  end

  index = ( x<=0.5 );                   % first method
  if any(index)                         % x in [0,0.5]
    if isempty(rnd)
      [y(index),ysup(index)] = erf_rnd1(x(index),rnd,6);
    else
      y(index) = erf_rnd1(x(index),rnd,6);
    end
  end
  Index = index;                        % store finished indices

  index = ( ~Index ) & ( x<x2 );        % second method
  if any(index)                         % x in (0.5,x2)
    y_index = 1 - erfc_rnd2(x(index));
    if rnd==-1
      y(index) = y_index.inf;
    elseif rnd==1
      y(index) = y_index.sup;
    else
      y(index) = y_index.inf;
      ysup(index) = y_index.sup;
    end
  end

  index = ( x>=x2 );
  if any(index)                         % x in [x2,inf]
    if isempty(rnd)                     % inclusion [y,ysup] of erf(x)
      y(index) = hex2num('3fefffffffffffff');   % pred(1)
      ysup(index) = 1;
    elseif rnd==1                       % y upper bound for erf(x)
      y(index) = 1;
    else                                % y lower bound for erf(x)
      y(index) = hex2num('3fefffffffffffff');   % pred(1)
    end
  end


function [nV, eV, aV, alt, lat, long, type, id] = DecodeBits_ADI(bits, currentLat, currentLong)
% Copyright 2015, The MathWorks, Inc.

% Read message bits and decode valid messages for position, velocity and
% altitude data

% Initialize data
nV = 0;
eV = 0;
aV = 0;
alt = 0;
lat = 0;
long = 0;
type = 'X';
id = [bits(9:12)'*[8;4;2;1] bits(13:16)'*[8;4;2;1] bits(17:20)'*[8;4;2;1] bits(21:24)'*[8;4;2;1] bits(25:28)'*[8;4;2;1] bits(29:32)'*[8;4;2;1]]

% Check 9th and 10th hex characters for mesasge type
tf1 = bits(33:36)'*[8;4;2;1];
tf2 = bits(37:40)'*[8;4;2;1];
if tf1 == 9 && tf2 == 9
    [nV, eV, aV] = AltVelCalc_ADI(bits);
    type = 'A';
elseif tf1 == 5 || tf1 == 6
    [lat, long, alt] = LatLongCalcSingle_ADI(bits, currentLat, currentLong);
    type = 'L';
elseif tf1 == 9 && tf2 == 0
    [lat, long, alt] = LatLongCalcSingle_ADI(bits, currentLat, currentLong);
    type = 'L';
end


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcWaterfall(Temp,Event,handles)
handles = guidata(handles.MU_matrix_display);

figure;
waterfall(double(handles.BMatrix));
colormap(handles.V.Color_map);

end


function T = RpToTrans(R, p)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes rotation matrix R and position p.
% Returns the corresponding homogeneous transformation matrix T in SE(3).
% Example Input:
%
% clear; clc;
% R = [[1, 0, 0]; [0, 0, -1]; [0, 1, 0]];
% p = [1; 2; 5];
% T = RpToTrans(R, p)
%
% Output:
% T =
%     1     0     0     1
%     0     0    -1     2
%     0     1     0     5
%     0     0     0     1

T = [R, p; 0, 0, 0, 1];
end


function [imgOut, exposure_value] = SelectOverexposedTMO(img, percent)
%
%        [imgOut, exposure_value] = SelectOverexposedTMO(img, percent)
%
%
%        A simple TMO that generates an SDR image with a given percentage
%        of overexposed pixels.
%
%        Input:
%           -img: input HDR image
%           -percent: percentage of overexposed pixels.
%
%        Output:
%           -imgOut: a single exposure with the selected percentage of
%           overexposed pixels.
%
%
%
%     Copyright (C) 2022 Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

if ~exist('percent', 'var')
    percent = 0.1;
end

gamma_inv = 1.0 / 2.2;

    function err = residual(c)

        if c > 0.0
            tmp = ClampImg((img * c).^gamma_inv, 0.0, 1.0);
            L = lum(tmp);
            tmp_percent = length(find(L >= 0.95)) / numel(L);

            err = abs(tmp_percent - percent);
        else
            err = 1e9;
        end
    end

exposure_start = 1.0;
exposure_value = fminsearch(@residual, exposure_start, getOpts);

imgOut = ClampImg(img * exposure_value, 0.0, 1.0);

end


% Copyright (C) 1993-2014, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

%%begin

% Forward dynamics is the computation of joint accelerations given position and
% velocity state, and actuator torques.  It is useful in simulation of a robot
% control system.
%
% Consider a Puma 560 at rest in the zero angle pose, with zero applied joint
% torques. The joint acceleration would be given by

mdl_puma560
p560.accel(qz, zeros(1,6), zeros(1,6))

% To be useful for simulation this function must be integrated.  fdyn() uses the
% MATLAB function ode45() to integrate the joint acceleration.  It also allows
% for a user written function to compute the joint torque as a function of
% manipulator state.
%
% To simulate the motion of the Puma 560 from rest in the zero angle pose
% with zero applied joint torques

tic
[t q qd] = p560.nofriction().fdyn(10, [], qz);
toc

% and the resulting motion can be plotted versus time

subplot(3,1,1)
plot(t,q(:,1))
xlabel('Time (s)');
ylabel('Joint 1 (rad)')
subplot(3,1,2)
plot(t,q(:,2))
xlabel('Time (s)');
ylabel('Joint 2 (rad)')
subplot(3,1,3)
plot(t,q(:,3))
xlabel('Time (s)');
ylabel('Joint 3 (rad)')

% Clearly the robot is collapsing under gravity, but it is interesting to
% note that rotational velocity of the upper and lower arm are exerting
% centripetal and Coriolis torques on the waist joint, causing it to rotate.

% This can be shown in animation also
clf
p560.plot(q)


function linplus_test535 ( )

%*****************************************************************************80
%
%% TEST535 tests R8S3_READ, R8S3_READ_SIZE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  input_file = 'r8s3_matrix.txt';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST535\n' );
  fprintf ( 1, '  For a R8S3 system,\n' );
  fprintf ( 1, '  R8S3_READ_SIZE reads the size of the matrix.\n' );
  fprintf ( 1, '  R8S3_READ reads the matrix.\n' );

  [ n, nz_num ] = r8s3_read_size ( input_file );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  R8S3_READ_SIZE reports matrix size data:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N =         %d\n', n );
  fprintf ( 1, '  Matrix nonzeros NZ_NUM = %d\n', nz_num );

  [ row, col, a ] = r8s3_read ( input_file, n, nz_num );

  isym = 0;

  r8s3_print_some ( n, n, nz_num, isym, row, col, a, 1, 1, ...
    10, 10, '  Initial 10x10 block of recovered R8S3 matrix:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Deleting the matrix data file "%s"\n', input_file );

  return
end


function [img1_rot, rot, err] = AlignLLPanoramas(img1, img2, bVisualization)
%
%
%     [img1_rot, rot, err] = AlignLLPanoramas(img1, img2, bVisualization)
%
%     This function finds the rotation around Y-axis in pixel for aligning
%     the panorma img1 (in longitude-latitude format) to the panorma
%     img2 (in longitude-latitude format).
%
%     Note that img1 and img2 have to share the same Y-axis in order to
%     produce a meaningful result!
%
%
%     Input:
%       -img1: unaligned image
%       -img2: reference panorama for alignment
%       -bVisualization: if it set to 1 this will show the result of
%                        minimization
%
%     Output:
%       -img1_rot: img1 rotated to be aligned to img2
%       -rot: rotation in pixel. Img1 needs to be shifted of rot pixels in
%             order to be aligned to img2. For the rotation use imShiftWrap.m
%       -err: matching error
%
%     Copyright (C) 2012-16  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

if(~exist('bVisualization', 'var'))
    bVisualization = 0;
end

c = size(img1, 2);

%Calculate the descriptor
Lwa_ext = (logMean(lum(img1)) + logMean(lum(img2))) / 2.0;

img1_tmo = ReinhardTMO(img1, 0.15, 1e9, 'global', [], Lwa_ext);
img2_tmo = ReinhardTMO(img2, 0.15, 1e9, 'global', [], Lwa_ext);

line1 = LLDescriptor(img1_tmo, 1)';
line2 = LLDescriptor(img2_tmo, 1)';

%minimization of the rotation
rot = 0;
err = sum((line1 - line2).^2);

for i=1:(c - 1)
    line1 = circshift(line1, 1);
    tmpErr = sum((line1 - line2).^2);

    if(tmpErr < err)
        rot = i;
        err = tmpErr;
    end
end

%plotting the result
if(bVisualization)
    line1 = circshift(line1, rot);
    plot(1:c, line1, 1:c, line2);
end

img1_rot = imShiftWrap(img1, rot);

end


%% Copyright (C) 2008 Eric Chassande-Mottin
%% Copyright (C) 2011 Carnë Draug
%% Copyright (C) 2016, 2018, 2022 Colin B. Macdonald
%%
%% This program is free software; you can redistribute it and/or modify it under
%% the terms of the GNU General Public License as published by the Free Software
%% Foundation; either version 3 of the License, or (at your option) any later
%% version.
%%
%% This program is distributed in the hope that it will be useful, but WITHOUT
%% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
%% FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
%% details.
%%
%% You should have received a copy of the GNU General Public License along with
%% this program; if not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defun laguerreL (@var{n}, @var{x})
%% Evaluate Laguerre polynomials.
%%
%% Compute the value of the Laguerre polynomial of order @var{n}
%% for each element of @var{x}.
%% For example, the Laguerre polynomial of order 14 evaluated at
%% the point 6 is
%% @example
%% @group
%% @c doctest: +SKIP_IF(compare_versions (OCTAVE_VERSION(), '6.0.0', '<'))
%% laguerreL (14, 6)
%%   @result{} 0.9765
%% @end group
%% @end example
%%
%% This implementation uses a three-term recurrence directly on the values
%% of @var{x}.  The result is numerically stable, as opposed to evaluating
%% the polynomial using the monomial coefficients.  For example, we can
%% compare the above result to a symbolic construction:
%% @example
%% @group
%% syms x
%% L = laguerreL (14, x);
%% exact = subs (L, x, 6)
%%   @result{} exact = (sym)
%%       34213
%%       ─────
%%       35035
%% @end group
%% @end example
%% If we extract the monomial coefficients and numerically evaluate the
%% polynomial at a point, the result is rather poor:
%% @example
%% @group
%% coeffs = sym2poly (L);
%% @c doctest: +XFAIL_IF(compare_versions (OCTAVE_VERSION(), '6.0.0', '<'))
%% polyval (coeffs, 6)
%%   @result{} 0.9765
%% err = ans - double (exact);
%% num2str (err, '%.3g')
%%   @result{} -1.68e-11
%% @end group
%% @end example
%% So please don't do that!  The numerical @code{laguerreL} function
%% does much better:
%% @example
%% @group
%% err = laguerreL (14, 6) - double (exact)
%%   @result{} err = 9.9920e-16
%% @end group
%% @end example
%%
%% @seealso{@@sym/laguerreL}
%% @end defun

function L = laguerreL(n, x)

  if (nargin ~= 2)
    print_usage ();
  end

  if (any (n < 0) || any (mod (n, 1) ~= 0))
    error('second argument "n" must consist of positive integers');
  end

  if (~isscalar (n) && isscalar (x))
    x = x*ones (size (n));
  elseif (~isscalar (n) && ~isscalar (x) && ~isequal (size (n), size (x)))
    error ('inputs must be same size or scalar')
  end

  L0 = ones (size (x), class(x));
  L1 = 1 - x;

  if (isscalar (n))
    if (n == 0)
      L = L0;
    elseif (n == 1)
      L = L1;
    else
      for k = 2:n
        L = (2*k-1-x)/k .* L1 - (k-1)/k * L0;
        L0 = L1;
        L1 = L;
      end
    end
  else
    L = L0;
    L(n >= 1) = L1(n >= 1);
    maxn = max (n(:));
    for k = 2:maxn
      I = (n >= k);  % mask for entries still to be updated
      L(I) = (2*k - 1 - x(I))/k .* L1(I) - (k - 1)/k * L0(I);
      L0 = L1;
      L1 = L;
    end
  end

end

%!error laguerreL (1)
%!error laguerreL (1, 2, 3)

%!assert (isequal (laguerreL (0, rand), 1))

%!test
%! x = rand;
%! assert (isequal (laguerreL (1, x), 1 - x))

%!test
%! x=rand;
%! y1=laguerreL(2, x);
%! p2=[.5 -2 1];
%! y2=polyval(p2,x);
%! assert(y1 - y2, 0, 10*eps);

%!test
%! x=rand;
%! y1=laguerreL(3, x);
%! p3=[-1/6 9/6 -18/6 1];
%! y2=polyval(p3,x);
%! assert(y1 - y2, 0, 20*eps);

%!test
%! x=rand;
%! y1=laguerreL(4, x);
%! p4=[1/24 -16/24 72/24 -96/24 1];
%! y2=polyval(p4,x);
%! assert(y1 - y2, 0, 30*eps)

%!error <positive integer> laguerreL(1.5, 10)
%!error <same size or scalar> laguerreL([0 1], [1 2 3])
%!error <same size or scalar> laguerreL([0 1], [1; 2])

%!test
%! % numerically stable implementation (in n)
%! L = laguerreL (10, 10);
%! Lex = 1763/63;
%! assert (L, Lex, -eps)
%! L = laguerreL (20, 10);
%! Lex = -177616901779/14849255421;  % e.g., laguerreL(sym(20),10)
%! assert (L, Lex, -eps)

%!test
%! % vectorized x
%! L = laguerreL (2, [5 6 7]);
%! Lex = [3.5 7 11.5];
%! assert (L, Lex, eps)

%!test
%! L = laguerreL (0, [4 5]);
%! assert (L, [1 1], eps)

%!test
%! % vector n
%! L = laguerreL ([0 1 2 3], [4 5 6 9]);
%! assert (L, [1 -4 7 -26], eps)

%!test
%! % vector n, scalar x
%! L = laguerreL ([0 1 2 3], 6);
%! assert (L, [1 -5 7 1], eps)

%!assert (isa (laguerreL (0, single (1)), 'single'))
%!assert (isa (laguerreL (1, single ([1 2])), 'single'))
%!assert (isa (laguerreL ([1 2], single ([1 2])), 'single'))


%DEMO_IMPROVEMARGINALS  Demonstration of marginal posterior improvements
%                       in Laplace and EP algorithms.
%
%  Description
%    Demonstration of marginal posterior corrections of latent
%    variables in classification task. Demonstrated corrections are
%    'fact' (EP & Laplace) and 'cm2' (Laplace). The corrected
%    posterior distributions are compared to histograms of MCMC
%    samples to assess the quality of corrections.
%
%   Reference
%     Cseke & Heskes (2011). Approximate Marginals in Latent Gaussian
%     Models. Journal of Machine Learning Research 12 (2011), 417-454
%
%  See also
%    GP_PREDCM, DEMO_IMPROVEMARGINALS2
%

% 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.

% ---------------------------
% Probit likelihood with EP
% ---------------------------
S = which('demo_improvemarginals');
L = strrep(S,'demo_improvemarginals.m','demodata/synth.tr');
x=load(L);
y=x(:,end);
y = 2.*y-1;
x(:,end)=[];
[n, nin] = size(x);

% Load sampled results so the demo won't take hours or days to finish...
L = strrep(S,'demo_improvemarginals.m','demodata/samples_marginal.mat');
load(L);

% Test data
xt1=repmat(linspace(min(x(:,1)),max(x(:,1)),20)',1,20);
xt2=repmat(linspace(min(x(:,2)),max(x(:,2)),20)',1,20)';
xt=[xt1(:) xt2(:)];

% Create a likelihood function
lik = lik_probit();
%lik = lik_logit();

% Create a covariance function
gpcf = gpcf_sexp('lengthScale', [0.9 0.9], 'magnSigma2', 10);

% Set the prior for the parameters of the covariance function
pl = prior_t();
pm = prior_sqrtunif();
gpcf = gpcf_sexp(gpcf, 'lengthScale_prior', pl,'magnSigma2_prior', pm); %

% Create the GP structure (type is by default FULL)
gp = gp_set('lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-9);

% Set the approximate inference method
gp = gp_set(gp, 'latent_method', 'EP');

ind = 22;
ng=50;
ngt=30;
pc_ep=zeros(ng, 1); p_ep=zeros(ng,1); c_ep=zeros(ng,1); fvec_ep=zeros(ng,1);
pc_ep_pred=zeros(ngt, 1); p_ep_pred=zeros(ngt,1); c_ep_pred=zeros(ngt,1); fvec_ep_pred=zeros(ngt,1);

% If we didnt load previously computed samples we would run the following
% to get the MCMC samples for latents

% gp2 = gp_set(gp, 'latent_method', 'MCMC');
%
% % set MC options
% latent_opt.repeat=10;
%
% % obtain MC samples
% rgp=gp_mc(gp2, x, y, 'latent_opt', latent_opt, 'nsamples', 4000, 'repeat', 2, 'display', 100);
% rgp=thin(rgp,100);
%
% f_mc = rgp.latentValues(:,ind);
f_mc = f_mc_probit;
for i=1:length(ind)
  figure;
  subplot(2,1,1);

  [testi, testi2] = hist(f_mc(:,i),50);
  width = testi2(2)-testi2(1);
  area = sum(testi.*width);
  testi = testi./area;
  b = bar(testi2,testi);
  h = findobj(gca,'Type','patch');
  set(h,'FaceColor','w')
  hold on;
  [Eft_ep, Varft_ep] = gp_pred(gp,x,y,x);
  start=tic;
  [pc_ep(:,i), fvec_ep(:,i), p_ep(:,i)] = gp_predcm(gp,x,y,'ind', ind(i), 'fcorr', 'fact'); tt_epfact=toc(start);
  s = plot(fvec_ep(:,i), p_ep(:,i), '-k', fvec_ep(:,i), norm_pdf(fvec_ep(:,i), Eft_ep(ind(i)), sqrt(Varft_ep(ind(i)))),'-m', fvec_ep(:,i), pc_ep(:,i), '-r');
  set(s,'LineWidth',2)
  set(get(get(b,'Annotation'),'LegendInformation'),...
    'IconDisplayStyle','off');
  legend('EP-L', 'EP-G', 'EP-FACT');
  title('Marginal corrections for probit likelihood (EP)');

  subplot(2,1,2)

  % Predictive corrections
  [Eft_ep_pred, Varft_ep_pred] = gp_pred(gp,x,y,xt);
  start=tic;
  [pc_ep_pred(:,i), fvec_ep_pred(:,i), p_ep_pred(:,i)] = gp_predcm(gp,x,y,xt, 'ind', ind(i), 'fcorr', 'fact', 'ng', ngt);tt_epfact2=toc(start);

%   if ~exist(p_mc, 'var')
%     % If sampled before
%     [Ef_mc, Varf_mc]=gpmc_preds(rgp, x, y, xt(ind,:));
%     p_mc=[];
%     for i2=1:size(rgp.etr,1)
%       p_mc=[p_mc norm_pdf(fvec_ep_pred(:,i), Ef_mc(:,i2), sqrt(Varf_mc(:,i2)))];
%     end
%     p_mc=mean(p_mc,2);
%   end

  plot(fvec_ep_pred(:,i), p_ep_pred(:,i), '-k', fvec_ep_pred(:,i), pc_ep_pred(:,i), '-r', fvec_ep_pred(:,i), ptx_prob, '-c');
  set(s,'LineWidth',2)
  legend('EP-G', 'EP-FACT', 'MCMC');
  title('Predictive marginal corrections for probit likelihood (EP)');
end

% ---------------------------
% Probit likelihood with Laplace
% ---------------------------

% Create the GP structure (type is by default FULL)
gp = gp_set(gp, 'latent_method', 'Laplace');

% Index for comparison values
ind = 22;
pc_la=zeros(ng, 1); p_la=zeros(ng,1); c_la=zeros(ng,1); fvec_la=zeros(ng,1);
pc_la_pred=zeros(ngt, 1); p_la_pred=zeros(ngt,1); c_la_pred=zeros(ngt,1); fvec_la_pred=zeros(ngt,1);
pc_la2=zeros(ng, 1); p_la2=zeros(ng,1); c_la2=zeros(ng,1); fvec_la2=zeros(ng,1);
pc_la_pred2=zeros(ngt, 1); p_la_pred2=zeros(ngt,1); c_la_pred2=zeros(ngt,1); fvec_la_pred2=zeros(ngt,1);

for i=1:length(ind)
  figure; subplot(2,1,1);
  [testi, testi2] = hist(f_mc(:,i),50);
  width = testi2(2)-testi2(1);
  area = sum(testi.*width);
  testi = testi./area;
  b = bar(testi2,testi);
  h = findobj(gca,'Type','patch');
  set(h,'FaceColor','w')
  hold on;
  [Eft_la, Varft_la] = gp_pred(gp,x,y,x);
  start=tic;[pc_la(:,i), fvec_la(:,i), p_la(:,i), c_la(:,i)] = gp_predcm(gp,x,y,'ind', ind(i), 'fcorr', 'cm2'); tt_lacm2=toc(start);
  start=tic;[pc_la2(:,i), fvec_la(:,i), p_la2(:,i), c_la2(:,i)] = gp_predcm(gp,x,y,'ind', ind(i), 'fcorr', 'fact'); tt_lafact=toc(start);
  s = plot(fvec_la(:,i), p_la(:,i), '-k', fvec_la(:,i), norm_pdf(fvec_la(:,i), Eft_la(ind(i)), sqrt(Varft_la(ind(i)))), '-m', ...
          fvec_la(:,i), pc_la2(:,i), '-r', fvec_la(:,i), pc_la(:,i), '-b');
  set(s,'LineWidth',2)
  set(get(get(b,'Annotation'),'LegendInformation'),...
    'IconDisplayStyle','off');
  legend('LA-L', 'LA-G', 'LA-FACT', 'LA-CM2');
  title('Marginal corrections for probit likelihood (Laplace)');

  % Predictive corrections
  subplot(2,1,2);
  [Eft_la_pred, Varft_la_pred] = gp_pred(gp,x,y,xt);
  start=tic;[pc_la_pred(:,i), fvec_la_pred(:,i), p_la_pred(:,i), c_la_pred(:,i)] = gp_predcm(gp,x,y,xt, 'ind', ind(i), 'fcorr', 'cm2', 'ng', 30); tt_lacm22=toc(start);
  start=tic;[pc_la_pred2(:,i), fvec_la_pred(:,i), p_la_pred2(:,i), c_la_pred2(:,i)] = gp_predcm(gp,x,y,xt, 'ind', ind(i), 'fcorr', 'fact','ng', 30); tt_lafact2=toc(start);
  s = plot(fvec_la_pred(:,i), p_la_pred2(:,i), '-k', fvec_la_pred(:,i), pc_la_pred(:,i), '-r', fvec_la_pred(:,i), pc_la_pred2(:,i), '-b', fvec_la_pred(:,i), ptx_lap, '-c');
  legend('LA-G', 'LA-CM2', 'LA-FACT', 'MCMC');
  title('Predictive marginal corrections for probit likelihood (Laplace)');
end

fprintf('Time elapsed for marginal corrections with EP-FACT: %.1f s and for predictions %.1f s\n', tt_epfact, tt_epfact2);
fprintf('Time elapsed for marginal corrections with LA-CM2: %.1f s and for predictions %.1f s\n', tt_lacm2, tt_lacm22);
fprintf('Time elapsed for marginal corrections with LA-FACT: %.1f s and for predictions %.1f s\n', tt_lafact, tt_lafact2);


function x = sample_fs(xf, grid_sz)

% Samples the Fourier series

sz = [size(xf,1) size(xf,2)];

if nargin < 2 || all(sz == grid_sz)
    x = prod(sz) * cifft2(xf);
else
    if any(grid_sz < sz)
        error('The grid size must be larger than or equal to the signal size')
    end
    tot_pad = grid_sz - sz;
    pad_sz = ceil(tot_pad/2);
    xf_pad = padarray(xf, pad_sz);
    if any(mod(tot_pad,2) == 1)
        % Handle odd padding
        xf_pad = xf_pad(1:end-mod(tot_pad(1),2), 1:end-mod(tot_pad(2),2), :, :);
    end
    x = prod(grid_sz) * cifft2(xf_pad);
end


function h = a2h(A);

% H = a2h(ABCD)
%
% ABCD to Hybrid-H transformation
% only for 2x2 matrices

d = A(1,1)*A(2,2) - A(1,2)*A(2,1);

while abs(A(2,2)) < 1e-8
    A(2,2) = A(2,2)*(1+rand*1e-8);
end;

h(1,1) = A(1,2)/A(2,2);
h(1,2) = d/A(2,2);
h(2,1) = -1/A(2,2);
h(2,2) = A(2,1)/A(2,2);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inverse dynamics for the 2dof planar robot
%
%   tau = inversedynamics_2dofplanar_fast(robot, q, qd, qdd, fe)
%
%   Where robot stores the kinematic and dynamic parameters for this robot.
%   q: joint positions.
%   qd: joint velocities.
%   qdd: joint accelerations.
%   fe: vector of external forces. (unused)
%
%   This function just executes the inverse dynamic model for this robot.
%   The equations to compute this dynamic model can be found in:
%   "ROBOT ANALYSIS. The mechanics of Serial and Parallel
%        manipulators". Lung Weng Tsai. John Wiley and Sons, inc. ISBN:
%        0-471-32593-7. page 405.
%
%   Author: Arturo Gil Aparicio arturo.gil@umh.es
%   Date: 08/03/2014
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function tau = inversedynamics_2dofplanar_fast(robot, q, qd, qdd, fe)

fprintf('\nComputing inverse dynamics for the %s robot', robot.name);

a = eval(robot.DH.a);
a1=a(1);
a2=a(2);

gc=9.81;

m1=robot.dynamics.masses(1);
m2=robot.dynamics.masses(2);

%express it as a general dynamic function
%M*qdd + V + G = Q, where Q is a vector of generalized forces or moments
% in this case, it is done just to represent the equations more clearly.
% have a look at the directdynamics_2dofplanar_fast function to observe
% this expression used to compute the direct dynamic model

%M is a 2x2 manipulator inertia matrix
M = [(m1*(1/3) + m2)*a1^2 + m2*a1*a2*cos(q(2)) + (1/3)*m2*a2^2 ...
      (1/2)*m2*a1*a2*cos(q(2))+(1/3)*m2*a2^2 ;
      (1/2)*m2*a1*a2*cos(q(2))+(1/3)*m2*a2^2  (1/3)*m2*a2^2];

V=[-m2*a1*a2*sin(q(2))*(qd(1)*qd(2)+(1/2)*qd(2)^2);
    (1/2)*m2*a1*a2*sin(q(2))*qd(1)^2];

G=[gc*(((1/2)*m1+m2)*a1*cos(q(1)) + (1/2)*m2*a2*cos(q(1)+q(2)));
    (1/2)*m2*gc*a2*cos(q(1)+q(2))];

tau = M*qdd + V + G;


function [kg] = ug2kg(ug)
% Convert mass from micrograms to kilograms.
% Chad Greene 2012
kg = ug*1E-9 ;


function [ y, symm ] = cvx_s_symmetric_ut( m, n, symm )
%CVX_S_SYMMETRIC_UT Symmetric matrices (upper triangle storage).
if m ~= n,
    cvx_throw( 'Symmetric structure requires square matrices.' );
end
symm = false;
nsq = n * n;
ntr = 0.5 * ( nsq + n );
c   = 0 : n - 1;
c   = c( ones( 1, n ), : );
r   = c';
mn  = min( r, c );
mx  = max( r, c );
y   = mn + 0.5 * mx .* ( mx + 1 ) + 1;
y   = sparse( y( : ), 1 : nsq, 1, ntr, nsq );

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function value = i4_btest ( i4, pos )

%*****************************************************************************80
%
%% I4_BTEST returns TRUE if the POS-th bit of an I4 is 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Military Standard 1753,
%    FORTRAN, DoD Supplement To American National Standard X3.9-1978,
%    9 November 1978.
%
%  Parameters:
%
%    Input, integer I4, the integer to be tested.
%
%    Input, integer POS, the bit position, between 0 and 31.
%
%    Output, logical VALUE, is TRUE if the POS-th bit of I4 is 1.
%
  i4_huge = 2147483647;

  if ( pos < 0 )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_BTEST - Fatal error!\n' );
    fprintf ( 1, '  POS < 0.\n' );
    error ( 'I4_BTEST - Fatal error!' );

  elseif ( pos < 31 )

    if ( 0 <= i4 )
      j = floor ( i4 );
    else
      j = floor ( ( i4_huge + i4 ) + 1 );
    end

    for k = 1 : pos
      j = floor ( j / 2 );
    end

    if ( mod ( j, 2 ) == 0 )
      value = 0;
    else
      value = 1;
    end

  elseif ( pos == 31 )

    if ( i4 < 0 )
      value = 1;
    else
      value = 0;
    end

  elseif ( 31 < pos )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4_BTEST - Fatal error!\n' );
    fprintf ( 1, '  31 < POS.\n' );
    error ( 'I4_BTEST - Fatal error!' );

  end

  return
end


function [W] = winding_number(V,F,O,varargin)
  % WINDING_NUMBER Compute the sum of solid angles of a triangle (tetrahedron)
  % described by points (vectors) V
  %
  % [W] = winding_number(V,F,O)
  % [W] = winding_number(V,F,O,'ParameterName',ParameterValue, ...)
  %
  % Inputs:
  %  V  #V by 3 list of vertex positions
  %  F  #F by 3 list of triangle indices
  %  O  #O by 3 list of origin positions
  %  Optional inputs:
  %    'Hierarchical'  followed by true or false. Use hierarchical evaluation.
  %      for mex: {true}, for matlab this is not supported
  %    'Fast' followed by whether to use fast-winding-number {false}
  %    'RayCast' followed by true or flase. Use ray cast version of approximate
  %      evaluation: {false}
  %    'TwoDRays' followed by true or false. Use 2d rays only.
  %    'NumRays' followed by the number of rays to cast for each origin
  % Outputs:
  %  W  no by 1 list of winding numbers
  %

  warning('not mex...');
  S = solid_angle(V,F,O);
  W = sum(S,2);
  switch size(F,2)
  case 3
    W = W/(2*pi);
  case 4
    W = W/(4*pi);
  end

end


function yhat=lfsmooth(varargin)
%
% a simple interface to locfit.
% output is a vector of smoothed values, at each data point.
% all locfit options, except evaluation structures, are valid.
%
% Example, to smooth a time series of observations,
%
% t = (1:100)';
% y = 2*sin(t/10) + normrnd(0,1,100,1);
% plot(t,y,'.');
% hold on;
% plot(t,lfsmooth(t,y,'nn',0.5));
% hold off;
%

% Minimal input validation
if nargin < 1
   error( 'At least one input argument required' );
end

fit = locfit(x,varargin{:},'module','simple');
yhat = fit.fit_points.fitted_values;

return;


function pass = test_isPeriodicTech(pref)
% Test isPeriodicTech().

if ( nargin == 0 )
    pref = chebfunpref;
end
tol = 1000*pref.cheb3Prefs.chebfun3eps;

f = chebfun3v(@(x,y,z) x, @(x,y,z) y);
pass(1) = ~isPeriodicTech(f);

f1 = chebfun3(@(x,y,z) cos(pi*x), 'trig');
f2 = chebfun3(@(x,y,z) sin(pi*y), 'trig');
pass(2) = isPeriodicTech([f1; f2]);
pass(3) = isPeriodicTech([f1; f2; f2]);
F = [f1; f2];
pass(4) = isPeriodicTech([F; f2]);

end


function test_issue1400

% WALLTIME 00:10:00
% MEM 2gb
% DEPENDENCY ft_selectdata

ntrial = 2;

data = [];
data.label = {'1'};
for i=1:ntrial
  t = (1:1000)/1000 + i; % time vectors are unaligned to show the error
  s = sin(i*2*pi*t); % make a sine with 1, 2, 3, ... Hz
  data.time{i} = t;
  data.trial{i}(1,:) = s;  % insert it in the first channel
end

disp('Demonstrate averaging over time:')
disp('Averaging trials 1, 2')
cfg = [];
cfg.trials = [1,2];
cfg.avgoverrpt = 'yes';
cfg.keeprpt = 'no';
cfg.verbosity = 'off';
tmpdata = ft_selectdata(cfg, data);

% disp(['trialinfo(1, 1) = ', num2str(tmpdata.trialinfo(1,1))])
disp(['trial 1 time = ', regexprep(num2str(data.time{1}(1:5)),'\s+',', ')])
disp(['trial 2 time = ', regexprep(num2str(data.time{2}(1:5)),'\s+',', ')])
disp(['average time = ', regexprep(num2str(tmpdata.time{1}(1:5)),'\s+',', ')])

% add some trialinfo which is a mix of numbers and letters
disp('Demonstrate averaging over trialinfo:')
data.trialinfo = table([1;2],{'a'; 'b'});
disp(['trialinfo class is: ', class(data.trialinfo)])

% This would return an error in ft_selectdata, because it
% averaging the table prior to discarding the averaged trialinfo
disp('Averaging trials 1, 2')
cfg = [];
cfg.trials = [1,2];
cfg.avgoverrpt = 'yes';
cfg.keeprpt = 'no';
cfg.verbosity = 'off';
tmpdata = ft_selectdata(cfg, data);


function a = tanh(a)
%TANH         Gradient hyperbolic tangent tanh(a)
%

% written  10/16/98     S.M. Rump
% modified 10/14/00     S.M. Rump  use Tony's trick
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    iaccelaration for sparse input
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/08/08     S.M. Rump  improved sparse multiplication: not using intval data type
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  % use full(a.x(:)): cures Matlab V6.0 bug
  % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
  % ax is full anyway
  ax = 1 ./ sqr(cosh(full(a.x(:))));
  a.x = tanh(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    if isa(a.x,'intval')
      adx = times(ax(ia),sa,0);
      if adx.complex
        a.dx = intval( sparse(ia,ja,adx.mid,sizeax,N) , sparse(ia,ja,adx.rad,sizeax,N) , 'midrad' );
      else
        a.dx = intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
      end
    else
      a.dx = sparse(ia,ja,ax(ia).*sa,sizeax,N);
    end
  else
    a.dx = a.dx .* ax(:,ones(1,N));
  end

  if rndold
    setround(rndold)
  end


    % design_matrix: data class for creating a design matrix to be used with a linear model including fmri data.
    %
    %
    % Inputs:
    % ---------------------------------------------------------------------
    % dat                       : M x N numeric matrix containing Observations and Variables
    %                           If dat is a file name then will try to
    %                           import data into design_matrix object.
    %                           Make sure there is only text in header and
    %                           not anywhere else.
    %
    % varname                   : Cell array containing variable names.  Must
    %                             match number of column in data matrix
    %
    % Examples:
    % ---------------------------------------------------------------------
    % DM = design_matrix([ones(10,1), (1:10)', (1:10).^2'],{'Intercept','X','X2'})
    %
    % Original version: Copyright Luke Chang 2/2014

    % Notes:
    % Need to add these Methods:
    % -create regressor from stim times
    % -pca

classdef design_matrix < handle
    properties
        dat = [];
        varname = {};
        fname = '';
    end

    methods
        function obj = design_matrix(dat, varname)
            class constructor % Initialize instance of design_matrix

            if(nargin > 1)
                try
                    if(~ismatrix(dat) || ~isnumeric(dat) || iscell(dat))
                        error('Make sure input data is a matrix')
                    end
                    if(length(varname) ~= size(dat,2) || ~iscell(varname));
                        error('Make sure the number of variable names corresponds to number of data columns.')
                    end
                    obj.dat = dat;
                    obj.varname = varname;
                catch err
                    error('Make sure input variable names are in a cell array with length equal to number of data columns and data is a matrix.')
                end
                obj.varname = varname;

            elseif(nargin > 0)

                %import data into design_matrix if dat is a filename
                if ischar(dat)
                    ftest = exist(dat,'file'); %check if valid file
                    if ftest == 2
                        data = importdata(dat);
                        obj.dat = data.data;
                        obj.varname = data.colheaders;
                    end

                elseif(~ismatrix(dat) || ~isnumeric(dat) || iscell(dat))
                    error('Make sure input data is a matrix')
                else

                    obj.dat = dat;
                end

            else % if nothing initialize empty object
                return
            end
        end

        function names(obj)
            % names(obj)
            %
            % List variable names for each regressor
            display(obj.varname)
        end

        function dim = size(obj, varargin)
            % dim = size(obj, varargin)
            %
            % Return dimensions of design matrix
            % Optional Input: Indicate Dimension(row = 1 or column = 2)

            if nargin > 1
                dim = size(obj.dat, varargin{1});
            else
                dim = size(obj.dat);
            end
        end

        function plot(obj)
            % plot(obj)
            %
            % Plot design matrix
            imagesc(obj.dat)
        end

        function save(obj, fname)
            % save(obj, fname)
            %
            % Save Design Matrix to file

            save(fname, obj)
        end

        function obj = addvariable(obj, x, varargin)
            % obj = addvariable(obj, x, varargin)
            %
            % Add regressor to design matrix
            %
            % optional inputs
            % -------------------------------------------------------------------
            % 'Name'        : 'Name' followed by Variable name
            %                  Default is 'newVx'
            %
            % 'Order'       : 'Order' followed by where to insert new data columns location
            %                  Default is end

            % Check Inputs
            if size(x,1) ~= size(obj,1)
                error('Make sure new variable column is the same length as design matrix')
            end

            % Defaults
            for i = 1:size(x,2)
                newname{i} = ['newV' num2str(i)];
            end
            isnewname = 0;
            varorder = size(obj,2); %add to end

            % Parse inputs
            % -------------------------------------------------------------------
            for varg = 1:length(varargin)
                if ischar(varargin{varg})
                    % reserved keywords
                    if strcmpi('name',varargin{varg})
                        if length(varargin{varg + 1}) == size(x,2)
                            newname = varargin{varg + 1};
                            isnewname = 1;
                        else
                            error('Make sure ''Name'' is follwed by a valid variable name')
                        end
                        varargin{varg} = {}; varargin{varg + 1} = {};
                    end

                    if strcmpi('order',varargin{varg})
                        if isnumeric(varargin{varg + 1}) && varargin{varg + 1} <= size(obj,2)
                            varorder = varargin{varg + 1};
                        else
                            error('Make sure ''Order'' is follwed by a valid column number')
                        end
                        varargin{varg} = {}; varargin{varg + 1} = {};
                    end
                end
            end
            % -------------------------------------------------------------------

            % Update Name vector if not empty
            if ~isempty(obj.varname)
                if varorder == 1 %begin
                    obj.varname = [newname, obj.varname];
                elseif varorder == size(obj,2) %end
                    obj.varname = [obj.varname, newname];
                else %Somewhere inbetween
                    obj.varname = [obj.varname(1:varorder), newname, obj.varname(varorder + 1 : end)];
                end
            end

            % Add new variables to design Matrix
            if varorder == 1 %begin
                obj.dat = [x, obj.dat];
            elseif varorder == size(obj,2) %end
                obj.dat = [obj.dat, x];
            else %Somewhere inbetween
                obj.dat = [obj.dat(:,1:varorder), x, obj.dat(:,varorder + 1 : end)];
            end
        end

        function obj = zscore(obj, varargin)
            % obj = zscore(obj, varargin)
            %
            % Standardize columns of design matrix
            %
            % optional inputs
            % -------------------------------------------------------------------
            % 'center'        : Only remove mean, don't standardize

            % Defaults
            center = 0; %Only remove mean

            % optional inputs
            % -------------------------------------------------------------------
            for varg = 1:length(varargin)
                if ischar(varargin{varg})
                    % reserved keywords
                    if strcmpi('center',varargin{varg})
                        center=1;
                        varargin{varg} = {};
                    end
                end
            end
            % -------------------------------------------------------------------

            if center
                obj.dat = obj.dat - repmat(mean(obj.dat),size(obj,1),1);
            else
                obj.dat = zscore(obj.dat);
            end
        end

        function obj = removevariable(obj, x)
            % obj = removevariable(obj, x)
            %
            % Remove columns from design_matrix
            %
            % Inputs
            % -------------------------------------------------------------------
            % x             : Input vector of columns to remove

            obj.dat(:,x) = [];
            obj.varname(x) = [];
        end

        function obj = addintercept(obj)
            % obj = addintercept(obj)
            %
            % Add intercept to design matrix

            %Check if Intercept exists
            if sum(strcmpi(obj.varname,'intercept')) > 0
                error('Intercept Name already included in obj.varname')
            end

            %Check if any variable only includes ones
            for i = 1:size(obj,2)
                if sum(obj.dat(:,i)==1) == size(obj,1)
                    error('There is already a column of ones that resembles an intercept')
                end
            end

            obj.dat = [obj.dat, ones(size(obj,1),1)];
            obj.varname = [obj.varname, 'Intercept'];
        end

        function obj = removeintercept(obj)
            %  obj = removeintercept(obj)
            %
            % Remove intercept from design matrix

            %Check if any variable only includes ones or if intercept is in varname
            for i = 1:size(obj,2)
                whereint(i) = sum(obj.dat(:,i)==1) == size(obj,1);
            end
            if sum(whereint) == 0
                error('There does not appear to be any column of ones that resembles an intercept')
            elseif sum(strcmpi('intercept',obj.varname)) == 0
                error('Intercept Name is not included in obj.varname')
            end

            % now remove intercept
            obj.dat(:,whereint) = [];
            obj.varname(whereint) = [];
        end

        function vif = vif(obj, varargin)
            % vif = vif(obj, varargin)
            %
            % Check for multicollinearity by getting variance inflation factors
            %
            % See original getvif.m in canlab repository - OptimizeDesign11/core_functions/getvif.m
            %
            % optional inputs
            % -------------------------------------------------------------------
            % 'nointercept'       : Remove intercept (turned off by default)

            % Defaults
            noint = 0;
            if strcmpi('nointercept',varargin)
                noint = 1;
            end

            %Remove intercept if asked
            if noint
                obj = removeintercept(obj);
            end

            %Calculate VIF
            for i = 1:size(obj.dat,2)
                X = obj.dat;
                y = X(:,i);
                X(:,i) = [];
                b = X\y;
                fits = X * b;
                rsquare = var(fits) / var(y);

                if rsquare == 1,rsquare = .9999999;end

                vif(i) = 1 / (1 - rsquare);
            end
        end

        function r = corr(obj)
            % Calculate pairwise correlation of regressors in design_matrix

            r = corr(obj.dat);
        end

        function obj = normalizedrank(obj, varargin)
            % obj = normalizedrank(obj, varargin)
            %
            % Rank each regressor and normalize between [0,1]
            %
            % See normalizedrank.m for optional inputs

            obj.dat = normalizedrank(obj.dat, varargin);
        end

        function obj = conv_hrf(obj, varargin)
            % obj = conv_hrf(obj, varargin)
            %
            % Convolve each regressors with hemodynamic response function
            % Uses spm_hrf.m
            %
            % optional inputs
            % -------------------------------------------------------------------
            % 'tr'           : Input TR to use for creating HRF
            %                  (e.g., 'tr', 3)
            %
            % 'select'       : Select Input vector of regressors to convolve
            %                  (e.g., 'select', [2,4])
            %
            % 'custom_hrf'   : Use custom HRF (e.g., 'custom_hrf', [1.00, 0.59, 0.39, 0.27]

            % Defaults
            include = (1:size(obj,2)); %Convolve entire Design Matrix by default
            tr = 2;

            % Check if spm_hrf is on path
            checkspm =  which('spm_hrf.m');
            if isempty(checkspm), error('Make sure spm is in matlab path'); end

            % Parse inputs
            % -------------------------------------------------------------------
            for varg = 1:length(varargin)
                if ischar(varargin{varg})
                    % reserved keywords
                    if strcmpi('tr',varargin{varg})
                        if isnumeric(varargin{varg + 1})
                            tr = varargin{varg + 1};
                            crf = spm_hrf(tr);
                        else
                            error('Make sure ''tr'' is followed by valid number')
                        end
                        varargin{varg} = {}; varargin{varg + 1} = {};
                    end

                    if strcmpi('select',varargin{varg})
                        if isnumeric(varargin{varg + 1}) && all(varargin{varg + 1} <= size(obj,2))
                            include = varargin{varg + 1};
                        else
                            error('Make sure ''select'' is followed by a valid column number')
                        end
                        varargin{varg} = {}; varargin{varg + 1} = {};
                    end

                    if strcmpi('custom_hrf',varargin{varg})
                        if isnumeric(varargin{varg + 1}) && all(varargin{varg + 1} <= size(obj,2))
                            crf = varargin{varg + 1};
                        else
                            error('Make sure ''custom_hrf'' is followed by a vector of a canonical response function')
                        end
                        varargin{varg} = {}; varargin{varg + 1} = {};
                    end
                end
            end
            % -------------------------------------------------------------------

            %Convolution of task
            for i = 1:length(include)
                convdat = conv(obj.dat(:,include(i)),crf);

                %Cut off extra data from convolution
                obj.dat(:,include(i)) = convdat(1:size(obj,1));
            end
        end

        function obj = hpfilter(obj, varargin)
            % obj = hpfilter(obj, varargin)
            %
            % Add High pass filter design matrix using spm's discrete
            % cosine Transform (spm_filter.m)
            %
            % optional inputs
            % -------------------------------------------------------------------
            % 'tr'           : Input TR to use for creating HRF
            %                  (e.g., 'tr', 3) Default = 2;
            %
            % 'duration'     : Duration of high pass filter in seconds
            %                  (e.g., 'duration', 100) Default = 180;

            % Defaults
            tr = 2;
            filterlength = 180;

            % Check if spm_hrf is on path
            checkspm =  which('spm_filter.m');
            if isempty(checkspm), error('Make sure spm is in matlab path'); end

            % Parse inputs
            % -------------------------------------------------------------------
            for varg = 1:length(varargin)
                if ischar(varargin{varg})
                    % reserved keywords
                    if strcmpi('tr',varargin{varg})
                        if isnumeric(varargin{varg + 1})
                            tr = varargin{varg + 1};
                        else
                            error('Make sure ''tr'' is followed by valid number')
                        end
                        varargin{varg} = {}; varargin{varg + 1} = {};
                    end
                    if strcmpi('duration',varargin{varg})
                        if isnumeric(varargin{varg + 1})
                            filterlength = varargin{varg + 1};
                        else
                            error('Make sure ''duration'' is followed by valid number')
                        end
                        varargin{varg} = {}; varargin{varg + 1} = {};
                    end
                end
            end
            % -------------------------------------------------------------------

            %create high pass filter
            K.RT = tr;
            K.row = 1:size(obj,1);
            K.HParam = filterlength;
            nK = spm_filter(K);
            if isempty(nK.X0), error('Check if filter duration is too long'); end

            %Add filter to design_matrix
            obj.dat = [obj.dat, nK.X0];

            %Add variable names
            for i = 1:size(nK.X0,2)
                filtname{i} = ['hpfilter' num2str(i)];
            end
            obj.varname = [obj.varname, filtname];
        end

        function stats = regress(obj, Y, varargin)
            % [B,BINT,R,RINT,STATS] = regress(obj, Y)
            %
            % Regress design matrix on vector Y
            % Uses matlab's regress function
            %
            % optional inputs
            % -------------------------------------------------------------------
            % 'robust'           : use robust regression

            % Defaults
            doRobust = 0;
            for i = 1:length(varargin)
                if strcmpi(varargin(i),'robust')
                    doRobust = 1;
                    find_robust = exist('robustfit');
                    if find_robust ~= 2
                        error('Make sure robustfit is on your path, requires the stats toolbox')
                    end
                    varargin(i) = [];
                end
            end

            if ~doRobust
                [stats.B, stats.BINT, stats.R, stats.RINT, stats.STATS] = regress(Y, obj.dat);
            else
                [stats.B, stats.STATS] = robustfit(obj.dat, Y,[],[],'off');
            end
        end

        function obj = onsettimes(obj, onset, names, tr, timing )
            % obj = onsettimes(obj, onset, names, tr, timing )
            %
            %Create stimulus regressor from onset times
            %
            % Inputs
            % -------------------------------------------------------------------
            % onset        : Input cell array of onset times for each
            %                   regressor in FSL's 3 column format (e.g., onset in sec, duration in sec, weight).
            %
            % names        : Cell array of variable names corresponding
            %                to each onset cell (e.g., {'BlueOn','RedOn'})
            %
            % tr           : Repetition time (e.g., 2)
            %
            % timing       : Timing converstion from onset array to design matrix
            %                  (e.g., 'sec2tr','tr2sec','sec2sec',or
            %                  'tr2tr'). Need to know which format each
            %                  array is in.

            %Convert Onset Times Into Boxcar Regressors
            r = zeros(size(obj,1),length(onset));
            for i = 1:length(onset)
                for j = 1:size(onset{i},1)
                    switch timing
                        case 'sec2tr'
                            if floor(onset{i}(j,1)/tr) == 0
                                r(1 : 1 + ceil(onset{i}(j,2)), i) = onset{i}(j,3);
                            else
                                r(floor(onset{i}(j,1) / tr) : floor(onset{i}(j,1) / tr) + ceil(onset{i}(j,2) / tr) - 1, i) = onset{i}(j,3);
                            end
                        case 'tr2sec'
                            r(floor(onset{i}(j,1) * tr) : floor(onset{i}(j,1) * tr) + ceil(onset{i}(j,2) * tr) - 1, i) = onset{i}(j,3);
                        case {'sec2sec', 'tr2tr'}
                            r(floor(onset{i}(j,1)) : floor(onset{i}(j,1)) + ceil(onset{i}(j,2)) - 1, i) = onset{i}(j,3);
                    end
                end
            end
            obj.dat = [obj.dat, r];

            %Add Variable names
            obj.varname = [obj.varname, names];
        end

        function obj = write(obj, varargin)
            % obj = write(obj, varargin)
            %
            % -------------------------------------------------------------------
            % write design_matrix object into csv file.  Will use obj.fname
            % or can specify optional name
            %
            % -------------------------------------------------------------------
            % Optional Inputs
            % -------------------------------------------------------------------
            % fname        : path and file name of csv file.
            % -------------------------------------------------------------------

            if nargin > 1 %use supplied file name
                if ischar(varargin{1})
                    hdr = sprintf('%s,',obj.varname{:});
                    hdr(end) = '';
                    dlmwrite(varargin{1}, hdr,'') %Write Header 1st
                    dlmwrite(varargin{1}, obj.dat, 'delimiter',',','-append','precision',10) %Append data
                end

            elseif ~isempty(obj.fname) %use obj.fname
                hdr = sprintf('%s,',obj.varname{:});
                hdr(end) = '';
                dlmwrite(obj.fname, hdr, ''); %Write Header 1st
                dlmwrite(obj.fname, obj.dat, 'delimiter',',','-append','precision',10) %Append data
            else
                error('Please supply valid file name with path to save.')
            end

        end

        function c = horzcat(varargin)
            % function c = horzcat(varargin)
            % -------------------------------------------------------------------
            % Implements the horzcat ([a b]) operator on design_matrix objects across variables.
            % Requires that each object has an equal number of rows
            % -------------------------------------------------------------------
            % Examples:
            % c = [dm1 dm2];
            % -------------------------------------------------------------------

            %check if number of rows is the same
                        %check if varnames are the same
            nrow = [];
            for i = 1:nargin
                nrow(i) = size(varargin{i},1);
            end
            for i = 1:nargin
                for j = 1:nargin
                    if nrow(i)~=nrow(j)
                        error('objects have a different number of rows')
                    end
                end
            end

            dat = [];
            varname = [];
            for i = 1:nargin
                    %Check if design_matrix object
                if ~isa(varargin{i}, 'design_matrix')
                    error('Input Data is not an design_matrix object')
                end

                dat = [dat, varargin{i}.dat];
                varname = [varname, varargin{i}.varname];
            end

            c = varargin{1};
            c.dat = dat;
            c.varname = varname;
        end

        function c = vertcat(varargin)
            % function c = vertcat(varargin)
            % -------------------------------------------------------------------
            % Implements the vertcat ([a b]) operator on design_matrix objects across rows.
            % Requires that each object has an equal number of columns and
            % that each varname is identical
            % -------------------------------------------------------------------
            % Examples:
            % c = [dm1; dm2];
            % -------------------------------------------------------------------

            %check if varnames are the same
            varname = {};
            for i = 1:nargin
                varname{i} = varargin{i}.varname;
            end
            for i = 1:nargin
                for j = 1:nargin
                    if ~strcmpi(varname{i},varname{j})
                        error('variable names do not match')
                    end
                end
            end
            dat = [];
            for i = 1:nargin
                %Check if design_matrix object
                if ~isa(varargin{i}, 'design_matrix')
                    error('Input Data is not an design_matrix object')
                end
                dat = [dat; varargin{i}.dat];
            end

            c = varargin{1};
            c.dat = dat;
        end

    end %methods
end %class


function w = quadwts(n)
%QUADWTS   Quadrature weights for Chebyshev points of 2nd kind.
%   QUADWTS(N) returns the N weights for Clenshaw-Curtis quadrature on 2nd-kind
%   Chebyshev points.
%
% See also CHEBPTS, BARYWTS.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DEVELOPER NOTE:
% We use a variant of Waldvogel's algorithm [1], due to Nick Hale. (See below)
% We note this is similar to Greg Von Winkel's approach, which can be found on
% the MathWorks File Exchange.
%
% Let $f(x) = \sum_{k=0}^nc_kT_k(x)$, then\vspace*{-3pt} }
%   I(f) = v.'*c
% where
%   v = \int_{-1}^1T_k(x)dx = { 2/(1-k^2) : k even
%                             { 0         : k odd
%     = v'*inv(TT)*f(x) where TT_{j,k} = T_k(x_j)
%     = (inv(TT)'*v)'*f(x)
% Therefore
%   I(f) = w.'f(x) => w = inv(TT).'*v;
% Here inv(TT).' = inv(TT) is an inverse discrete cosine transform of Type I.
%
% Furthermore, since odd entries in v are zero, can compute via FFT without
% doubling up from N to 2N (though we still need to double up from N/2 to N to
% facilitate the use of ifft).
%
% References:
%   [1] Joerg Waldvogel, "Fast construction of the Fejer and Clenshaw-Curtis
%       quadrature rules", BIT Numerical Mathematics 46 (2006), pp 195-202.
%   [2] Greg von Winckel, "Fast Clenshaw-Curtis Quadrature",
%       http://www.mathworks.com/matlabcentral/fileexchange/6911, (2005)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ( n == 0 )                      % Special case (no points!)
    w = [];
elseif ( n == 1 )                  % Special case (single point)
    w = 2;
else                               % General case
    c = 2./[1, 1-(2:2:(n-1)).^2];  % Exact integrals of T_k (even)
    c = [c, c(floor(n/2):-1:2)];   % Mirror for DCT via FFT
    w = ifft(c);                   % Interior weights
    w([1,n]) = w(1)/2;             % Boundary weights
end

end


function pass = test_mean(pref)

if ( nargin == 0 )
    pref = chebfunpref();
end

%% Two arguments:
f = chebfun(@sin, pref);
g = chebfun(@cos, pref);
h = chebfun(@(x) .5*(sin(x) + cos(x)), pref);
pass(1) = normest(mean(f, g) - h) < 10*eps;

f = chebfun(@(x) [sin(x), cos(x)], pref);
g = chebfun(@(x) [cos(x), 1i*exp(x)], pref);
h = .5*(f+g);
pass(2) = normest(mean(f, g) - h) < 10*eps;

%% One argument:
f = chebfun(@sin, pref);
pass(3) = abs(mean(f)) < eps;
pass(4) = abs(mean(f.')) < eps;

f = chebfun(@(x) [sin(x), x], pref);
pass(5) = norm(mean(f), inf) < eps;
pass(6) = norm(mean(f.'), inf) < eps;

f = chebfun(@(x) [sin(x), x], [0, 6], pref);
pass(7) = norm(mean(f) - sum(f)/6, inf) < vscale(f)*eps;
pass(8) = norm(mean(f.') - sum(f).'/6, inf) < vscale(f)*eps;

%% singular function: a finite case

% define the domain:
dom = [-2 7];

op = @(x) sin(100*x)./((x-dom(1)).^0.5.*(x-dom(2)).^0.5);
f = chebfun(op, dom, 'exps', [-0.5 -0.5], 'splitting', 'on');
m = mean(f);
m_exact = -0.01273522016443600i;
pass(9) = abs(m - m_exact) < 1e5*eps*abs(m_exact);

%% singular function: an infinite case

% define the domain:
dom = [-2 7];

op = @(x) sin(100*x)./((x-dom(1)).^1.5.*(dom(2)-x).^0.5);
f = chebfun(op, dom, 'exps', [-1.5 -0.5], 'splitting', 'on');
m = mean(f);
pass(10) = ( isinf(m) );

%% singular function: a NaN case

% define the domain:
dom = [-2 7];

op = @(x) sin(98*x)./((x-dom(1)).^1.5.*(dom(2)-x).^1.5);
f = chebfun(op, dom, 'exps', [-1.5 -1.5], 'splitting', 'on');
m = mean(f);
pass(11) = ( isnan(m) );

%% Test for functions defined on unbounded domain:

% Functions on [-inf inf]:

% Set the domain:
dom = [-Inf 2 Inf];

op1 = @(x) x.^2.*exp(-x.^2);
op2 = @(x) (1-exp(-x.^2))./x.^2 + 2;
f = chebfun({op1 op2}, dom);
M = mean(f);
pass(12) = isnan(M);

% Function defined on [0 Inf]:

% Specify the domain:
dom = [0 Inf];

op = @(x) 0.75+sin(10*x)./exp(x);
f = chebfun(op, dom, 'splitting', 'on');
M = mean(f);
pass(13) = abs(M - 0.75) < 1e2*eps*vscale(f);

x = chebfun('x');
f = mean([x 3*x], 2);
pass(14) = norm(f-2*x) < 1e2*eps;

f = mean([x 3*x], 1);
pass(15) = norm( f - mean([x 3*x]) ) == 0;

pass(16) = (norm(mean(x,1) - mean(x.',2)) == 0);

pass(17) = (norm(mean(x') - 0) < 1e2*eps);

pass(18) = (norm(mean(x',1) - x') == 0);

end

function out = normest(f, dom)

% Generate a few random points to use as test values.
seedRNG(6178);
if ( nargin == 1 )
    x = 2 * rand(100, 1) - 1;
else
    x = sum(dom) * rand(10, 1) - dom(1);
end

out = norm(feval(f, x), inf);
end


function s=minmod(a,b)
% The minmod limiter
s=0.5*(sign(a)+sign(b)).*min(abs(a),abs(b));


%DEMO_NEURALNETCOV  Demonstration of Gaussian process with a neural
%                   network covariance function
%
%  Description
%    Infinite neural network solutions in 2D and 1D regression
%    problems with a comparison to Gaussian process solution given
%    by squared exponential covariance function. The noisy
%    observations y are assumed to satisfy
%
%         y = f + e,    where e ~ N(0, s^2)
%
%    where f is an unknown underlying function. A zero mean
%    Gaussian process prior is assumed for f
%
%         f ~ N(0, K),
%
%    where K is the covariance matrix whose elements are given by
%    neural network (or squared exponential) covariance function. A
%    prior is assumed for parameters of the covariance functions,
%    and the inference is done with a MAP estimate for parameter
%    values.
%
%    For more detailed discussion of infinite neural networks, see
%    e.g.
%
%      Neal, R. M. (1996). Bayesian Learning for Neural Networks.
%      Springer-Verlag.
%
%      Williams, C. K. I. (1996). Computing with infinite networks.
%      In Advances in Neural Information Processing Systems 9. MIT
%      Press, Cambridge, MA.
%
%
%  See also DEMO_REGRESSION1

% Copyright (c) 2010 Jaakko Riihim�ki, 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.

% 2D REGRESSION DATA

% create 2D example data
x=rand(300,2)*2-1;
y=zeros(size(x,1),1); y(x(:,1)>0&x(:,2)>0)=1;
y=y+0.1*randn(size(y));

[n, nin] = size(x);

% --- Construct the model ---

% squared exponential covariance function
gpcf1 = gpcf_sexp('lengthScale', ones(1,nin), 'magnSigma2', 1);
% neural network covariance function
gpcf2 = gpcf_neuralnetwork('weightSigma2', ones(1,nin), 'biasSigma2', 1);
% Gaussian noise structures
lik = lik_gaussian('sigma2', 0.2^2);

% a prior structure for GP parameters
pt = prior_t('s2', 4);
gpcf1 = gpcf_sexp(gpcf1, 'lengthScale_prior', pt, 'magnSigma2_prior', pt);
gpcf2 = gpcf_neuralnetwork(gpcf2, 'weightSigma2_prior', pt, 'biasSigma2_prior', pt);

gp = gp_set('lik', lik, 'cf', gpcf1);
gp2 = gp_set('lik', lik, 'cf', gpcf2);

% --- MAP estimate using scaled conjugate gradient algorithm ---
%     (see scg for more details)

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);
gp2=gp_optim(gp2,x,y,'opt',opt);

% create points where predictions are made
[xt1,xt2]=meshgrid(-1.5:0.05:1.5,-1.5:0.05:1.5);
xt=[xt1(:) xt2(:)];
% compute the predictions
[Eft_map, Varft_map] = gp_pred(gp, x, y, xt);
[Eft_map2, Varft_map2] = gp_pred(gp2, x, y, xt);

% Plot the predictions and data
figure, set(gcf, 'color', 'w')
mesh(xt1, xt2, reshape(Eft_map,size(xt1,1),size(xt1,2)));
hold on
plot3(x(:,1), x(:,2), y, '*')
axis on;
title('GP (squared exponential) predictions and the data points');

figure, set(gcf, 'color', 'w')
mesh(xt1, xt2, reshape(Eft_map2,size(xt1,1),size(xt1,2)));
hold on
plot3(x(:,1), x(:,2), y, '*')
axis on;
title('GP (neural network) predictions and the data points');

% 1D REGRESSION DATA

% create a 1D toy data
x=rand(100,1)*4-2;
y=norm_pdf(4*x)+0.05*randn(size(x));
[n, nin] = size(x);

gpcf1 = gpcf_sexp('lengthScale', ones(1,nin), 'magnSigma2', 1);
gpcf2 = gpcf_neuralnetwork('weightSigma2', ones(1,nin), 'biasSigma2', 1);

gpcf1 = gpcf_sexp(gpcf1, 'lengthScale_prior', pt, 'magnSigma2_prior', pt);
gpcf2 = gpcf_neuralnetwork(gpcf2, 'weightSigma2_prior', pt, 'biasSigma2_prior', pt);
lik = lik_gaussian();

gp = gp_set('lik', lik, 'cf', gpcf1);
gp2 = gp_set('lik', lik, 'cf', gpcf2);

% --- MAP estimate using scaled conjugate gradient algorithm ---
%     (see fminscg for more details)

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3);
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);
gp2=gp_optim(gp2,x,y,'opt',opt);

% create points where predictions are made
xgrid=linspace(min(x)-1.5,max(x)+1.5,200)';
[Eft_map, Varft_map, lpyt_map, Eyt_map, Varyt_map] = gp_pred(gp, x, y, xgrid, 'yt', ones(200,1));
[Eft_map2, Varft_map2, lpyt_map2, Eyt_map2, Varyt_map2] = gp_pred(gp2, x, y, xgrid, 'yt', ones(200,1));

% Plot the predictions and data
color1=ones(1,3)*0.8; color2=ones(1,3)*0.5;
figure, set(gcf, 'color', 'w'), hold on
h1=fill([xgrid' fliplr(xgrid')], [(Eyt_map+1.96*sqrt(Varyt_map))' fliplr((Eyt_map-1.96*sqrt(Varyt_map))')], color1, 'edgecolor', color1);
% GP mean
h2=plot(xgrid, Eyt_map, 'color', color2, 'linewidth', 3);
% observations
h3=plot(x, y, 'xk', 'markersize', 10, 'linewidth', 2);
% true function
h4=plot(xgrid, norm_pdf(4*xgrid), 'color', 'r', 'linewidth', 2);
legend([h1 h2 h3 h4], 'GP 95% CI', 'GP mean', 'observations', 'true latent function')
title('GP (squared exponential) predictions and the data points');

figure, set(gcf, 'color', 'w'), hold on
h1=fill([xgrid' fliplr(xgrid')], [(Eyt_map2+1.96*sqrt(Varyt_map2))' fliplr((Eyt_map2-1.96*sqrt(Varyt_map2))')], color1, 'edgecolor', color1);
% GP mean
h2=plot(xgrid, Eyt_map2, 'color', color2, 'linewidth', 3);
% observations
h3=plot(x, y, 'xk', 'markersize', 10, 'linewidth', 2);
% true function
h4=plot(xgrid, norm_pdf(4*xgrid), 'color', 'r', 'linewidth', 2);
legend([h1 h2 h3 h4], 'GP 95% CI', 'GP mean', 'observations', 'true latent function')
title('GP (neural network) predictions and the data points');


function	[W ,Ydim, Xdim, Dtau] = get_estimated_weight(Model,parm,wmode);
% return Weight matrix from 'Model'
%  [W ,Ydim, Xdim, Dtau] = get_estimated_weight(Model,parm,wmode);
% W : Weight matrix : 2D-matrix [Ydim x (Xdim * Dtau)]
% Dtau : time embedding dim
% Ydim : Output space dim
% Xdim : Input space dim
%   To get temporal filter shape of Weight matrix
% Wd = reshape( W, [Ydim, Xdim, Dtau]);
% Wd(n,m,:) : Weight for n-th output & m-th input data
%
% 2008-5-20 Masa-aki Sato

if isfield(parm,'Dtau')
	Dtau  = parm.Dtau;
else
	Dtau  = 1;
end

M_all = Model.M_all;
Ydim  = size(Model.W,1);
Xdim  = M_all/Dtau;

if isfield(Model,'ix_act')
	% Active index
	ix_act = Model.ix_act;

	W = zeros(Ydim ,M_all);
	W(:,ix_act) = Model.W;
else
	W =  Model.W;
end

if exist('wmode','var') && wmode==0, return; end;

if length(parm.xmean) == Xdim,
	parm.xmean = repmat(parm.xmean ,[Dtau 1]);
	parm.xnorm = repmat(parm.xnorm ,[Dtau 1]);
end

% Scale back by normalization factor
if isfield(parm,'xnorm') & isfield(parm,'ynorm')
	W = (parm.ynorm(:)*(1./parm.xnorm(:)')) .* W;
end


close;
clear;

load models.mat;
scale_rat = 10;
NN_Number = 5;

%[ img1_desc_types, detectedPts1, locations1,pts1 ] = create_mesh_sift_features(vertex1, faces1, scale_rat);
[ img1_desc_types, ~, ~,pts1 ] = create_mesh_spin_features(vertex1, faces1,5,scale_rat);

%[ img2_desc_types, detectedPts2, locations2,pts2] = create_mesh_sift_features(vertex2, faces2, scale_rat);
[ img2_desc_types, ~, ~,pts2] = create_mesh_spin_features(vertex2, faces2,5,scale_rat);

DescrData = pdist2(img1_desc_types,img2_desc_types);

[~,NN_Data]=sort(DescrData,2,'ascend');
NN_Data=NN_Data(:,1:NN_Number);


function [lambda, r2] = duration_amplitude(self, law, min_amplitude, mag_zone)
    %DURATION_AMPLITUDE Use the duration-amplitde
    %Compute the fraction of a data series above each
    %amplitude level, plot the duration vs amplitude data, and then
    %allow user input to fit a regression line.
    %   rsamObject.duration_amplitude(law, min_amplitude)
    %
    %   Inputs:
    %       law = 'exponential' or 'power'
    %       min_amplitude = (Optional) the smallest amplitude to use on
    %       the x-axis. Otherwise will be 10^10 times smallest than the
    %       largest amplitude.
    %
    %   Outputs:
    %       (None) A graph is plotted, the user clicks two points, and
    %       from that slope, the characteristic amplitude is computed
    %       and shown on the screen.

    y = self.data;
    n = length(y);
    a = abs(y);
    max_amplitude = max(a);
    if ~exist('min_amplitude', 'var')
        min_amplitude = max([min(a) max(a)*1e-10]);
    end

    % Method 1
    index=0;
    x = min_amplitude;
    while x < max_amplitude,
        i = find(a>x);
        f = length(i);
        index = index+1;
        frequency(index) = f;
        threshold(index) = x;
        x = x * 1.2;
    end
    clear x y a  f  n  min_amplitude max_amplitude index ;

    %             % Method 2
    %             threshold = [0.0 logspace(min_amplitude, max_amplitude, 50)];
    %             nsamples=[];
    %             for d = 1:length(threshold)
    %                 i = find(a > threshold(d));
    %                 frequency(d) = length(i)/length(y);
    %             end
    %             clear d, nsamples, i, y

    %% PLOT_DURATION_AMPLITUDE
    % plot graph, user select two points, compute
    % characteristic from slope.
    % Use different method depending on whether it is a
    % power law or exponential.

    % define x and y
    switch law
        case {'exponential'}
            x = threshold;
            xlabelstr = 'RMS Displacement(nm)';
        case {'power'}
            x = log10(threshold);
            xlabelstr = 'log10(RMS Displacement(nm))';
        otherwise
            error('law unknown')
    end
    y=log10(frequency);

    % plot duration-amplitude data as circles
    figure
    plot(x,y,'o');
    xlabel(xlabelstr);
    ylabel('log10(Cumulative Minutes)');
    hold on;
    %set(gca,'XLim',[xmin xmax]);

    lambda=0;
    r2=0;

    % check if we have pre-set the magnitude range, effectively
    % our x1 and x2 click points with ginput
    if exist('mag_zone','var') % no user select
        switch law
            case {'power'}
                x1=min(mag_zone);
                x2=max(mag_zone);
            case {'exponential'}
                x1=10^min(mag_zone);
                x2=10^max(mag_zone);

            otherwise
                error('law unknown')
        end
        if x1<min(x)
            x2=min(x);
        end
        y1=interp1(x,y,x1);
        if x2>max(x)
            x2=max(x);
        end
        y2=interp1(x,y,x2);

        % draw a dotted line to show where user selected
        %plot([x1 x2], [y1 y2], '-.');

        % select requested data range and do a least squares fit
        ii = find(x >= x1 & x <= x2);
        wx = x(ii);
        wy = y(ii);
        [p,S]=polyfit(wx,wy,1);
        yfit = polyval(p,wx);
        thiscorr = corrcoef(wy, yfit)
        %try
        if numel(thiscorr)>1
            r2 = thiscorr(1,2);

            % compute lambda
            switch law
                case {'exponential'}
                    lambda = -p(1)/log10(exp(1));
                case {'power'}
                    lambda = -p(1);
                otherwise
                    error('law unknown')
            end

            disp(sprintf('characteristic D_R_S=%.2f cm^2, R^2=%.2f',lambda,r2));

            % draw the fitted line
            xf = [min(wx) max(wx)];
            yf = xf * p(1) + p(2);
            plot(xf, yf,'-');

            %ylabel('log10(t/t0)');
            %xlabel(sprintf('D_R_S (%s) (cm^2)',measure));

            % Add legend
            yrange=get(gca,'YLim');
            xlim = get(gca,'XLim');
            xmax=max(xlim);

            xpos = xmax*0.65;
            ypos = (yrange(2)-yrange(1))*0.8;
            r2str=sprintf('%.2f',r2);
            lambdastr=sprintf('%.2f',lambda);
            if strcmp(law,'exponential')
                tstr = [' \lambda=',lambdastr,' R^2=',r2str];
            else
                tstr = [' \gamma=',lambdastr,' R^2=',r2str];
            end

            text(xpos, ypos, tstr, ...
                'FontName','Helvetica','FontSize',[14],'FontWeight','bold');
        else
            lambda=NaN;
            r2=NaN;
        end

    else

        % user select a range of data
        disp('Left-click Select lowest X, any other mouse button to ignore this station')
        [x1, y1, button1]=ginput(1);
        if button1==1
            disp('Left-click Select highest X, any other mouse button to ignore this station')
            [x2, y2, button2]=ginput(1);
            if button2==1
                if x2>x1
                   % draw a dotted line to show where user selected
                    plot([x1 x2], [y1 y2], '-.');

                    % select requested data range and do a least squares fit
                    ii = find(x >= x1 & x <= x2);
                    wx = x(ii);
                    wy = y(ii);
                    [p,S]=polyfit(wx,wy,1);
                    yfit = polyval(p,wx);
                    thiscorr = corrcoef(wy, yfit)

                    r2 = thiscorr(1,2);

                    % compute lambda
                    switch law
                        case {'exponential'}
                            lambda = -p(1)/log10(exp(1));
                        case {'power'}
                            lambda = -p(1);
                        otherwise
                            error('law unknown')
                    end

                    disp(sprintf('characteristic D_R_S=%.2f cm^2, R^2=%.2f',lambda,r2));

                    % draw the fitted line
                    xf = [min(wx) max(wx)];
                    yf = xf * p(1) + p(2);
                    plot(xf, yf,'-');

                    %ylabel('log10(t/t0)');
                    %xlabel(sprintf('D_R_S (%s) (cm^2)',measure));

                    % Add legend
                    yrange=get(gca,'YLim');
                    xlim = get(gca,'XLim');
                    xmax=max(xlim);

                    xpos = xmax*0.65;
                    ypos = (yrange(2)-yrange(1))*0.8;
                    r2str=sprintf('%.2f',r2);
                    lambdastr=sprintf('%.2f',lambda);
                    if strcmp(law,'exponential')
                        tstr = [self.sta,' \lambda=',lambdastr,' R^2=',r2str];
                    else
                        tstr = [self.sta,' \gamma=',lambdastr,' R^2=',r2str];
                    end

                    text(xpos, ypos, tstr, ...
                        'FontName','Helvetica','FontSize',[14],'FontWeight','bold');

                end
            end
        end
    end
end


function out = echo_enc_bf(signal, text, d0, d1, alpha, L)
%ECHO_ENC Echo Hiding with Backward and Forward Echo Kernels
%
%   INPUT VARIABLES
%       signal : Cover signal
%       text   : Message to hide
%       d0     : Delay rate for bit0
%       d1     : Delay rate for bit1
%       alpha  : Echo amplitude
%       L      : Length of frames
%
%   OUTPUT VARIABLES
%       out    : Stego signal
%
%   Kadir Tekeli (kadir.tekeli@outlook.com)

if nargin < 4
	d0 = 150;     %Delay rate for bit0
	d1 = 200;     %Delay rate for bit1
end

if nargin < 5
	alpha = 0.5;  %Echo amplitude
end

if nargin < 6
	L = 8*1024;   %Length of frames
end

[s.len, s.ch] = size(signal);
bit = getBits(text);
nframe = floor(s.len/L);
N = nframe - mod(nframe,8);      %Number of frames (for 8 bit)

if (length(bit) > N)
	warning('Message is too long, being cropped!');
	bits = bit(1:N);
else
	warning('Message is being zero padded...');
	bits = [bit, num2str(zeros(N-length(bit), 1))'];
end

[echo_zro, echo_one] = bf_echo(signal, d0, d1, alpha);  %Echo signals
mix = mixer(L, bits, 0, 1, 256) * ones(1, s.ch);        %Mixer signal

%%%%%%%%%%%%%%%%%%%%%%% EMBEDDING MESSAGE... %%%%%%%%%%%%%%%%%%%%%%%
out = signal(1:N*L, :) + echo_zro(1:N*L, :) .* abs(mix-1) ...
                       + echo_one(1:N*L, :) .* mix;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
out = [out; signal(N*L+1:s.len, :)];   %Rest of the signal
end


classdef ContrastiveLoss < dagnn.GenericLoss
  properties
    margin = 1;
  end

  methods
    function outputs = forward(obj, inputs, params)
      switch numel(inputs)
        case 3
          outputs{1} = vl_nncontrloss(inputs{:}, 'margin', obj.margin);
        case 4
          outputs{1} = vl_nncontrloss(inputs{1:3}, 'margin', inputs{4});
        otherwise
          error('Invalid number of inputs.');
      end
      if true
        isPos = inputs{3} == 1;
        % posDist = mean(norm(reshape(inputs{1}(:, :, :, isPos) - inputs{2}(:, :, :, isPos), [], sum(isPos))));
        % negDist = mean(norm(reshape(inputs{1}(:, :, :, ~isPos) - inputs{2}(:, :, :, ~isPos), [], sum(~isPos))));
        posDist = sqrt(sum((reshape(inputs{1}(:, :, :, isPos) - inputs{2}(:, :, :, isPos), [], sum(isPos))).^2,1));
        negDist = sqrt(sum((reshape(inputs{1}(:, :, :, ~isPos) - inputs{2}(:, :, :, ~isPos), [], sum(~isPos))).^2,1));
        eer = vl_eer([-1*ones(1,numel(posDist)) 1*ones(1,numel(negDist))],[posDist negDist]);
        fprintf('PD: %.2f ND: %.2f EER: %.3f ', mean(posDist), mean(negDist) ,eer);
      end

      logoutputs = outputs;
      logoutputs{1} = numel(inputs{3})*eer;
      obj.account(inputs, logoutputs);
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
      switch numel(inputs)
        case 3
          [dzdx1, dzdx2] = vl_nncontrloss(inputs{:}, derOutputs{1}, 'margin', obj.margin);
        case 4
          [dzdx1, dzdx2] = vl_nncontrloss(inputs{1:3}, derOutputs{1}, 'margin', inputs{4});
        otherwise
          error('Invalid number of inputs.');
      end
      derInputs = {dzdx1, dzdx2, []};
      derParams = {} ;
    end

    function obj = ContrastiveLoss(varargin)
      obj.load(varargin{:}) ;
    end
  end
end


function sws = sw_normalized(sw, swc, sor)
%SW_NORMALIZED maps the saturation from [0, 1] to [swc, 1-sor]
sws=((sw>swc).*(sw<1-sor).*(sw-swc)./(1-sor-swc)+(sw>=1-sor));
end


classdef ChTargetFactory < handle

    methods (Access = public, Static)

        function Ch_star = create(cParams)

            switch cParams.type
                case 'negative_poisson'

                    E1  = cParams.E_plus;
                    E0  = cParams.E_minus;
                    nu1 = cParams.nu_plus;
                    nu0 = cParams.nu_minus;
                    kappa_f = @(E,nu) E/2*(1-nu);
                    mu_f = @(E,nu) E/2*(1-nu);

                    k_plus = kappa_f(E1,nu1);
                    mu_plus = mu_f(E1,nu1);

                    k_minus = kappa_f(E0,nu0);
                    mu_minus = mu_f(E0,nu0);

                    nu = @(k,mu) (k-mu)/(k+mu);
                    E = @(k,mu) (4*k*mu)/(k+mu);
                    C = @(E,nu) (E/(1-nu*nu)*[1 nu 0; nu 1 0; 0 0 (1-nu)/2]);

                    kappa_nu_min = k_minus;
                    mu_nu_min = mu_plus;

                    nu_min = nu(kappa_nu_min,mu_nu_min);
                    E_nu_min = E(kappa_nu_min,mu_nu_min);
                    C_nu_min = C(E_nu_min,nu_min);
                    Ch_star = C_nu_min;

                case 'nu_0_6' %From Sigmund Thesis% rho = 0.38
                    C = @(E,nu) (E/(1-nu*nu)*[1 nu 0; nu 1 0; 0 0 (1-nu)/2]);
                    nu = -0.6;
                    E = (1-nu*nu)*0.04;
                    Ch_star = C(E,nu);

                case 'Seba' % Es=0.08; nus=-0.25
                    Ch_star = [0.0853    -0.0213       0;
                               -0.0213    0.0853       0;
                                   0         0    0.0533];

                case 'Nu0_2'
                    %Ch_star = [0.0287   -0.0069    0.0068;
                    %           -0.0069    0.0287    0.0068;
                    %           0.0068    0.0068    0.0137];

                    %Ch_star = [0.0129   -0.0158    0.0000;
                    %           -0.0158    0.0812   -0.0000;
                    %           0.0000   -0.0000    0.0021];

                    Ch_star = [  0.0157   -0.0174   -0.0000;
                                -0.0174    0.0815   -0.0000;
                                -0.0000   -0.0000    0.0025];

                case 'nu_0_8' %From Sigmund Thesis% rho = 0.25
                    C = @(E,nu) (E/(1-nu*nu)*[1 nu 0; nu 1 0; 0 0 (1-nu)/2]);
                    nu = -0.8;
                    E = (1-nu*nu)*0.02;
                    Ch_star = C(E,nu);

                case 'Vfrac07'
                    Ch_star =[
                        0.4256    0.2837         0
                        0.2837    0.4256         0
                        0         0    0.1419];

                case 'Vfrac06'
                    Ch_star =[
                        0.2909    0.1940         0
                        0.1940    0.2909         0
                        0         0    0.0970];

                case 'Vfrac05'
                    Ch_star =[
                        0.1892    0.1261         0
                        0.1261    0.1892         0
                        0         0    0.0631];

                case 'Vfrac04'
                    Ch_star =[
                        0.1141    0.0761         0
                        0.0761    0.1141         0
                        0         0    0.0380];

                case 'Vfrac04b' %Circle of 0.4 radius
                         Ch_star = [ 0.7519    0.2237   -0.0000;
                                  0.2237    0.7519   -0.0000;
                                 -0.0000   -0.0000    0.2245];

                case 'Vfrac03'
                    Ch_star =[
                        0.0611    0.0407         0
                        0.0407    0.0611         0
                        0         0    0.0204];

                case 'HorizontalRectangleInclusion'
                    Ch_star = [0.4637    0.0010    0.0000
                               0.0010    0.0031    0.0000
                               -0.0000    0.0000    0.0011];

                case 'Composite'
                    Ch_star =[1 0.15 0;
                        0.15 0.5 0;
                        0 0 0.2];
                case 'HoneyComb'
                    Ch_star =0.094*[1 0.75 0
                        0.75 1 0
                        0 0 0.125];
                case 'InvertedHoneyComb'
                    Ch_star =0.08*[1 -0.5 0
                        -0.5 1 0
                        0 0 0.06];
                case 'AcousticZeroShearA'
                    Ch_star =[1 1 0;
                        1 1 0;
                        0 0 0];
                case 'NegativePoiss06'
                    Ch_star =0.04*[1 -0.6 0;
                        -0.6 1 0;
                        0 0 0.8];
                case 'IsotropyHexagon'
                    Ch_star = [0.5931 0.1882 0.0000;
                               0.1882 0.5931 0.0000;
                               0.0000 0.0000 0.2025];
            end
        end
    end

end


function [tg,theta] = tgmo(tmap,ntex,radius,norient,varargin)
% function [tg,theta] = tgmo(tmap,ntex,radius,norient,...)
%
% Compute the texture gradient at a single scale and multiple
% orientations.
%
% INPUT
%	tmap		Texton map, values in [1,ntex].
%	ntex		Number of textons.
%	radius		Radius of disc for texture gradient.
%	norient		Number of orientation at which to compute
%			the texture gradient.
%	'smooth'	Smoothing method, one of
%			{'gaussian','savgol','none'}, default 'none'.
%	'sigma'		Sigma for smoothing, default to radius.
%	'tsim'		Texton similarity matrix.  If not
%			provided, then use chi-squared.
%
% OUTPUT
%	tg		Size [h w norient] array of tg images.
%	theta		Vector of disc orientations (which are
%			orthogonal to the texture gradient).
%
% David R. Martin <dmartin@eecs.berkeley.edu>
% March 2003

% process options
smooth = 'none';
sigma = radius;
usechi2 = true;
for i = 1:2:numel(varargin),
  opt = varargin{i};
  if ~ischar(opt), error('option names not a string'); end
  if i==numel(varargin), error(sprintf('option ''%s'' has no value',opt)); end
  val = varargin{i+1};
  switch opt,
   case 'smooth',
    switch val,
     case {'none','gaussian','savgol'}, smooth=val;
     otherwise, error(sprintf('invalid option smooth=''%s''',val));
    end
   case 'sigma', sigma=val;
   case 'tsim', tsim=val; usechi2=false;
   otherwise, error(sprintf('invalid option ''%s''',opt));
  end
end

radius = max(1,radius);
norient = max(1,norient);
theta = (0:norient-1)/norient*pi;

[h,w] = size(tmap);
tg = zeros(h,w,norient);
fwrite(2,'[');
for i = 1:norient,
  fwrite(2,'.');
  if usechi2,
    tg(:,:,i) = tgso(tmap,ntex,radius,theta(i),...
                     'smooth',smooth,'sigma',sigma);
  else
    tg(:,:,i) = tgso(tmap,ntex,radius,theta(i),...
                     'smooth',smooth,'sigma',sigma,'tsim',tsim);
  end
end
fwrite(2,sprintf(']\n'));