%[2019]-"A new meta-heuristic optimizer: Pathfinder algorithm"

% (8/12/2020)

function PFA = jPathFinderAlgorithm(feat,label,opts)
% Parameters
lb    = 0;
ub    = 1;
thres = 0.5;

if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'thres'), thres = opts.thres; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = zeros(N,dim);
for i = 1:N
	for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
	end
end
% Fitness
fit  = zeros(1,N);
fitP = inf;
for i = 1:N
  fit(i) = fun(feat,label,(X(i,:) > thres),opts);
  % Pathfinder update
  if fit(i) < fitP
    fitP = fit(i);
    Xpf  = X(i,:);
  end
end
% Set previous pathfiner
Xpf_old = Xpf;
% Pre
Xpf_new = zeros(1,dim);
Xnew    = zeros(N,dim);

curve = zeros(1,max_Iter);
curve(1) = fitP;
t = 2;
% Iterations
while t <= max_Iter
  % Alpha & beta in [1,2]
  alpha = 1 + rand();
  beta  = 1 + rand();
  for d = 1:dim
    % Define u2 in [-1,1]
    u2 = -1 + 2 * rand();
    % Compute A (2.6)
    A  = u2 * exp(-(2 * t) / max_Iter);
    % Update pathfinder (2.4)
    r3 = rand();
    Xpf_new(d) = Xpf(d) + 2 * r3 * (Xpf(d) - Xpf_old(d)) + A;
  end
  % Boundary
  Xpf_new(Xpf_new > ub) = ub; Xpf_new(Xpf_new <lb) = lb;
  % Update previous path
  Xpf_old = Xpf;
  % Fitness
  Fnew = fun(feat,label,(Xpf_new > thres),opts);
  % Greedy selection
  if Fnew < fitP
    fitP = Fnew;
    Xpf  = Xpf_new;
  end
  % Sort member
  [fit, idx] = sort(fit,'ascend');
  X          = X(idx,:);
  % Update first solution
  if Fnew < fit(1)
    fit(1) = Fnew;
    X(1,:) = Xpf_new;
  end
  % Update
  for i = 2:N
    % Distance (2.5)
    Dij = norm(X(i,:) - X(i-1,:));
    for d = 1:dim
      % Define u1 in [-1,1]
      u1  = -1 + 2 * rand();
      % Compute epsilon (2.5)
      eps = (1 - (t / max_Iter)) * u1 * Dij;
      % Define R1, R2
      r1  = rand();
      r2  = rand();
      R1  = alpha * r1;
      R2  = beta * r2;
      % Update member (2.3)
      Xnew(i,d) = X(i,d) + R1 * (X(i-1, d) - X(i,d)) + ...
        R2 * (Xpf(d) - X(i,d)) + eps;
    end
    % Boundary
    XB = Xnew(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    Xnew(i,:) = XB;
  end
  % Fitness
  for i = 2:N
    % Fitness
    Fnew = fun(feat,label,(Xnew(i,:) > thres),opts);
    % Selection
    if Fnew < fit(i)
      fit(i) = Fnew;
      X(i,:) = Xnew(i,:);
    end
    % Pathfinder update
    if fit(i) < fitP
      fitP = fit(i);
      Xpf  = X(i,:);
    end
  end
  curve(t) = fitP;
  fprintf('\nIteration %d Best (PFA)= %f',t,curve(t))
  t = t + 1;
end
% Select features
Pos   = 1:dim;
Sf    = Pos((Xpf > thres) == 1);
sFeat = feat(:,Sf);
% Store results
PFA.sf = Sf;
PFA.ff = sFeat;
PFA.nf = length(Sf);
PFA.c  = curve;
PFA.f  = feat;
PFA.l  = label;
end


function [DIP, teta, hr]= DipoleGeneratorAbnormal(N,fs,rr,alphai,bi,tetai,teta0)
%
% [DIP teta]= DipoleGeneratorAbnormal(N,fs,rr,alphai,bi,tetai,teta0)
% Synthetic cardiac dipole generator using the 'differential form' of the
% dipole equations. Refer to references of the toolbox for further details.
%
% inputs:
% N: signal length
% fs: sampling rate
% rr: rr interval time series
% alphai: structure contaning the amplitudes of Gaussian functions used for
%       modeling the x, y, and z coordinates of the cardiac dipole
% bi: structure contaning the widths of Gaussian functions used for
%       modeling the x, y, and z coordinates of the cardiac dipole
% tetai: structure contaning the phase of Gaussian functions used for
%       modeling the x, y, and z coordinates of the cardiac dipole
% teta0: initial phase of the synthetic dipole

%
% output:
% DIP: structure contaning the x, y, and z coordinates of the cardiac dipole
% teta: vector containing the dipole phase
%
%
% Open Source ECG Toolbox, version 2.0, April 2008
% Released under the GNU General Public License
% Copyright (C) 2008  Reza Sameni
% Sharif University of Technology, Tehran, Iran -- GIPSA-Lab, Grenoble, France
% reza.sameni@gmail.com

% This program is free software; you can redistribute it and/or modify it
% under the terms of the GNU General Public License as published by the
% Free Software Foundation; either version 2 of the License, or (at your
% option) any later version.
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
% Public License for more details.

% Last update Shamim Nemati Feb 13 2009

rr = rr(:); t = [cumsum(rr)]; tp = linspace(0,sum(rr),N);
rrp = spline(t,rr,tp);
f=1./rrp; % make a heart rate (per second) time-series from rr intervals resampled to the current sampling frequency
hr = f*60;
w = 2*pi*f;     dt = 1/fs;
teta = zeros(1,N); X = zeros(1,N); Y = zeros(1,N); Z = zeros(1,N);

teta(1) = teta0;

for i = 1:N-1
    teta(i+1) = teta(i) + w(i)*dt; %dtheta/dt = w
    if(teta(i+1)>pi) % beat transition ------------------------------------
        teta(i+1) = teta(i+1) - 2*pi;
    end
    %------------------------------------------
    dtetaix = mod(teta(i) - tetai.x + pi , 2*pi) - pi;
    dtetaiy = mod(teta(i) - tetai.y + pi , 2*pi) - pi;
    dtetaiz = mod(teta(i) - tetai.z + pi , 2*pi) - pi;

    if(i==1),
        X(i) = sum(alphai.x .* exp(-dtetaix .^2 ./ (2*bi.x .^ 2)));
        Y(i) = sum(alphai.y .* exp(-dtetaiy .^2 ./ (2*bi.y .^ 2)));
        Z(i) = sum(alphai.z .* exp(-dtetaiz .^2 ./ (2*bi.z .^ 2)));
    end
    %------ Eq.(1) in Clifford et al. CinC2008 ------
    X(i+1) = X(i) - dt*sum(w(i)*alphai.x ./ (bi.x) .^ 2 .* dtetaix .* exp(-dtetaix .^2 ./ (2* bi.x .^ 2)));   % x state variable
    Y(i+1) = Y(i) - dt*sum(w(i)*alphai.y ./ (bi.y) .^ 2 .* dtetaiy .* exp(-dtetaiy .^2 ./ (2* bi.y .^ 2)));   % y state variable
    Z(i+1) = Z(i) - dt*sum(w(i)*alphai.z ./ (bi.z) .^ 2 .* dtetaiz .* exp(-dtetaiz .^2 ./ (2* bi.z .^ 2)));   % z state variable
    %------------------------------------------------

end
DIP.x = X;
DIP.y = Y;
DIP.z = Z;


function [mu,dmu,k,gamma] = sparse_learning(Phi,T,lambda,iters,flag1,flag2,flag3)
% *************************************************************************
%
% *** PURPOSE ***
% Implements generalized versions of SBL and FOCUSS for learning sparse
% representations from possibly overcomplete dictionaries.
%
%
% *** USAGE ***
% [mu,dmu,k,gamma] = sparse_learning(Phi,T,lambda,iters,flag1,flag2,flag3);
%
%
% *** INPUTS ***
% Phi       = N X M dictionary
% T         = N X L data matrix
% lambda    = scalar trade-off parameter (balances sparsity and data fit)
% iters     = maximum number of iterations
%
% flag1     = 0: fast Mackay-based SBL update rules
% flag1     = 1: fast EM-based SBL update rule
% flag1     = 2: traditional (slow but sometimes better) EM-based SBL update rule
% flag1     = [3 p]: FOCUSS algorithm using the p-valued quasi-norm
%
% flag2     = 0: regular initialization (equivalent to min. norm solution)
% flag2     = gamma0: initialize with gamma = gamma0, (M X 1) vector
%
% flag3     = display flag; 1 = show output, 0 = supress output
%
% *** OUTPUTS ***
% mu        = M X L matrix of weight estimates
% dmu       = delta-mu at convergence
% k         = number of iterations used
% gamma     = M X 1 vector of hyperparameter values
%
%
% *************************************************************************
% Written by:  David Wipf, david.wipf@mrsc.ucsf.edu
% *************************************************************************

% *** Control parameters ***
MIN_GAMMA       = 1e-16;
MIN_DMU         = 1e-12;
MAX_ITERS       = iters;
DISPLAY_FLAG    = flag3;     % Set to zero for no runtime screen printouts

% *** Initializations ***
[N M] = size(Phi);
[N L] = size(T);

if (~flag2)         gamma = ones(M,1);
else                gamma = flag2;  end;

keep_list = [1:M]';
m = length(keep_list);
mu = zeros(M,L);
dmu = -1;
k = 0;

% *** Learning loop ***
for k=0:MAX_ITERS-1

    % *** Prune things as hyperparameters go to zero ***
    if (min(gamma) < MIN_GAMMA )
		index = find(gamma > MIN_GAMMA);
		gamma = gamma(index);
		Phi = Phi(:,index);
		keep_list = keep_list(index);
        m = length(gamma);

        if (m == 0)   break;  end;
    end;

    % *** Compute new weights ***
    G = repmat(sqrt(gamma)',N,1);
    PhiG = Phi.*G;
    [U,S,V] = svd(PhiG,'econ');

    [d1,d2] = size(S);
    if (d1 > 1)     diag_S = diag(S);
    else            diag_S = S(1);      end;

    U_scaled = U(:,1:min(N,m)).*repmat((diag_S./(diag_S.^2 + lambda + 1e-16))',N,1);
    Xi = G'.*(V*U_scaled');

    mu = Xi*T;

    % *** Update hyperparameters ***
    gamma_old = gamma;
    mu2_bar = sum(mu.^2,2);

    if (flag1(1) == 0)
        % MacKay fixed-point SBL
        R_diag = real( (sum(Xi.'.*Phi)).' );
        gamma = mu2_bar./(L*R_diag);

    elseif (flag1(1) == 1)
        % Fast EM SBL
        R_diag = real( (sum(Xi.'.*Phi)).' );
        gamma = sqrt( gamma.*real(mu2_bar./(L*R_diag)) );

    elseif (flag1(1) == 2)
        % Traditional EM SBL
        PhiGsqr = PhiG.*G;
        Sigma_w_diag = real( gamma - ( sum(Xi.'.*PhiGsqr) ).' );
        gamma = mu2_bar/L + Sigma_w_diag;

    else
        % FOCUSS
        p = flag1(2);
        gamma = (mu2_bar/L).^(1-p/2);
    end;

    % *** Check stopping conditions, etc. ***
    if (DISPLAY_FLAG) disp(['iters: ',num2str(k),'   num coeffs: ',num2str(m), ...
            '   gamma change: ',num2str(max(abs(gamma - gamma_old)))]); end;
end;

% *** Expand weights, hyperparameters ***
temp = zeros(M,1);
if (m > 0) temp(keep_list,1) = gamma;  end;
gamma = temp;

temp = zeros(M,L);
if (m > 0) temp(keep_list,:) = mu;  end;
mu = temp;

return;


function U = initializeSubspace(inputFolder, imgDir, param)
    % initial lize the  subspace by svd

    trainSize = min(length(imgDir), param.trainSize);
    sampleSize = min(param.sampleSize, trainSize);

    randIndex =   randperm(trainSize);

    imageIndex =  randIndex(1:sampleSize);

    %imageIndex =  randperm(trainSize, sampleSize);

    U0=[];
    for i=1:sampleSize
        index = imageIndex(i);
        imgPath = [inputFolder, imgDir(index).name];
        im = imread(imgPath);
        im = im(:);
        U0 = [U0 im];
    end

    [U, D] = svds(double(U0), param.rank);
end


function threshold = getAdaptiveThreshold(prob_map, obj_coords, cfg)
%GETADAPTIVETHRESHOLD Returns the threshold to separate foreground from
%background/surroundings based on cumulative histograms
% Parameters:
%   prob_map   [MxNx1] probability map
%   obj_coords Object rectangle defined as a 4 element vector: [x,y,w,h]
%   cfg        DAT configuration

% Object region
obj_prob_map = imcrop(prob_map, obj_coords);
H_obj =  hist(obj_prob_map(:), cfg.adapt_thresh_prob_bins);
H_obj = H_obj./sum(H_obj);
cum_H_obj = cumsum(H_obj);

% Surroundings
H_dist = hist(prob_map(:), cfg.adapt_thresh_prob_bins);
% Remove object information
H_dist = H_dist - H_obj;
H_dist = H_dist./sum(H_dist);
cum_H_dist = cumsum(H_dist);

k = zeros(size(cum_H_obj));
for i = 1:length(k)-1, k(i) = cum_H_obj(i+1) - cum_H_obj(i); end
x = abs(cum_H_obj - (1 - cum_H_dist)) + (cum_H_obj < 1 - cum_H_dist) + (1 - k);
[~,i] = min(x);
%Final threshold result should lie between 0.4 and 0.7 to be not too restrictive
threshold = max(.4,min(.7, cfg.adapt_thresh_prob_bins(i)));


function x = spm_normrnd(m, C, N)
% Random samples from Gaussian distribution
% FORMAT x = spm_normrnd(m, C, N)
% m        - [d x 1] mean
% C        - [d x d] covariance or cell array {dC, vC} so that
%            [vC, diag(dC)] = eig(C)
% N        - number of samples
%
% x        - [d x N] matrix of samples
%__________________________________________________________________________
% Copyright (C) 2009 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_normrnd.m 3603 2009-11-30 18:56:50Z guillaume $

if iscell(C)
    deig = C{1};
    evec = C{2};
else
    [evec, eval] = eig(C);
    deig         = diag(eval);
end

i = (abs(imag(deig))>0) | (deig<0);
if any(i)
  %warning('Covariance matrix is not positive semi-definite: redefined');
  deig(i)   = [];
  evec(:,i) = [];
end

proj = randn(N,length(deig)) * diag(sqrt(deig));
x    = repmat(m(:),1,N) + evec*proj';


% change in angle to closest point on wall in fly's coordinate system
function [data,units] = compute_dangle2wall(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);
  % set sign so that negative means going toward 0, positive means going
  % away from 0
  if trx(fly).nframes <= 1,
    data{i} = [];
  else
    data{i} = sign(trx(fly).angle2wall(1:end-1)).*...
      modrange(diff(trx(fly).angle2wall,1,2),-pi,pi)./trx(fly).dt;
  end
end
units = parseunits('rad/s');


function lmi = lmi_input(lmi, umax, phi)
%LMI_INPUT

% reference:
%	K. Tanaka, H. O. Wang
%	Fuzzy Control Systems Design and Analysis, (2002)
%	page 66,69 (theorem 11,13)

% TODO: known x(0), output constraint

R = size(lmi.A, 1);
X = lmi.X;
M = lmi.M;

% constraints on the control value

% phi^2 I < X
lmi.F = lmi.F + set(phi^2 * eye(lmi.n) < X, 'phi^2 I < X');

% [X, Mr'; Mr, mu^2 I] > 0
for r = 1:R
	lmi.F = lmi.F + set([X M{r}'; M{r} umax^2*eye(lmi.m)] > 0, sprintf('type3 lmi %d', r));
end


function [dat, W, A, r_values] = proc_cspoc(dat, maxmin_flag, varargin)
% PROC_CSPOC - Canonical Source Power Co-modulation Analysis (cSPoC)
%
% Optimizes spatial filters such that the power of the filtered signals are
% maximally positively (or negatively) correlated. The function can be used
% in two ways:
% (1) The data of a single subject are bandpass filtered in multiple
% frequency bands, using the function proc_filterbank
% (2) The data of multiple subjects are bandpass filtered in a single
% frequency band
%
%Synopsis:
% [DAT, SPOC_W, SPOC_A, R_VALUES]= proc_cspoc(DAT, MINMAX_FLAG, <OPT>)
%
%Arguments:
% DAT         - either a data structure of epoched data of one subject,
%               containing multiband filtered channels (obtained via
%               proc_filterbank), or a cell array, each cell containing a
%               data structure of epoched data that has been multiband
%               filtered
% MINMAX_FLAG - either 1 for maximizing or -1 for minimizing correlation
% OPT         - struct or property/value list of optional properties:
%  .nComponentPairs   - either the string 'all' or an integer, determining
%                       the number of components pairs to be returned,
%                       default: 'all'
%  .nRepeats          - number of re-starts per component pair, default: 10
%  .maxIter           - maximum number of optimizer iterations, default: 200
%  .averageOverEpochs - when optimizing the correlations, average the
%                       source envelopes within epochs, default: false
%
%Returns:
% DAT    - updated data structure
% SPOC_W  - SPOC projection matrix (spatial filters, in the columns)
% SPOC_A  - estimated mixing matrix (activation patterns, in the columns)
% LAMBDA - eigenvalue score of SPOC projections

props= {'nComponentPairs'       'all'       'STRING|DOUBLE[1]'
        'nRepeats'              10          'DOUBLE[1]'
        'averageOverEpochs'     0           'BOOLE'
        'maxIter'               200         'DOUBLE[1]'
       };

if nargin==0,
  dat = props; return
end

dat = misc_history(dat);
misc_checkType(dat, 'STRUCT(x clab y)|CELL');

opt = opt_proplistToStruct(varargin{:});
opt = opt_setDefaults(opt, props);
opt_checkProplist(opt, props);

%% contruct data cells
% cSPoC is performed on one multi bandpass filtered data set
if not(iscell(dat))
    % check that all clabs contain the string 'flt'
    if any(cellfun(@(x) isempty(x),cellfun(@(x) strfind(x,'flt'),dat.clab,'UniformOutput',0)))
        error('data are not band-pass filtered by proc_filterbank')
    end
    N = length(unique(str2mat(cellfun(@(x) x(end),dat.clab)')));
    X = cell(1,N);
    for ii = 1:N
        dat2 = proc_selectChannels(dat,sprintf('*flt%d',ii));
        X{ii} = dat2.x;
    end
% cSPoC is performed on multiple single bandpass filtered data sets
else
    N = length(dat);
    X = cell(1,N);
    for ii = 1:N
        X{ii} = dat{ii}.x;
    end
end

%% run cSPoC
opt = renameStructField(opt,'nComponentPairs','n_component_sets');
opt = renameStructField(opt,'nRepeats','n_repeats');
opt = renameStructField(opt,'averageOverEpochs','average_over_epochs');

[W,A,r_values] = cspoc(X,maxmin_flag,opt);

%% project the data onto cSPoC filters
datnew = [];
for ii = 1:N
    if not(iscell(dat))
        dat2 = proc_selectChannels(dat,sprintf('*flt%d',ii));
    else
        dat2 = dat{ii};
    end
    dat2 = proc_linearDerivation(dat2,W{ii},'prependix',sprintf('cspoc%d_',ii));
    datnew = proc_appendChannels(datnew,dat2);
end

dat = datnew;


function [ fea, out ] = ex_spanner( varargin )
%EX_SPANNER STL geometry import and stress calculation of a spanner.
%
%   [ FEA, OUT ] = EX_SPANNER( VARARGIN ) Example to import a STL CAD
%   geometry and calculate displacements on a fixed spanner. The load
%   force may be distributed in the tangential load direction with the
%   force fraction parameter FRAC.
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       E           scalar {190e3}         Modulus of elasticity [N/mm^2]
%       nu          scalar {0.29}          Poissons ratio
%       force       scalar {1000}          Load force [N]
%       frac        scalar {0}             Fraction of stress against pulling direction
%       sfun        string {sflag1}        Shape function for displacements
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { 'E',        190e3;
            'nu',       0.29;
            'force',    1000;
            'frac',     0;
            'sfun',     'sflag1';
            'iplot',    1;
            'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});

% Define scaling factor m to mm.
USE_METERS = true;
s = double( ~USE_METERS + USE_METERS*1e-3 );

% Import (and scale) grid.
fea.sdim = {'x','y','z'};
fea.geom = impexp_stl( 'spanner1.stl', 'import', [], 'solid', 2, 'extend', 0 );
fea.grid = gridgen( fea, 'hmax', 5, 'gridgen', 'robust', 'fid', opt.fid );
fea.grid.p = fea.grid.p*s;

% Add linear elasticity physics mode and define material parameters.
fea = addphys( fea, @linearelasticity );
fea.phys.el.sfun            = { opt.sfun opt.sfun opt.sfun };
fea.phys.el.eqn.coef{1,end} = { opt.nu };
fea.phys.el.eqn.coef{2,end} = { opt.E/s^2 };

% Set all boundaries to no load per default.
n_bdr  = max(fea.grid.b(3,:));
bc_sel = cell(3,n_bdr);
[bc_sel{:}] = deal(0);

% Fix all displacements on mandible boundaries.
i_fix   = [13 14];
i_force = [3 1];
faxis   = [1 3];
[bc_sel{:,i_fix}] = deal(1);
fea.phys.el.bdr.coef{5} = bc_sel;

% Apply force for x > 140 mm.
force = opt.force/(6*s*80*s);
fea.phys.el.bdr.coef{7}{faxis(1),i_force(1)} = ['-',num2str((1-opt.frac)*force),'*(y>140*',num2str(s),')'];
fea.phys.el.bdr.coef{7}{faxis(2),i_force(2)} = [num2str(opt.frac*force),'*(y>140*',num2str(s),')'];

% Parse and solve problem.
fea = parsephys(fea);
fea = parseprob(fea);
fea.sol.u = solvestat( fea, 'fid', opt.fid, 'icub', 1+str2num(strrep(opt.sfun,'sflag','')) );

% Postprocessing.
if( opt.iplot>0 )
  subplot(1,2,1)
  postplot( fea, 'surfexpr', ['sqrt(u^2+v^2+w^2)/',num2str(s)] )
  view([30 20])
  title('Total displacement (mm)')

  subplot(1,2,2)
  DSCALE = 5;
  dp = zeros(size(fea.grid.p));
  for i=1:3
    dp(i,:) = DSCALE*evalexpr( fea.dvar{i}, fea.grid.p, fea );
  end
  fea_disp.grid   = fea.grid;
  fea_disp.grid.p = fea_disp.grid.p + dp;
  plotgrid( fea_disp )
  title(['Displacement plot'])
  view([30 20])
end

% Error checking.
u = fea.sol.u(unique(fea.eqn.dofm{1}(:)))/s;
v = fea.sol.u(unique(fea.eqn.ndof(1)+fea.eqn.dofm{2}(:)))/s;
w = fea.sol.u(unique(sum(fea.eqn.ndof(1:2))+fea.eqn.dofm{3}(:)))/s;
out.disp = sqrt( u.^2 + v.^2 + w.^2 );
out.pass = nan;
if( ~(got.frac || got.E || got.nu || got.force) )
  out.pass = abs( max(out.disp) - 2.5 )/2.5 < 0.1;
end

if( nargout==0 )
  clear fea out
else
  fea.grid.p = fea.grid.p/s;
end


% FORMAT:
%
% sampval = time2sample(timeop, time, fs, rounding, offset)
%
% INPUTS:
%
% timeop      - second or millisecond; 0 or 1
% timeval      - time value
% fs          - sample rate
%
% Optional inputs:
%
% rounding    - 1 means round the result {default}, 0 means do not round
% offset      - offset time in samples (you can use time2sample recursively here). Default 0
%
% OUTPUT:
%
% sampval     - time in samples
%
%
% Author: Javier Lopez-Calderon
% Center for Mind and Brain
% University of California, Davis,
% Davis, CA
% 2013

function sampval = time2sample(timeop, timeval, fs, rounding, offset)
if nargin<1
        help time2sample
        return
end
if nargin<5
        offset = 0;
end
if nargin<4
        rounding = 1;
end
if nargin<3
        fs = 1;
end
if nargin<2
        error('Two inputs are requiered at least.')
end
if timeop==0 % sec
    kktime=1;
elseif timeop==1 % msec
    kktime=1000;
else
    error('unknow timeop input.')
end
sampval = offset + timeval*fs/kktime;
if rounding
        sampval = round(sampval);
end


%% Simulation Parameters
name        ='SBBGK1d'; % Simulation Name
CFL         = 0.05;     % CFL condition
r_time      = 1/1000;  % Relaxation time
tEnd        = 0.1;      % End time
theta       = 0;        % {-1} BE, {0} MB, {1} FD.
quad        = 2;        % for NC = 1 , GH = 2
method      = 1;        % for TVD = 1, WENO3 = 2, WENO5 = 3
IC_case     = 1;        % IC: {1}Sod's, {2}LE, {3}RE, {4}DS, {5}SS, {6}Cavitation
plot_figs   = 1;        % 0: no, 1: yes please!
write_ans   = 1;        % 0: no, 1: yes please!
RRR=8.314;
% Using DG
P_deg       = 0;        % Polinomial Degree
Pp          = P_deg+1;  % Polinomials Points
% Using RK integration time step
RK_stages   = 4;        % Number of RK stages

gamma = 2.5; %1.4;        % Ratio of specific heats
 etpfix   = 0.90;	% {#} Harten's sonic entropy fix value, {0} no entropy fix
[MB_r,u,MB_p] = Euler_IC1d(x,1);
%% Microscopic Velocity Discretization (using Discrete Ordinate Method)
% that is to make coincide discrete values of microscopic velocities with
% values as the value points for using a quadrature method, so that we can
% integrate the velocity probability distribution to recover our
% macroscopics properties.
switch quad

    case{1} % Newton Cotes Quadrature:
    V  = [-20,20];  % range: a to b
    nv = 200;       % nodes desired (may not the actual value)
    [v,w,k] = cotes_xw(V(1),V(2),nv,5); % Using Netwon Cotes Degree 5

    case{2} % Gauss Hermite Quadrature:
    nv = 60;          % nodes desired (the actual value)
    [v,w] = GaussHermite(nv); % for integrating range: -inf to inf
    k = 1;            % quadrature constant.
    w = w.*exp(v.^2); % weighting function of the Gauss-Hermite quadrature

    otherwise
        error('Order must be between 1 and 2');
end

%% Velocity-Space Grid:
% The actual nv value will be computed using 'lenght' vector function:
nv = length(v);
% Using D.O.M.
    v = repmat(v,1,nx);     w = repmat(w,1,nx);
	p = zeros(nv,nx);
    %% Load Macroscopic Velocity, Temperature and Fugacity
    [rho_0,u0,t0] = BGK_IC1d(x,IC_case);
 rho_0 = repmat(rho_0,nv,1);                       ux = repmat(u0,nv,1);       t = repmat(t0,nv,1);
    f0 = f_equilibrium_1d(rho_0,ux,v,t,RRR);

    %% Marching Scheme
% First we need to define how big is our time step. Due to the discrete
% ordinate method the problem is similar to evolve the same problem for
% every mesoscopic velocity.
dt = dx*CFL/max(v(:,1));
dtdx = dt/dx;  % precomputed to save someflops

% Time domain discretization
time = 0:dt:tEnd;


function spm_MDP_urn
% Demo for active inference with the urn task
%__________________________________________________________________________
%
% This demonstration uses the Urn or Beads Task to illustrate how choice
% behaviour can be simulated using active inference - in the context of
% Markov decision processes. In the urn task, a succession of draws
% from one of two urns are made and the agent has to decide whether the
% balls are being drawn from an urn with predominantly red or green balls.
% We model this in terms of a state-space with four dimensions: number of
% balls drawn (n),  number of green balls drawn (k), choice (undecided, red
% or green)and the true (hidden) state of the urn (red or green). With
% this relatively simple state-space, the utility of any hidden state is
% simply quantified by the log-odds ratio of making a correct
% decision. From binomial theory this is (2k - n)*log(p/(1 - p)), where p
% is the proportion of red or green balls. Having defined the utility
% function of states, we can then use the MDP formulation of active
% inference using variational Bayes to simulate choice behaviour.
%
% This routine first provides an illustration of a game in which a decision
% is delayed until the last draw to look at inferences during successive
% draws - with a special focus on precision. The illustration here shows
% a decrease in precision when an unexpected (green ball) is drawn during a
% sequence of red balls.
%
% We then characterise changes in choice probability (and latency to the
% decision) in terms of its dependency on threshold criteria (on the odds
% ratio) and hyperpriors about precision (alpha or the scale parameter of a
% standard gamma distribution). The routine concludes with an illustration
% of how to estimate model parameters using the likelihood of observed
% (simulated) choices.
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_MDP_urn.m 6592 2015-11-06 16:20:48Z guillaume $

% set up and preliminaries
%==========================================================================
rng('default')

T     = 8;                         % number of trials in each game
Pa    = .85;                       % proportion of red or green goals

% hidden (and initial) states (a red urn with no drawers)
%--------------------------------------------------------------------------
D          = zeros(T,T,3,2);       % #balls - 1 x #green - 1 x #u x #urns
D(1,1,1,1) = 1;
S          = spm_vec(D);
D(1,1,1,2) = 1;
D          = spm_vec(D);

% likelihood - everything is seen  apart from the hidden states of the urn
%--------------------------------------------------------------------------
A     = kron([1 1],eye(3*T*T));

% transition probabilities (B{1} - wait; B{2} - red; B{3} - green)
%--------------------------------------------------------------------------
for i = 1:6
    Bn{i,i} = kron(spm_speye(T,T),spm_speye(T,T));
end
Br      = spm_speye(T,T,0)*Pa + spm_speye(T,T,-1)*(1 - Pa);
Bg      = spm_speye(T,T,0)*(1 - Pa) + spm_speye(T,T,-1)*Pa;
Bn{1,1} = kron(Br,spm_speye(T,T,-1));
Bn{4,4} = kron(Bg,spm_speye(T,T,-1));

B{1}    = spm_cat(Bn);
B{2}    = kron(eye(2),kron([0 0 0;1 1 0;0 0 1],eye(T*T)));
B{3}    = kron(eye(2),kron([0 0 0;0 1 0;1 0 1],eye(T*T)));

% priors over final state (exp(utility))
%--------------------------------------------------------------------------
L     = zeros(T,T,3,2);
W     = zeros(T,T,3,2);
rho   = log(Pa/(1 - Pa));
for n = 0:(T - 1)
    W(n + 1,:,:,:) = - n/8;
    for k = 0:n
        L(n + 1,k + 1,2,1) = (n - 2*k)*rho;
        L(n + 1,k + 1,3,2) = (2*k - n)*rho;
    end
end
C     = spm_softmax(spm_vec((L > 3) + W)*4);

% allowable policies (one decision before the game ends)
%--------------------------------------------------------------------------
V     = [ones(T,T) + eye(T,T), ones(T,T) + 2*eye(T,T)];

% MDP Structure
%==========================================================================
MDP.T = T;                          % process depth (the horizon)
MDP.S = S;                          % initial state
MDP.A = A;                          % likelihood
MDP.B = B;                          % transition probabilities (priors)
MDP.C = C;                          % terminal cost probabilities (priors)
MDP.D = D;                          % prior over initial states
MDP.V = V;                          % allowable policies

% Solve - an example game (with high offer at t = 10)
%==========================================================================
spm_figure('GetWin','Figure 1'); clf

% create a sequence of draws (outcomes) - with and oddball on the 4th
%--------------------------------------------------------------------------
k     = cumsum([0 0 0 1 0 0 0 0]);
a     = [1 1 1 1 1 1 2];
o     = zeros(1,length(a) - 1);
for n = 1:length(o)
    o(n) = sub2ind([T,T,3],n,k(n) + 1,1);
end

% Active inference (precluding a decision until the last trial)
%--------------------------------------------------------------------------
MDP.o    = o;
MDP.a    = a;
MDP.plot = gcf;
MDP.N    = 8;
MDP      = spm_MDP_game(MDP);

% plot convergence and precision
%--------------------------------------------------------------------------
subplot(4,2,7)
plot(MDP.d)
xlabel('Latency (updates)','FontSize',12)
ylabel('Precision of beliefs','FontSize',12)
title('Expected precision','FontSize',16)
spm_axis tight

% deconvolve to simulate dopamine responses
%--------------------------------------------------------------------------
nd  = length(MDP.d);
K   = tril(toeplitz(exp(-((1:nd) - 1)'/8)));

subplot(4,2,8)
plot(pinv(K)*MDP.d(:)), hold on
xlabel('Latency (updates)','FontSize',12)
ylabel('Precision of beliefs','FontSize',12)
title('Simulated dopamine responses','FontSize',16)
spm_axis tight

% Illustrate dependency on parameters
%==========================================================================
spm_figure('GetWin','Figure 2'); clf

% create a sequence of draws (outcomes) - no oddballs
%--------------------------------------------------------------------------
k     = cumsum([0 0 0 0 0 0 0 0]);
o     = zeros(1,length(k));
for n = 1:length(k)
    o(n) = sub2ind([T,T,3],n,k(n) + 1,1);
end

% probability distribution over time: P(1,:) is no action
%--------------------------------------------------------------------------
PrT      = @(P) [1 cumprod(P(1,1:end - 1))].*(1 - P(1,:));
MDP.plot = 0;                        % plot convergence
MDP.N    = 4;                        % number of variational iterations
MDP.a    = ones(1,T);                % and action
MDP.o    = o;                        % and outcomes

% beliefs about final state - decision threshold (log likelihood)
%--------------------------------------------------------------------------
DP    = MDP;
PF    = [];
DF    = [];
p     = linspace(0,8,8);
for i = 1:length(p)
    DP.C    = spm_softmax(spm_vec((L > p(i)) + W)*4);
    DP      = spm_MDP_game(DP);
    PF(i,:) = 1 - DP.P(1,:);
    DF(i,:) = PrT(DP.P);
end

% probability of accepting
%--------------------------------------------------------------------------
subplot(2,2,1)
plot(PF')
xlabel('Latency (trials)','FontSize',12)
ylabel('Probability of deciding','FontSize',12)
title('Increasing decision threshold','FontSize',16)
axis square xy

% distribution of acceptance latencies
%--------------------------------------------------------------------------
subplot(2,2,2)
plot(DF')
xlabel('Latency (trials)','FontSize',12)
ylabel('Latency of decision','FontSize',12)
title('Latency of decision','FontSize',16)
axis square xy

% Hyperpriors - prior precision (alpha)
%--------------------------------------------------------------------------
DP    = MDP;
PF    = [];
DF    = [];
p     = linspace(2,16,8);
for i = 1:length(p)
    DP.alpha  = p(i);
    DP      = spm_MDP_game(DP);
    PF(i,:) = 1 - DP.P(1,:);
    DF(i,:) = PrT(DP.P);
end

% probability of accepting
%--------------------------------------------------------------------------
subplot(2,2,3)
plot(PF')
xlabel('Latency (trials)','FontSize',12)
ylabel('Probability of deciding','FontSize',12)
title('Increasing prior precision','FontSize',16)
axis square xy

% distribution of acceptance latencies
%--------------------------------------------------------------------------
subplot(2,2,4)
plot(DF')
xlabel('Latency (trials)','FontSize',12)
ylabel('Latency of decision','FontSize',12)
title('Latency of decision','FontSize',16)
axis square xy

% Simulate multiple trials and try to infer prior precision (alpha)
%==========================================================================
spm_figure('GetWin','Figure 3'); clf

% Simulate multiple trials and record likelihood
%--------------------------------------------------------------------------
MDP.plot = 0;
MDP.N    = 4;

p     = linspace(2,16,8);
for t = 1:8

    % run game
    %----------------------------------------------------------------------
    MDP.s = [];
    MDP.o = [];
    MDP.a = [];
    MDP   = spm_MDP_game(MDP);

    % place outcomes in DP
    %----------------------------------------------------------------------
    DP    = MDP;
    [a j] = find(MDP.U);
    [o j] = find(MDP.O);
    DP.o  = o;
    DP.a  = a;
    y(t)  = find(a > 1);

    % get log-likelihood for different parameter values
    %----------------------------------------------------------------------
    for i = 1:length(p);
        DP.alpha = p(i);
        DP       = spm_MDP_game(DP);
        LL(i,t)  = sum(log(DP.P(find(DP.U))));
    end

end

% approximate the MAP with the ML and use the Laplace assumption
%--------------------------------------------------------------------------
Lp    = sum(LL,2);
dp    = mean(diff(p,1));
dLdpp = diff(Lp,2)/(dp^2);
[l i] = max(Lp(2:end - 1) + (dLdpp < 0)*1024);
Cp    = inv(-dLdpp(i));
Ep    = p(i + 1);

% plot responses
%--------------------------------------------------------------------------
subplot(2,2,1)
hist(y,1:T)
xlabel('Latency','FontSize',12)
ylabel('Probability','FontSize',12)
title('Distribution of responses','FontSize',16)
axis square

% plot log likelihood over trials
%--------------------------------------------------------------------------
subplot(2,2,2)
plot(p,LL - min(LL(:)))
xlabel('Parameter','FontSize',12)
ylabel('Probability','FontSize',12)
title('Log-likelihood over games','FontSize',16)
axis square

% plot likelihood
%--------------------------------------------------------------------------
subplot(2,2,3)
plot(p,Lp - min(Lp))
xlabel('Parameter','FontSize',12)
ylabel('Log-likelihood','FontSize',12)
title('Log-likelihood','FontSize',16)
axis square

% plot posterior
%--------------------------------------------------------------------------
subplot(2,2,4)
tp  = 8;
pp  = linspace(p(1),p(end),64);
plot(pp,spm_Npdf(pp,Ep,Cp)), hold on
plot([tp tp],[0 1/8],':'),   hold off
xlabel('Parameter','FontSize',12)
ylabel('Probability','FontSize',12)
title('Posterior probability','FontSize',16)
axis square

return


function [ux,ispos] = psdfactor(x,K)
% [ux,ispos] = psdfactor(x,K)
%
% PSDFACTOR  UX'*UX Cholesky factorization
%
% **********  INTERNAL FUNCTION OF SEDUMI **********
%
% See also sedumi

% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA
%
% disp('The SeDuMi binaries are not installed.')
% disp('In Matlab, launch "install_sedumi" in the folder you put the SeDuMi files.')
% disp('For more information see the file Install.txt.')
% error(' ')

Ks = K.s;
if isempty(Ks),
    ux = zeros(0,1);
    ispos = true;
    return
end
Kq = Ks .* Ks;
nr = K.rsdpN;
nc = length(Ks);
N  = sum(Kq) + sum(Kq(nr+1:end));
ux = zeros(N,1);
xi = length(x) - N;
ui = 0;
for i = 1 : nc,
    ki = Ks(i);
    qi = Kq(i);
    XX = x(xi+1:xi+qi);
    xi = xi + qi;
    if i > nr,
        XX = XX + 1j*x(xi+1:xi+qi);
        xi = xi + qi;
    end
    XX = reshape(XX,ki,ki);
    [XX,flag]=chol(XX,'lower');
    if flag,
        ispos = false;
        return
    end
    XX = XX + tril(XX,-1)';
    ux(ui+1:ui+qi) = real(XX);
    ui = ui + qi;
    if i > nr,
        ux(ui+1:ui+qi) = imag(XX);
        ui = ui + qi;
    end
end
ispos = true;


function bss_eval_test(T,N,W,L)
% check the behaviour of some basic functions of the toolbox BSS_EVAL
%
%
% Usage: bss_eval_test(T,N,L)
%
% Input:
%   - T: number of samples of the test signals (default 1000)
%   - N: number of sources in the test signals (default 2)
%   - W: hop size = half-size of the window (default ceil(T/20));
%   - L: number of taps of the filter (default 0)
%
% Ouput: None
%
% Developers:  - Cedric Fevotte (fevotte@tsi.enst.fr) - Emmanuel Vincent
% (emmanuel.vincent@irisa.fr) - Remi Gribonval (remi.gribonval@irisa.fr)

if nargin<1
    T = 1000;
end
if nargin<2
    N=2;
end
if nargin<3
    W=ceil(T/20);
end
if nargin<4
    L=0;
end

% 1. Generate some data
S  = rand(N,T);

% 2. check that the SDR,SIR and SAR are almost infinite when performing a
% tvfilt decomposition with half overlapping rectangular windows
win = ones(1,W*2);
[starget,einterf,eartif]=bss_decomp_tvfilt(S(1,:),1,S,win,W,L);
[SDR,SIR,SAR]=bss_crit(starget,einterf,eartif);
disp(['SDR: ' num2str(SDR) ' SIR: ' num2str(SIR) 'SAR: ' num2str(SAR)]);
disp('Results for rectangular windows are expected to exceed 200 dB');

% 3. check that the SDR,SIR and SAR are almost infinite when performing a
% tvfilt decomposition with half overlapping triangular windows
win = triang(W*2)';
[starget,einterf,eartif]=bss_decomp_tvfilt(S(1,:),1,S,win,W,L);
[SDR,SIR,SAR]=bss_crit(starget,einterf,eartif);
disp(['SDR: ' num2str(SDR) ' SIR: ' num2str(SIR) 'SAR: ' num2str(SAR)]);
disp('Results for triangular windows are expected to exceed 200 dB');

%plot(starget,'r')
% hold on;
% triangsum = zeros(N*100+length(win),1);
% for i=1:100
%     idxrange = (i-1)*N+(1:length(win));
%     triangsum(idxrange) = triangsum(idxrange)+win';
% end
% plot(triangsum);

function w = triang(n)
% TRIANG Triangular window.
if rem(n,2)
% It's an odd length sequence
w = 2*(1:(n+1)/2)/(n+1);
w = [w w((n-1)/2:-1:1)]';
else
% It's even
w = (2*(1:(n+1)/2)-1)/n;
w = [w w(n/2:-1:1)]';
end


classdef TP3 < PROBLEM
% <multi> <real> <large/none> <robust>
% Test problem for robust multi-objective optimization
% delta --- 0.05 --- Maximum disturbance degree
% H     ---   50 --- Number of disturbances

%------------------------------- Reference --------------------------------
% A. Gaspar-Cunha, J. Ferreira, and G. Recio, Evolutionary robustness
% analysis for multi-objective optimization: benchmark problems, Structural
% and Multidisciplinary Optimization, 2014, 49: 771-793.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        delta;      % Maximum disturbance degree
        H;          % Number of disturbances
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            [obj.delta,obj.H] = obj.ParameterSet(0.05,50);
            obj.M = 2;
            if isempty(obj.D); obj.D = 10; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            PopObj(:,1) = 1 - PopDec(:,1).^2;
            g = 1 + 10*mean(PopDec(:,2:end),2);
            PopObj(:,2) = sin(pi/2*PopDec(:,1)).*g;
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(~,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = sin(pi/2*sqrt(1-R(:,1)));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            switch metName
                case {'Mean_IGD','Mean_HV','Worst_IGD','Worst_HV'}
                    score = feval(metName,Population,obj);
                otherwise
                    score = feval(metName,Population,obj.optimum);
            end
        end
        %% Perturb solutions multiple times
        function PopX = Perturb(obj,PopDec,N)
            if nargin < 3; N = obj.H; end
            Delta = repmat(obj.delta.*(obj.upper-obj.lower),N*size(PopDec,1),1);
            w     = UniformPoint(N,obj.D,'Latin');
            Dec   = 2*Delta.*w(reshape(repmat(1:end,size(PopDec,1),1),1,[]),:) + repmat(PopDec,N,1) - Delta;
            Dec   = obj.CalDec(Dec);
            PopX  = SOLUTION(Dec,obj.CalObj(Dec),obj.CalCon(Dec));
        end
    end
end


function p10_story ( )

%*****************************************************************************80
%
%% P10_STORY prints the "story" for problem 10.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    None
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  100 uniformly random X values between -2 and 5 were selected, and\n' );
  fprintf ( 1, '  the formula Y = 2 + 5 * X + 10 * N(0,1) was evaluated, where N(0,1)\n' );
  fprintf ( 1, '  represents random normal values with 0 mean and unit variance.\n' );

  return
end


function linpack_d_test20 ( )

%*****************************************************************************80
%
%% TEST20 tests DPPFA and DPPDI.
%
%  Discussion:
%
%    DPPFA factors a packed positive definite symmetric matrix,
%    and DPPDI can compute the determinant or the inverse.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST20\n' );
  fprintf ( 1, '  For a positive definite symmetric packed matrix,\n' );
  fprintf ( 1, '  DPPFA factors the matrix.\n' );
  fprintf ( 1, '  DPPDI computes the inverse or determinant.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Set the matrix A.
%
  k = 0;
  for j = 1 : n
    for i = 1 : j
      k = k + 1;
      if ( i == j - 1 )
        a(k) = -1.0;
      elseif ( i == j )
        a(k) = 2.0;
      else
        a(k) = 0.0;
      end
    end
  end
%
%  Factor the matrix.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Factor the matrix.\n' );

  [ a, info ] = dppfa ( a, n );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Error, DPPFA returns INFO = %d\n', info );
    return
  end
%
%  Invert the matrix.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Get the determinant and inverse.\n' );

  job = 11;
  [ a, det ] = dppdi ( a, n, job );
%
%  Print the results.
%
  fprintf ( 1, '\n' );
  fprintf ( 1,'  Determinant  = %f * 10 ^ %f\n', det(1), det(2) );
%
%  DPPDI produces only the 'upper half triangle' of the inverse,
%  which is actually symmetric.  Thus, the lower half could be
%  produced by copying from the upper half.  However, the first row
%  of A, as returned, is exactly the first row of the inverse.
%
  k = 0;
  for j = 1 : n
    for i = 1 : j
      k = k + 1;
      b(i,j) = a(k);
      b(j,i) = a(k);
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Inverse:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  %14f', b(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function varargout = voxel(i,d,c,alpha)
% VOXEL Draw a 3-D voxel.
%    VOXEL(S) Draws a voxel centered at the specified point S. S is a
%       coordinate point in the form [ x y z ].
%    VOXEL(S,EDGE) Draws a voxel using the specified EDGE size. If no
%       EDGE is provided, it is set by default to 1. EDGE is a three
%       element vector [dx,dy,dz].
%    VOXEL(S,EDGE,C) Uses the specified colour C to draw the faces of the
%       voxel. C is a character string to specify color (type 'help plot'
%       to see list of valid colors). If no C is provided, it is set by
%       default to blue.
%    VOXEL(S,EDGE,C,ALPHA) Uses the given ALPHA to define the transparency
%       level (1 for opaque, 0 for transparent). If no ALPHA is given, it
%       is set to 1.
%    H = VOXEL(...) Return a vector of handles for the voxel drawn.
%
%
% See also PLOT, PATCH

%   Suresh Joel Apr 15,2003
%           Updated Feb 25, 2004

switch(nargin),
case 0
    disp('Too few arguements for voxel');
    return;
case 1
    d=[ 1 1 1 ]; %default length of side of voxel is 1
    c='b';       %default color of voxel is blue
    alpha=1;
case 2
    c='b';
    alpha=1;
case 3
    alpha=1;
end;

x=[i(1)+[0 0 0 0 d(1) d(1) d(1) d(1)]; ...
        i(2)+[0 0 d(2) d(2) 0 0 d(2) d(2)]; ...
        i(3)+[0 d(3) 0 d(3) 0 d(3) 0 d(3)]]';
h = [];
for n=1:3,
    if n==3,
        x=sortrows(x,[n,1]);
    else
        x=sortrows(x,[n n+1]);
    end;
    temp=x(3,:);
    x(3,:)=x(4,:);
    x(4,:)=temp;
    h1=patch(x(1:4,1),x(1:4,2),x(1:4,3),c);
    set(h1,'FaceAlpha',alpha);
    h = vertcat(h1,h);
    temp=x(7,:);
    x(7,:)=x(8,:);
    x(8,:)=temp;
    h1=patch(x(5:8,1),x(5:8,2),x(5:8,3),c);
    set(h1,'FaceAlpha',alpha);
    h = vertcat(h1,h);
end;

if nargout>0
  varargout{1} = h;
end


function calpak_test74 ( )

%*****************************************************************************80
%
%% CALPAK_TEST74 tests YMDF_INC_JULIAN, YMDF_NEXT_JULIAN, YMDF_PREV_JULIAN.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2013
%
%  Author:
%
%    John Burkardt
%
  days = 10.25;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALPAK_TEST74\n' );
  fprintf ( 1, '  For the Julian calendar:\n' );
  fprintf ( 1, '  YMDF_INC_JULIAN increments a date by days;\n' );
  fprintf ( 1, '  YMDF_NEXT_JULIAN computes the next day,\n' );
  fprintf ( 1, '  YMDF_PREV_JULIAN computes the previous day.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  YMDF date    Tomorrow	    Yesterday	   +10.25 days\n' );
  fprintf ( 1, '\n' );

  i = 0;

  while ( 1 )

    i = i + 1;
    jed = jed_test ( i );

    if ( jed < 0 )
      break
    end

    [ y1, m1, d1, f1 ] = jed_to_ymdf_julian ( jed );
    s1 = ymdf_to_s_julian ( y1, m1, d1, f1 );

    [ y2, m2, d2, f2 ] = ymdf_next_julian ( y1, m1, d1, f1 );
    s2 = ymdf_to_s_julian ( y2, m2, d2, f2 );

    [ y3, m3, d3, f3 ] = ymdf_prev_julian ( y1, m1, d1, f1 );
    s3 = ymdf_to_s_julian ( y3, m3, d3, f3 );

    [ y4, m4, d4, f4 ] = ymdf_inc_julian ( y1, m1, d1, f1, days );
    s4 = ymdf_to_s_julian ( y4, m4, d4, f4 );

    fprintf ( 1, '  %15s  %15s  %15s  %15s\n', s1, s2, s3, s4 );

  end

  return
end


function [ xmin, ixmin ] = r8row_min ( m, n, x )

%*****************************************************************************80
%
%% R8ROW_MIN returns the minimums of rows of an R8ROW.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns in the array.
%
%    Input, real X(M,N), the R8ROW.
%
%    Output, real XMIN(M), the minimums of the rows of X.
%
%    Output, integer IXMIN(M); IXMIN(I) is the column of X in which
%    the minimum for row I occurs.
%
  for i = 1 : m

    ixmin(i) = 1;
    xmin(i) = x(i,1);
    for j = 2 : n
      if ( x(i,j) < xmin(i) )
        ixmin(i) = j;
        xmin(i) = x(i,j);
      end
    end

  end

  return
end


function [vertices, faces] = curveToMesh(curve, varargin)
%CURVETOMESH  Create a mesh surrounding a 3D curve.
%
%   [V, F] = curveToMesh(CURVE)
%   Computes the vertices and the faces of the mesh surrounding the
%   specified 3D curve.
%
%   [V, F] = curveToMesh(CURVE, THICKNESS)
%   Specifies the thickness of the mesh (distance between mesh vertices and
%   curve vertices). Default is 0.5.
%
%   [V, F] = curveToMesh(CURVE, THICKNESS, NCORNERS)
%   Also specifies the number of mesh vertices around each curve vertex.
%   Default is 8.
%
%
%   Example
%     % Creates a tubular mesh around a trefoil knot curve
%     t = linspace(0, 2*pi, 200)';
%     x = sin(t) + 2 * sin(2 * t);
%     y = cos(t) - 2 * cos(2 * t);
%     z = -sin(3 * t);
%     curve = [x, y, z];
%     [v2, f2] = curveToMesh(curve, .5, 16);
%     figure;
%     drawMesh(v2, f2);
%     axis equal; view(3);
%     axis([-4 4 -4 4 -2 2]);
%
%   See also
%     meshes3d, torusMesh, surfToMesh

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2015-01-07,    using Matlab 8.4.0.150421 (R2014b)
% Copyright 2015 INRA - Cepia Software Platform.

radius = .1;
if nargin > 1
    radius = varargin{1};
end

nCorners = 8;
if nargin > 2
    nCorners = varargin{2};
end

nNodes = size(curve, 1);
nVerts = nNodes * nCorners;

vertices = zeros(nVerts, 3);

% create reference corners, that will be rotated and translated
t = linspace(0, 2*pi, nCorners + 1)';
t(end) = [];
baseCorners = radius * [cos(t) sin(t) zeros(size(t))];

for iNode = 1:nNodes
    % coordinate of current node
    node = curve(iNode, :);

    % compute local tangent vector
    iNext = mod(iNode, nNodes) + 1;
    tangentVector = normalizeVector3d(curve(iNext, :) - node);

    % convert to spherical coordinates
    [theta, phi, rho] = cart2sph2(tangentVector); %#ok<ASGLU>

    % apply transformation to place corners around current node
    rotY = createRotationOy(theta);
    rotZ = createRotationOz(phi);
    trans = createTranslation3d(node);
    transformMatrix = trans * rotZ * rotY;
    corners = transformPoint3d(baseCorners, transformMatrix);

    % concatenate with other corners
    vertices( (1:nCorners) + (iNode - 1) * nCorners, :) = corners;
end

% indices of vertices
inds = (1:nVerts)';
add1 = repmat([ones(nCorners-1, 1) ; 1-nCorners], nNodes, 1);

% generate faces
faces = [inds ...
    mod(inds + add1 - 1, nVerts) + 1 ...
    mod(inds + nCorners + add1 - 1, nVerts) + 1 ...
    mod(inds + nCorners - 1, nVerts) + 1];


function fl = luflops (L, U)
%LUFLOPS compute the flop count for sparse LU factorization
%
%  Example:
%      fl = luflops (L,U)
%
%  Given a sparse LU factorization (L and U), return the flop count required
%  by a conventional LU factorization algorithm to compute it.   L and U can
%  be either sparse or full matrices.  L must be lower triangular and U must
%  be upper triangular.  Do not attempt to use this on the permuted L from
%  [L,U] = lu (A).  Instead, use [L,U,P] = lu (A) or [L,U,P,Q] = lu (A).
%
%  Note that there is a subtle undercount in this estimate.  Suppose A is
%  completely dense, but during LU factorization exact cancellation occurs,
%  causing some of the entries in L and U to become identically zero.  The
%  flop count returned by this routine is an undercount.  There is a simple
%  way to fix this (L = spones (L) + spones (tril (A))), but the fix is partial.
%  It can also occur that some entry in L is a "symbolic" fill-in (zero in
%  A, but a fill-in entry and thus must be computed), but numerically
%  zero.  The only way to get a reliable LU factorization would be to do a
%  purely symbolic factorization of A.  This cannot be done with
%  symbfact (A, 'col').
%
%  See NA Digest, Vol 00, #50, Tuesday, Dec. 5, 2000
%
% See also symbfact

%    Copyright 1998-2007, Timothy A. Davis

Lnz = full (sum (spones (L))) - 1 ;	% off diagonal nz in cols of L
Unz = full (sum (spones (U')))' - 1 ;	% off diagonal nz in rows of U
fl = 2*Lnz*Unz + sum (Lnz) ;


function test_suite = test_midwt
initTestSuite;

function test_midwt_1D
       x = makesig('LinChirp',8);
       h = daubcqf(4,'min');
       L = 2;
       [y,L] = mdwt(x,h,L);
       [x_new,L] = midwt(y,h,L);
assertVectorsAlmostEqual(x, x_new,'relative',0.0001);

function test_midwt_2D
       load lena512;
       x = lena512;
       h = daubcqf(6);
       [y,L] = mdwt(x,h);
       [x_new,L] = midwt(y,h);
assertEqual(L,9);
assertVectorsAlmostEqual(x, x_new,'relative',0.0001);


function linplus_test151 ( )

%*****************************************************************************80
%
%% TEST151 tests R8BB_INDICATOR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n1 = 6;
  n2 = 2;
  n = n1 + n2;
  ml = 1;
  mu = 1;
  na = ( 2 * ml + mu + 1 ) * n1 + 2 * n1 * n2 + n2 * n2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST151\n' );
  fprintf ( 1, '  R8BB_INDICATOR sets up an indicator matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N     = %d\n', n );
  fprintf ( 1, '  Matrix suborder N1 = %d\n', n1 );
  fprintf ( 1, '  Matrix suborder N2 = %d\n', n2 );
  fprintf ( 1, '  Lower bandwidth ML = %d\n', ml );
  fprintf ( 1, '  Upper bandwidth MU = %d\n', mu );
%
%  Set the matrix.
%
  a = r8bb_indicator ( n1, n2, ml, mu );

  r8bb_print ( n1, n2, ml, mu, a, '  The indicator matrix:' );

  return
end


function determ = fibonacci1_determinant ( n, f1, f2 )

%*****************************************************************************80
%
%% FIBONACCI1_DETERMINANT returns the determinant of the FIBONACCI1 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real F1, F2, the first two elements of the sequence
%    that will generate the Fibonacci sequence.
%
%    Output, real DETERM, the determinant.
%
  if ( n == 1 )
    determ = 1.0;
  elseif ( n == 2 )
    determ = -1.0;
  else
    determ = 0.0;
  end

  return
end


%                           runBenchmarkSuccessRate.m
%
% This example benchmarks algorithms based on their ability to reconstruct
% a synthetic signal (random Gaussian) using synthetic measurements
% (random Gaussian).  The benchmark shows how the
% different methods behave as the number of measurements increases.  The
% y-axis plots the success rate (rate of exact signal recovery).  This is
% done by setting params.policy='successrate'.
%
% The algorithms currently used in this implementation are all instances of
% PhaseMax, but with different levels of accuracy in the initializer.  To
% control the level of initialization accuracy, we set the initializer to
% "angle", and specify the angle between the true signal, and the initial
% guess.
%
% This script does the following:
%
% 1. Set up parameters and create a list of algorithm structs.
%
% 2. Invoke the general benchmark function benchmarkPR. A graph of errors
% (under specified error metrics) of different algorithms will be shown.
%
% The benchmark program compares the performance of specified algorithms on
% 1D gaussian measurements that have different m/n ratio(i.e. the ratio
% between the number of measurements and the number of unknowns).
%
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START----------------------------------

clc
clear
close all

%% 1.Set up parameters
% Choose x label (values shown on the x axis of the benchmark plot) and
% y label (values shown on the y-axis). The value on the x axis is the m/n
% ratio. The value on the y axis is 'reconerror', which is the relative
% 2-norm difference between the true and recovered signal.
xitem = 'm/n';
xvalues = 2:.5:12;
yitem = 'reconerror';

% Choose Dataset and set up dataSet '1DGaussian' specific parameters
dataSet = '1DGaussian';

% Set up general parameters
params.verbose = false;
params.numTrials = 20;         % run several random trials for each scenario, and report average results
params.n = 500;                 % num of unknown elements
params.isComplex = true;        % use complex matrices? or just stick to real?
params.policy = 'successrate';	% use the successrate
params.successConstant = 1e-4;

%  Each of these algorithm is an instance of PhaseMax.  However, they each
%  are initialized with starting points of different accuracies.  The
%  "angle" initializer grabs the "initAngle" entry from the options, and
%  produces an initializer that makes this angle with the true signal.
pmax25 = struct('algorithm','phasemax','initMethod','angle');
pmax25.tol=1e-6;
pmax25.initAngle=25/360*2*pi;

pmax36 = struct('algorithm','phasemax','initMethod','angle');
pmax36.tol=1e-6;
pmax36.initAngle=36/360*2*pi;

pmax45 = struct('algorithm','phasemax','initMethod','angle');
pmax45.tol=1e-6;
pmax45.initAngle=45/360*2*pi;

% Grab your pick of algorithms.
algorithms = {pmax25,pmax36,pmax45};

% Run benchmark
results = benchmarkSynthetic(xitem, xvalues, yitem, algorithms, dataSet, params);


function [NF,NL,NQc,NH,NPinf,NdF,NdQc,NdPinf,T] = ss_balance(F,L,Qc,H,Pinf,dF,dQc,dPinf)
% SS_BALANCE - Balance state space model for improved numerical stability
%
% Syntax:
%   [F,L,Qc,H,Pinf,dF,dQc,dPinf,T] = ss_balance(F,L,Qc,H,Pinf,dF,dQc,dPinf)
%
% In:
%   F           - Feedback matrix
%   L           - Noise effect matrix
%   Qc          - Spectral density of white noise process w(t)
%   H           - Observation model matrix
%   Pinf        - Covariance of the stationary process
%   dF          - Derivatives of F w.r.t. parameters
%   dQc         - Derivatives of Qc w.r.t. parameters
%   dPinf       - Derivatives of Pinf w.r.t. parameters
%
% Out:
%   (...)       - As above, but balanced
%   T           - T is the balancing matrix from 'balance'
%
% Description:
%   This function takes the SDE state space model matrices as inputs and
%   outputs the same matrices, but in a numerically more stable form.
%   This balancing is based on the Matlab 'balance' function (see the
%   reference).
%
%   The state space model is given as follows in terms of a stochastic
%   differential equation
%
%      df(t)/dt = F f(t) + L w(t),
%
%   where w(t) is a white noise process with spectral denisty Qc. The
%   observation model matrix is denoted by matrix H.
%
% References:
%   [1] Beresford N. Parlett and Christian Reinsch (1969). Balancing
%       a matrix for calculation of eigenvalues and eigenvectors.
%       Numerische Mathematik, 13(4): 293-304.
%
% See also:
%   BALANCE
%
% Copyright:
%   2014 Arno Solin and Simo Sarkka
%
%  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.

%% Balance the state space model

  % This is based on the following:
  %
  %  dx/dt = F x + L w
  %      y = H x
  %
  % Let T z = x, which gives
  %
  %  dz/dt = inv(T) F T z + inv(T) L w
  %      y = H T z
  %
  % See also: [A,B,C,E,S,P] = aebalance(A,B,C,E)

  % Balance the dynamic model matrix
  [T,NF] = balance(F);

  % Balance noise effect matrix
  NL = T\L;

  % Balance the measurement model
  NH = H*T;

  % Balance spectral density
  NQc = Qc;

  % Balance stationary state covariance matrix
  if nargin < 5
    NPinf = [];
  else
    L = chol(Pinf,'lower');
    LL = T\L;
    NPinf = LL*LL';
    %NPinf = T\Pinf/T;
  end

  % Balance partial derivatives in F
  if nargin < 6
    NdF = [];
  else
    NdF = dF;
    for j=1:size(dF,3)
      NdF(:,:,j) = T\dF(:,:,j)*T;
    end
  end

  % Balane partial derivatives in dQc;
  if nargin < 7
    NdQc = [];
  else
    NdQc = dQc;
  end

  % Balance partial derivatives in dF
  if nargin < 8
    NdPinf = [];
  else
    NdPinf = dPinf;
    for j=1:size(dF,3)
      NdPinf(:,:,j) = T\dPinf(:,:,j)/T;
    end
  end


function [mm] = ft2mm(ft)
% Convert length from feet to millimeters.
% Chad Greene 2012
mm = ft*304.8;


function exact = p04_exact ( )

%*****************************************************************************80
%
%% P04_EXACT returns the exact integral for problem 4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real EXACT, the value of the integral.
%
  exact = ( sqrt ( 32.0 ) / 3.0 ) * ( sqrt ( 27.0 ) - sqrt ( 8.0 ) - 1.0 );

  return
end


function [G] = spm_morlet_conv(G,w,dt,wnum)
% temporal convolution of complex spectral responses with Morlet envelope
% FORMAT [G] = spm_morlet_conv(G,w,dt,wnum)
%
% G      - (t x w x n x n) cross spectral density
% w      - Frequencies (Hz)
% dt     - sampling interval (sec)
% wnum   - Wavelet number: default = 2  s.d. = wnum/(2*pi*w)
%
% G      - convolved cross spectral density
%__________________________________________________________________________
%
% This routine simply smooths a cross spectral response to emulate a
% wavelet transform.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_morlet_conv.m 6857 2016-08-19 15:17:06Z karl $

% setup and defaults
%--------------------------------------------------------------------------
if nargin < 4, wnum = 8; end
[nt,nw,ni,nj] = size(G);
iw            = 1:nw;

% get (non-stationary) convolution matrix for frequencies
%--------------------------------------------------------------------------
f     = (-nw:nw)';
H     = zeros(nw,nw);
for i = 1:nw
    s      = w(i)/wnum;
    h      = exp(-f.^2/(2*s^2));
    h      = h(iw + nw - i);
    H(:,i) = h/sum(h);
end

% convolution over frequencies
%--------------------------------------------------------------------------
for i = 1:ni
    for j = 1:nj
        G(:,:,i,j) = G(:,:,i,j)*H;
    end
end

% convolution over time
%--------------------------------------------------------------------------
for k = 1:nw
    s     = wnum/(2*pi*w(k));
    t     = -(s*4):dt:(s*4);
    h     = exp(-t.^2/(2*s^2));
    h     = spm_convmtx(h',nt,'square');
    h     = diag(1./sum(h,2))*h;
    for i = 1:ni
        for j = 1:nj
            G(:,k,i,j) = h*G(:,k,i,j);
        end
    end
end


function [MB] = bit2MB(bit)
% Convert computery things from bits to megabytes.
% Chad A. Greene 2012
MB = bit*2^-23;


function g = p14_g ( n, x )

%*****************************************************************************80
%
%% P14_G evaluates the gradient for problem 14.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 March 2000
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real G(N), the gradient of the objective function.
%
  g = zeros ( n, 1 );

  for j = 1 : n

    if ( mod ( j, 2 ) == 1 )
      g(j) = - 2.0 * ( 1.0 - x(j) );
    else
      g(j) = 200.0 * ( x(j) - x(j-1) * x(j-1) );
      g(j-1) = g(j-1) - 400.0 * x(j-1) * ( x(j) - x(j-1) * x(j-1) );
    end

  end

  return
end


function q = cubic_integrate ( a, b )

%*****************************************************************************80
%
%% CUBIC_INTEGRATE integrates the cubic from A to B.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 February 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the integration interval.
%
%    Output, real Q, the integral from A to B.
%
  q = cubic_antiderivative ( b ) - cubic_antiderivative ( a );

  return
end


%% Ellipse Based Shape Parameters
%
%%
% In this section we discuss geometric properties of grains that are
% related to ellipses fitted to the grains. Additionally to the orientation
% |omega|, and the lengths |a|, |b| of the long axis and short axes that
% are computed by the command <grain2d.fitEllipse.html |[omega,a,b] =
% grains.fitEllipse|>  the following properties based on the fitted
% ellipses are avaiable.
%
% || <grain2d.longAxis.html |longAxis|> || long axis as @vector3d || <grain2d.shortAxis.html |shortAxis|>  || short axis as @vector3d ||
% || <grain2d.centroid.html |centroid|> || midpoint  || <grain2d.aspectRatio.html |aspectRatio|>  || long axis / short axis ||
%
% In order to demonstrate these properties we start by reconstructing the
% grain structure from a sample EBSD data set.

% load sample EBSD data set
mtexdata forsterite silent

% reconstruct grains and smooth them
[grains, ebsd.grainId] = calcGrains(ebsd('indexed'),'angle',5*degree);
ebsd(grains(grains.grainSize<10)) = [];
[grains, ebsd.grainId] = calcGrains(ebsd('indexed'),'angle',5*degree);
grains(grains.isBoundary) = [];

grains=smooth(grains('indexed'),10,'moveTriplePoints');

% plot the grains
plot(grains,'micronbar','off','lineWidth',2)

%% Fit Ellipses
%
% The basic command for fitting ellipses is <grain2d.fitEllipse
% |fitEllipse|>

[omega,a,b] = grains.fitEllipse;

plotEllipse(grains.centroid,a,b,omega,'lineColor','w','linewidth',2)

%%
% The returned variable |omega| is the angle describing the rotation of the
% ellipses and |a| and |b| are the length of the longest and shortest half
% axis. The midpoints of the ellipses can be computed by the command
% <grain2d.centroid.html |grains.centroid|>.  Note, that the ellipses are
% scaled such that the area of the ellipse coincides with the actual grain
% area. Alternatively, one can also scale the ellipse to fit the boundary
% length by using the option |boundary|.
%
%% Long and Short Axes
%
% The direction of the long and the short axis of the fitted ellipse can be
% obtained by the comands <grain2d.longAxis.html |grains.longAxis|> and
% <grain2d.shortAxis.html |grains.shortAxis|>. These directions are only
% well defined if the fitted ellipse is not to close to a perfect circle. A
% measure for how distinct the ellipse is from a perfect circle is the
% <grain2d.aspectRatio.html aspect ratio> which is defined as the quotient
% $a/b$ between the longest and the shortest axis. For a perfect circle
% the apect ratio is $1$ and increases to infinity when the ellipse becomes
% more and more elongated.
%
% Lets colorize the grains by their apect ratio and plot on top the long
% axis directions:

% visualize the aspect ratio
plot(grains,grains.aspectRatio,'linewidth',2,'micronbar','off')
setColorRange([0,4])
mtexColorbar('title','aspect ratio')

% and on top the long axes
hold on
quiver(grains,grains.longAxis,'Color','white')
hold off

%% Shape perfered orientation
%
% If we look at grains, we might wonder if there is a characteristic
% difference in the grain shape fabric between e.g. Forsterite and
% Enstatite. In contrast to crystal prefered orientations which which
% describe on the alignment of the atome lattices the shape prefered
% orientation (SPO) describes the algnment of the grains by shape in the
% bulk fabric.
%
% *Long Axis Distribution*
%
% The most direct way to analyse shape prefered orientations are rose
% diagrams of the distribution of the grain long axes. For those diagrams
% it is useful to weight the long axis by the grain area such that larger
% grains have a bigger impact on the distribution and by the aspect ratio
% as for grains with a small aspect ratio the long axis is not so well
% defined.

numBin = 50;

subplot(1,2,1)
weights = grains('forsterite').area .* (grains('forsterite').aspectRatio-1);
histogram(grains('forsterite').longAxis,numBin, 'weights', weights)
title('Forsterite')

subplot(1,2,2)
weights = grains('enstatite').area .* (grains('enstatite').aspectRatio - 1);
histogram(grains('enstatite').longAxis,numBin,'weights',weights)
title('Enstatite')

%%
% Instead of the histogram we may also fit a circular density distribution
% to the to the long axes using the command <calcDensity.thml
% |calcDensity|>.

tdfForsterite = calcDensity(grains('forsterite').longAxis,...
  'weights',norm(grains('forsterite').longAxis),'halfwidth');

tdfEnstatite = calcDensity(grains('enstatite').longAxis,...
  'weights',norm(grains('enstatite').longAxis));

plotSection(tdfForsterite, vector3d.Z, 'linewidth', 3)

hold on
plotSection(tdfEnstatite, vector3d.Z, 'linewidth', 3)
hold off

%%
%

close all
[freq,bc] = calcTDF(grains('fo'),'binwidth',3*degree);
plotTDF(bc,freq/sum(freq));

[freq,bc] = calcTDF(grains('en'),'binwidth',3*degree);
hold on
plotTDF(bc,freq/sum(freq));
hold off
legend('Forsterite','Enstatite','Location','eastoutside')
mtexTitle('long axes')

%% *Shortest Caliper Distribution*
%
% Alternatively, we may wonder if the common long axis of grains is does
% suitably represented by the direction normal to the shortest caliper of
% the grains. This can particularly be the case for aligned rectangular
% particles. The command <calcTDF.html |calcTDF|> also takes a list of
% angles and a list of weights or lengths as input

cPerpF = caliper(grains('fo'),'shortestPerp');
cPerpE = caliper(grains('en'),'shortestPerp');

[freqF,bcF] = calcTDF(cPerpF.rho, 'weights',cPerpF.norm, 'binwidth',3*degree);
[freqE,bcE] = calcTDF(cPerpE.rho, 'weights',cPerpE.norm, 'binwidth',3*degree);

plotTDF(bcF,freqF/sum(freqF));
hold on
plotTDF(bcE,freqE/sum(freqE));
hold off
legend('Forsterite','Enstatite','Location','eastoutside')

%%
% We can also smooth the functions with a wrapped Gaussian

pdfF = circdensity(bcF, freqF, 5*degree,'sum');
pdfE = circdensity(bcE, freqE, 5*degree,'sum');

plotTDF(bcF,pdfF);
hold on
plotTDF(bcE,pdfE);
hold off
mtexTitle('n.t.s. density estimate')
legend('Forsterite','Enstatite','Location','eastoutside')

%%
% Because best fit ellipses are always symmetric and the projection
% function of an entire grain always only consider the convex hull, grain
% shape fabrics can also be characterized by the the length weighted rose
% diagram of the directions of grain boundary segments.

[freqF,bcF] = calcTDF(grains('fo').boundary);
plotTDF(bcF,freqF/sum(freqF));
pdfF = circdensity(bcF, freqF, 5*degree,'sum');
hold on
plotTDF(bcF,pdfF);
hold off
mtexTitle('Forsterite grain boundaries')
nextAxis
[freqE,bcE] = calcTDF(grains('en').boundary);
plotTDF(bcE,freqE/sum(freqE));
pdfE = circdensity(bcE, freqE, 5*degree,'sum');
hold on
plotTDF(bcE,pdfE);
hold off
mtexTitle('Enstatite grain boundaries')

%% Characteristic Shape
%
% Note that this distribution is very prone to inherit artifacts based on
% the fact that most EBSD maps are sampled on a regular grid. We tried to
% overcome this problem by heavily smoothing the grain boundary. The little
% peaks at 0 and 90 degree are very likely still related to this sampling
% artifact.
%
% If we just add up all the individual elements of the rose diagram in
% order of increasing angles, we derive the characteristic shape. It can be
% regarded as to represent the average grain shape.

[csAngleF, csRadiusF] = characteristicShape(bcF,freqF);
[csAngleE, csRadiusE] = characteristicShape(bcE,freqE);

close all
plotTDF(csAngleF,csRadiusF,'nolabels');
hold on
plotTDF(csAngleE,csRadiusE,'nolabels');
hold off
legend('Forsterite','Enstatite','Location','eastoutside')

%%
% We may wonder if these results are significantly different or not
% TODO: get deviation from an ellipse etc


            % Diferite reprezentari grafice speciale
            % **************************************
			% ***** Grafica nr.1. *****
	subplot(4,2,1)
			% Desenarea unui poligon
			% - desemnarea coordonatelor varfurilor
	x=[0 2 4 3 1 0];
	y=[0 1 2 4 3 0];
			% - reprezentarea poligonului
	fill(x,y,'y')
			% - plasarea unui titlu
	title('FILL')
			% ***** Grafica nr.2. *****
	subplot(4,2,2)
	x=0:pi/10:2*pi;
	y=sin(x);
			% Reprezentarea grafica utilizand comanda bar
	bar(x,y)
	title('BAR')
			% - fixarea limitelor axelor de coordonate
	axis([0 2*pi -1.1 1.1])
			% ***** Grafica nr.3. *****
	subplot(4,2,3)
			% Reprezentarea grafica utilizand comanda stem
	stem(x,y)
	title('STEM')
	axis([0 2*pi -1.1 1.1])
			% ***** Grafica nr.4. *****
	subplot(4,2,4)
			% Reprezentarea grafica utilizand comanda stairs
	stairs(x,y)
	title('STAIRS')
	axis([0 2*pi -1.1 1.1])
			% ***** Grafica nr.5. *****
	subplot(4,2,5)
			% Generarea unui vector de eroare aleatoare
	e=rand(size(x)).*y./5;
			% Reprezentarea grafica utilizand comanda errorbar
	errorbar(x,y,e)
	title('ERRORBAR')
	axis([0 2*pi -1.1 1.1])
			% ***** Grafica nr.6. *****
	subplot(4,2,6)
			% Regenerarea vectorului x cu un pas mai mic
	x=0:pi/100:2*pi;
			% Reincarcarea vectorului y
	y=sin(x);
			% Reprezentarea grafica utilizand comanda hist
	hist(y,20)
	title('HIST')
            % ***** Grafica nr.7. *****
	subplot(4,2,7)
			% Reprezentarea grafica utilizand comanda pie
	pie([2 4 3 6],{'Nord','Sud','Est','Vest'})
	title('PIE')
            % ***** Grafica nr.8. *****
	subplot(4,2,8)
			% Reprezentarea grafica utilizand comanda area
	z = [1,5,6;3,4,7;2,8,3;2,6,10];
	area(z)
	title('AREA')


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   example 3 DOF planar robot.
%
%   Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche.
%   email: arturo.gil@umh.es date:   05/03/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

robot.name='Example 3DOF planar arm';

%kinematic data DH parameters
robot.DH.theta='[q(1) q(2) q(3)]';
robot.DH.d='[0  0  0]';
robot.DH.a='[1  1  1]';
%robot.DH.a='[3  1  0.5]';
robot.DH.alpha='[0  0  0]';

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

%Jacobian matrix
robot.J=[];

robot.J=['[-a(1)*sin(q(1))-a(2)*sin(q(1)+q(2))-a(3)*sin(q(1)+q(2)+q(3))  -a(2)*sin(q(1)+q(2))-a(3)*sin(q(1)+q(2)+q(3)) -a(3)*sin(q(1)+q(2)+q(3));' ...
           'a(1)*cos(q(1))+a(2)*cos(q(1)+q(2))+a(3)*cos(q(1)+q(2)+q(3))  a(2)*cos(q(1)+q(2))+a(3)*cos(q(1)+q(2)+q(3))  a(3)*cos(q(1)+q(2)+q(3));' ...
           '               0                                  0  0;' ...
           '               0                                  0  0;' ...
           '               0                                  0  0;' ...
           '               1                                  1  1]'];

robot.kind=['R' 'R' 'R'];

%Function name to compute inverse kinematic
robot.inversekinematic_fn = 'inversekinematic_3dofplanar(robot, T)';
robot.directkinematic_fn = 'directkinematic(robot, q)';

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-180) deg2rad(180); %Axis 1, minimum, maximum
                deg2rad(-180) deg2rad(180);
                deg2rad(-180) deg2rad(180)]; %Axis 2, minimum, maximum

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = []; %empty, not available

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time
% end effectors maximum velocity
robot.linear_velmax = 0; %m/s, example, not available

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

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

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%DYNAMIC PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
robot.has_dynamics=1;

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

%link masses (kg)
robot.dynamics.masses=[1 1 1];

%COM of each link with respect to own reference system
robot.dynamics.r_com=[-0.5      0         0; %(rx, ry, rz) link 1
                      -0.5      0         0;
                      -0.5      0         0];%(rx, ry, rz) link 2

%link masses
m1 = robot.dynamics.masses(1);
m2 = robot.dynamics.masses(2);
m3 = robot.dynamics.masses(3);

a=eval(robot.DH.a);
L1 = a(1);
L2 = a(2);
L3 = a(3);

%Momentos de inercia de cada eslabon.
% Ixx	Iyy	Izz	Ixy	Iyz	Ixz, por cada fila
robot.dynamics.Inertia=[0   m1*L1^2/3   m1*L1^2/3    0	0	0;
                        0   m2*L2^2/3   m2*L2^2/3    0	0	0;
                        0   m3*L3^2/3   m3*L3^2/3    0	0	0];

%Actuator rotor inertia
robot.motors.Inertia=[0 0 0];
%Reduction ratio motor/joint speed
robot.motors.G=[1 1 1];
%consider friction
robot.friction=0;
%Viscous friction factor, motor referred
%robot.B = [1e-3  1e-3 1e-3];
robot.motors.Viscous = [10  10 10];
%Coulomb friction, motor referred
robot.motors.Coulomb = [0 0;
            0 0;
            0 0];


function results = vl_test_pegasos(varargin)
% VL_TEST_KDTREE
vl_test_init ;

function s = setup()
randn('state',0) ;

s.biasMultiplier = 10 ;
s.lambda = 0.01 ;

Np = 10 ;
Nn = 10 ;
Xp = diag([1 3])*randn(2, Np) ;
Xn = diag([1 3])*randn(2, Nn) ;
Xp(1,:) = Xp(1,:) + 2 + 1 ;
Xn(1,:) = Xn(1,:) - 2 + 1 ;

s.X = [Xp Xn] ;
s.y = [ones(1,Np) -ones(1,Nn)] ;
%s.w = exact_solver(s.X, s.y, s.lambda, s.biasMultiplier)
s.w = [1.181106685845652 ;
       0.098478251033487 ;
       -0.154057992404545 ] ;

function test_problem_1(s)
for conv = {@single,@double}
  vl_twister('state',0) ;
  conv = conv{1} ;
  w = vl_pegasos(conv(s.X), int8(s.y), s.lambda, ...
                 'NumIterations', 100000, ...
                 'BiasMultiplier', s.biasMultiplier, ...
                 'Preconditioner', conv([1 1 .1])) ;
  vl_assert_almost_equal(w, conv(s.w), 0.1) ;
end

function test_continue_training(s)
for conv = {@single,@double}
  conv = conv{1} ;

  vl_twister('state',0) ;
  w = vl_pegasos(conv(s.X), int8(s.y), s.lambda, ...
                 'NumIterations', 3000, ...
                 'BiasMultiplier', s.biasMultiplier) ;

  vl_twister('state',0) ;
  w1 = vl_pegasos(conv(s.X), int8(s.y), s.lambda, ...
                 'StartingIteration', 1, ...
                 'NumIterations', 1500, ...
                  'BiasMultiplier', s.biasMultiplier) ;
  w2 = vl_pegasos(conv(s.X), int8(s.y), s.lambda, ...
                  'StartingIteration', 1501, ...
                  'StartingModel', w1, ...
                  'NumIterations', 1500, ...
                  'BiasMultiplier', s.biasMultiplier) ;
  vl_assert_almost_equal(w,w2,1e-7) ;
end

function test_continue_training_with_perm(s)
perm = uint32(randperm(size(s.X,2))) ;
for conv = {@single,@double}
  conv = conv{1} ;

  vl_twister('state',0) ;
  w = vl_pegasos(conv(s.X), int8(s.y), s.lambda, ...
                 'NumIterations', 3000, ...
                 'BiasMultiplier', s.biasMultiplier, ...
                 'Permutation', perm) ;

  vl_twister('state',0) ;
  w1 = vl_pegasos(conv(s.X), int8(s.y), s.lambda, ...
                 'StartingIteration', 1, ...
                 'NumIterations', 1500, ...
                 'BiasMultiplier', s.biasMultiplier, ...
                  'Permutation', perm) ;
  w2 = vl_pegasos(conv(s.X), int8(s.y), s.lambda, ...
                  'StartingIteration', 1501, ...
                  'StartingModel', w1, ...
                  'NumIterations', 1500, ...
                  'BiasMultiplier', s.biasMultiplier, ...
                  'Permutation', perm) ;
  vl_assert_almost_equal(w,w2,1e-7) ;
end

function w = exact_solver(X, y, lambda, biasMultiplier)
N = size(X,2) ;
model = svmtrain(y', [(1:N)' X'*X], sprintf(' -c %f -t 4 ', 1/(lambda*N))) ;
w = X(:,model.SVs) * model.sv_coef ;
w(3) = - model.rho / biasMultiplier ;
format long ;
disp('model w:')
disp(w)


function c = gsp_lanczos_op(G,fi,s,param)
%GSP_LANCZOS_OP Perform the lanczos approximation of the signal s

if nargin < 4, param = struct; end
if ~isfield(param,'verbose'), param.verbose = 1; end;
if ~isfield(param,'order'), param.order = 30; end

Nf = numel(fi);
Nv = size(s,2);
c = zeros(G.N*Nf,Nv);

for jj = 1:Nv

    if sum(abs(s(:,jj)))>eps
        [V,H] = lanczos(G.L, param.order, s(:,jj));

        [Uh, Eh] = eig(H);

        Eh = diag(Eh);
        Eh(Eh<0) = 0;
        fie = gsp_filter_evaluate(fi,Eh);
        V = V*Uh;

        for ii=1:Nf
           c((1:G.N) + G.N*(ii-1),jj) = V * (fie(:, ii) .* (V'*s(:,jj)));
        end
    else
        c(:,jj) = 0;
    end
end

% [V,H] = lanczos(G.L, k, s);
%
% for jj = 1:Nv
%     ind = (1:size(H,1))+size(H,1)*(jj-1);
%     [Uh, Eh] = eig(H(:,ind));
%
%
%     Eh = diag(Eh);
%     fie = gsp_filter_evaluate(fi,Eh);
%     V(:,ind) = V(:,ind)*Uh;
%
%
%     for ii=1:Nf
%        c((1:G.N) + G.N*(ii-1),jj) = V(:,ind) * fie(:, ii) .* (V(:,ind)'*s(:,jj));
%     end
% end

end

function [V,H,orth] = lanczos(A,order,x)

[N,M] = size(x);

% normalization
norm2vec = @(x) (sum(x.^2,1)).^0.5;
q = x./repmat(norm2vec(x),N,1);

% Initialization
hiv =0:order:(order*M-1); % helping indice vector

V = zeros(N,M*order);
V(:,1+hiv) = q;

H = zeros(order+1,order*M);

r = A*q;
H(1,1+hiv) = sum(q .* r, 1 );
r = r - repmat(H(1,1+hiv),N,1).*q;
H(2,1+hiv) = norm2vec(r);

if (nargout > 2)
    orth = zeros(M,1);
    orth(1) = norm(V'*V - M);
end

for k = 2:order

    if (sum(abs(H(k,k-1+hiv))) <= eps)
        H = H(1:k-1,sum_ind(1:k-1,hiv));
        V = V(:,sum_ind(1:k-1,hiv));
        if (nargout > 2)
            orth = orth(1:k-1);
        end
        return;
    end

    H(k-1,hiv+k) = H(k,hiv+k-1);
    v = q;
    q = r./repmat(H(k-1,k+hiv),N,1);
    V(:,k+hiv) = q;

    r = A*q;
    r = r - repmat(H(k-1,k+hiv),N,1).*v;
    H(k,k+hiv) = sum(q .* r, 1 );

    r = r - repmat(H(k,k+hiv),N,1).*q;
    % The next line has to be checked
    r = r - V*(V'*r); % full reorthogonalization
    H(k+1,k+hiv) = norm2vec(r);

    if (nargout > 2)
        orth(k) = [orth, norm(V'*V - M)];
    end
end

H = H(1:order,1:order);

% H = zeros(order+1,order);
%
% q = x/norm(x);
% V(:,1) = q;
%
% r = A*q;
%
% H(1,1) = q'*r;
% r = r - H(1,1)*q ;
% H(2,1) = norm(r);
%
% if (nargout > 2)
%  orth = norm(V'*V - 1);
% end
%
% for k = 2:order
%
%     if (abs(H(k,k-1)) <= 1e-15)
%         H = H(1:k-1,1:k-1);
%         return;
%     end
%
%     H(k-1,k) = H(k,k-1);
%     v = q;
%     q = r/H(k-1,k);
%     V(:,k) = q;
%
%     r = A*q;
%     r = r - H(k-1,k)*v;
%     H(k,k) = q'*r;
%
%     r = r - H(k,k)*q;
%     r = r - V*(V'*r); % full reorthogonalization
%     H(k+1,k) = norm(r);
%
%     if (nargout > 2)
%         orth = [orth, norm(V'*V - eye(k))];
%     end
% end
%
% H = H(1:order,1:order);
%
end

function ind = sum_ind(ind1,ind2)
    ind = repmat(ind1(:),1,numel(ind2)) + repmat((ind2(:))',numel(ind1),1);
    ind = (ind(:))';
end


function [dx, dxGrad] = cstDyn(x,u)
% [dx, dxGrad] = cstDyn(x,u)
%
% Computes the dynamics (and gradients) for a 1d point mass on a
% friction-less plane with a force actuator.
%

% q = x(1,:);   %Position
dq = x(2,:);   %Velocity
ddq = u(1,:);  %Acceleration

dx = [dq;ddq];   %Pack up derivative of state

if nargout == 2   % Analytic gradients
    nTime = length(u);

    dqGrad = zeros(1,6,nTime); %6 = [time + pos + vel + force + slack];
    dqGrad(1,3,:) = 1; %gradient dq wrt dq

    ddqGrad = zeros(1,6,nTime);  %6 = [time + angle + rate +  force + slack];
    ddqGrad(1,4,:) = 1;  %gradient ddq wrt u

    dxGrad = cat(1, dqGrad, ddqGrad);

end

end


function [omckk,Tckk,Rckk,H,x,ex,JJ] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond),

%compute_extrinsic
%
%[omckk,Tckk,Rckk,H,x,ex] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c)
%
%Computes the extrinsic parameters attached to a 3D structure X_kk given its projection
%on the image plane x_kk and the intrinsic camera parameters fc, cc and kc.
%Works with planar and non-planar structures.
%
%INPUT: x_kk: Feature locations on the images
%       X_kk: Corresponding grid coordinates
%       fc: Camera focal length
%       cc: Principal point coordinates
%       kc: Distortion coefficients
%       alpha_c: Skew coefficient
%
%OUTPUT: omckk: 3D rotation vector attached to the grid positions in space
%        Tckk: 3D translation vector attached to the grid positions in space
%        Rckk: 3D rotation matrices corresponding to the omc vectors
%        H: Homography between points on the grid and points on the image plane (in pixel)
%           This makes sense only if the planar that is used in planar.
%        x: Reprojections of the points on the image plane
%        ex: Reprojection error: ex = x_kk - x;
%
%Method: Computes the normalized point coordinates, then computes the 3D pose
%
%Important functions called within that program:
%
%normalize_pixel: Computes the normalize image point coordinates.
%
%pose3D: Computes the 3D pose of the structure given the normalized image projection.
%
%project_points.m: Computes the 2D image projections of a set of 3D points

if nargin < 8,
   thresh_cond = inf;
end;

if nargin < 7,
   MaxIter = 20;
end;

if nargin < 6,
   alpha_c = 0;
	if nargin < 5,
   	kc = zeros(5,1);
   	if nargin < 4,
      	cc = zeros(2,1);
      	if nargin < 3,
         	fc = ones(2,1);
         	if nargin < 2,
            	error('Need 2D projections and 3D points (in compute_extrinsic.m)');
            	return;
         	end;
      	end;
   	end;
	end;
end;

% Initialization:

[omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c);

% Refinement:
[omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond);

% computation of the homography (not useful in the end)

H = [Rckk(:,1:2) Tckk];

% Computes the reprojection error in pixels:

x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);

ex = x_kk - x;

% Converts the homography in pixel units:

KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1];

H = KK*H;

return;

% Test of compte extrinsic:

Np = 4;
sx = 10;
sy = 10;
sz = 5;

om = randn(3,1);
T = [0;0;100];

noise = 2/1000;

XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)];
xx = project_points(XX,om,T);

xxn = xx + noise * randn(2,Np);

[omckk,Tckk] = compute_extrinsic(xxn,XX);

[om omckk om-omckk]
[T Tckk T-Tckk]

figure(3);
plot(xx(1,:),xx(2,:),'r+');
hold on;
plot(xxn(1,:),xxn(2,:),'g+');
hold off;


function value = r8_tan ( x )

%*****************************************************************************80
%
%% R8_TAN evaluates the tangent of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the tangent of X.
%
  persistent nterms
  persistent pi2rec
  persistent sqeps
  persistent tancs
  persistent xmax
  persistent xsml

  pi2rec = 0.011619772367581343075535053490057;

  if ( isempty ( nterms ) )
    tancs = [ ...
      +0.22627932763129357846578636531752, ...
      +0.43017913146548961775583410748067E-01, ...
      +0.68544610682565088756929473623461E-03, ...
      +0.11045326947597098383578849369696E-04, ...
      +0.17817477903926312943238512588940E-06, ...
      +0.28744968582365265947529646832471E-08, ...
      +0.46374854195902995494137478234363E-10, ...
      +0.74817609041556138502341633308215E-12, ...
      +0.12070497002957544801644516947824E-13, ...
      +0.19473610812823019305513858584533E-15, ...
      +0.31417224874732446504614586026666E-17, ...
      +0.50686132555800153941904891733333E-19, ...
      +0.81773105159836540043979946666666E-21, ...
      +0.13192643412147384408951466666666E-22, ...
      +0.21283995497042377309866666666666E-24, ...
      +0.34337960192345945292800000000000E-26, ...
      +0.55398222121173811200000000000000E-28, ...
      +0.89375227794352810666666666666666E-30, ...
      +0.14419111371369130666666666666666E-31 ]';
    nterms = r8_inits ( tancs, 19, 0.1 * r8_mach ( 3 ) );
    xmax = 1.0 / r8_mach ( 4 );
    xsml = sqrt ( 3.0 * r8_mach ( 3 ) );
    sqeps = sqrt ( r8_mach ( 4 ) );
  end

  y = abs ( x );

  if ( xmax < y )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_TAN - Warning!\n' );
    fprintf ( 1, '  No precision because |X| is big.\n' );
    value = 0.0;
    return
  end

  ainty = r8_aint ( y );
  yrem = y - ainty;
  prodbg = 0.625 * ainty;
  ainty = r8_aint ( prodbg );
  y = ( prodbg - ainty ) + 0.625 * yrem + pi2rec * y;
  ainty2 = r8_aint ( y );
  ainty = ainty + ainty2;
  y = y - ainty2;

  ifn = r8_aint ( mod ( ainty, 2.0 ) );

  if ( ifn == 1 )
    y = 1.0 - y;
  end

  if ( 1.0 - y < abs ( x ) * sqeps )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_TAN - Warning!\n' );
    fprintf ( 1, '  Answer < half precision.\n' );
    fprintf ( 1, '  |X| big or X near pi/2 or 3*pi/2.\n' );
  end

  if ( y == 1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_TAN - Fatal error!\n' );
    fprintf ( 1, '  X is pi/2 or 3*pi/2.\n' );
    value = 0.0
    error ( 'R8_TAN - Fatal error!' )
  end

  if ( y <= 0.25 )

    value = y;
    if ( xsml < y )
      value = y * ( 1.5 + r8_csevl ( 32.0 * y * y - 1.0, tancs, nterms ) );
    end

  elseif ( y <= 0.5 )

    value = 0.5 * y * ( 1.5 + r8_csevl ( ...
      8.0 * y * y - 1.0, tancs, nterms ) );
    value = 2.0 * value / ( 1.0 - value * value );

  else

    value = 0.25 * y * ( 1.5 + r8_csevl ( ...
      2.0 * y * y - 1.0, tancs, nterms ) );
    value = 2.0 * value / ( 1.0 - value * value );
    value = 2.0 * value / ( 1.0 - value * value );

  end

  if ( x < 0.0 )
    value = - abs ( value );
  elseif ( 0.0 < x )
    value = + abs ( value );
  end

  if ( ifn == 1 )
    value = - value;
  end

  return
end


function [X, r] = odePinv(A,varargin)
%
% the standard pinv has been modified to also return the rank of the matrix
%
%PINV   Pseudoinverse.
%   X = PINV(A) produces a matrix X of the same dimensions
%   as A' so that A*X*A = A, X*A*X = X and A*X and X*A
%   are Hermitian. The computation is based on SVD(A) and any
%   singular values less than a tolerance are treated as zero.
%   The default tolerance is MAX(SIZE(A)) * NORM(A) * EPS(class(A)).
%
%   PINV(A,TOL) uses the tolerance TOL instead of the default.
%
%   Class support for input A:
%      float: double, single
%
%   See also RANK.

%   Copyright 1984-2004 The MathWorks, Inc.
%   $Revision: 5.12.4.2 $  $Date: 2004/12/06 16:35:27 $

if isempty(A)     % quick return
  X = zeros(size(A'),class(A));
  return
end

[m,n] = size(A);

if n > m
   [X, r] = odePinv(A',varargin{:})';
else
   [U,S,V] = svd(A,0);
   if m > 1, s = diag(S);
      elseif m == 1, s = S(1);
      else s = 0;
   end
   if nargin == 2
      tol = varargin{1};
   else
      tol = max(m,n) * eps(max(s));
   end
   r = sum(s > tol);
   if (r == 0)
      X = zeros(size(A'),class(A));
   else
      s = diag(ones(r,1)./s(1:r));
      X = V(:,1:r)*s*U(:,1:r)';
   end
end


function [C] = tensor_product(A,B)
  Au = tensor_unfold(A);
  Bu = tensor_unfold(B);

  Auc = circmat(Au,size(A));

  AucBu = Auc*Bu;

  C = tensor_fold(AucBu,size(A));
end


function [bler, ber] = simulation(N, K, design_epsilon, max_runs, max_err, resolution, ebno_vec, list_size_vec, g, crc_length)
R = (K - crc_length)/N;
lambda_offset = 2.^(0 : log2(N));
llr_layer_vec = get_llr_layer(N);
bit_layer_vec = get_bit_layer(N);
[G_crc, H_crc] = crc_generator_matrix(g, K - crc_length);
crc_parity_check = G_crc';

%beta expansion Code Construction, proposed by Huawei
% beta = sqrt(sqrt(2));
% % channels = PW(N, beta);
% % channels = HPW(N, beta);
% channels = EPW(N, beta);
% [~, channel_ordered] = sort(channels, 'descend');
% info_bits = sort(channel_ordered(1 : K));
% frozen_bits = ones(N , 1);
% frozen_bits(info_bits) = 0;

%Bhattacharyya Code (BEC) Construction
% channels = get_BEC_IWi(N, design_epsilon);
% [~, channel_ordered] = sort(channels, 'descend');
% info_bits = sort(channel_ordered(1 : K), 'ascend');
% frozen_bits = ones(N , 1);
% frozen_bits(info_bits) = 0;
% info_bits_logical = logical(mod(frozen_bits + 1, 2));

%Gaussian approximation Code Construction
design_snr = 2.5;
sigma_cc = 1/sqrt(2 * R) * 10^(-design_snr/20);
[channels, ~] = GA(sigma_cc, N);
[~, channel_ordered] = sort(channels, 'descend');
info_bits = sort(channel_ordered(1 : K), 'ascend');
frozen_bits = ones(N, 1);
frozen_bits(info_bits) = 0;
frozen_bits = logical(frozen_bits);

%Following MATLAB code is used for matrix-multiplication based systematic polar encoding
FN = get_GN(N);
GAA = FN(info_bits, info_bits);
GAAC = FN(info_bits, frozen_bits);
Generate_xAC = mod(GAAC' * GAA', 2);
systematic_encoding_algorithm = 1;

%This value can be 1, 2, 3, and 4.
%1 : 'Matrix_multiplication_systematic_encoder'
%!!!!But when N is large,the Generator matrix will be large, too. The computer may not have such large storage.
%2 : 'Sarkis_Two_step_systematic_encoder'
%3 : 'Arikan_SC_style_systematic_encoder'
%4 : 'Arikan_Recursive_systematic_encoder'. This one is slow.

%Special constituent nodes
node_type_matrix = get_node_structure(frozen_bits);
psi_vec = get_psi_for_advanced_sc_decoder(node_type_matrix);

%Results Stored
bler = zeros(length(ebno_vec), length(list_size_vec));
num_runs = zeros(length(ebno_vec), length(list_size_vec));
ber = zeros(length(ebno_vec), length(list_size_vec));
%Loop starts
tic
for i_run = 1 : max_runs
    if  mod(i_run, max_runs/resolution) == 1
        disp(' ');
        disp(['Sim iteration running = ' num2str(i_run)]);
        disp(['N = ' num2str(N) ' K = ' num2str(K)]);
        disp(['List size = ' num2str(list_size_vec)]);
        disp('Current block error performance');
        disp(num2str([ebno_vec'  bler./num_runs]));
        disp('Current bit error performance');
        disp(num2str([ebno_vec'  ber./num_runs/K]));
        disp(' ')
    end
    info  = rand(K - crc_length, 1) > 0.5;
    info_with_crc = [info; mod(crc_parity_check * info, 2)];
    switch systematic_encoding_algorithm
        case 1
            parity_check_bits = mod(Generate_xAC * info_with_crc, 2);
            x = zeros(N, 1);
            x(info_bits) = info_with_crc;
            x(frozen_bits) = parity_check_bits;
        case 2
            x = sarkis_systematic_polar_encoder(info_with_crc, info_bits, frozen_bits, N, lambda_offset, llr_layer_vec);
        case 3
            x = arikan_sc_systematic_polar_encoder(info_with_crc, frozen_bits, info_bits, lambda_offset, llr_layer_vec, bit_layer_vec);
        case 4
            x = zeros(N, 1);
            x(info_bits) = info_with_crc;
            x = arikan_recursive_systematic_polar_encoder(x, mod(frozen_bits + 1, 2));
    end
    bpsk = 1 - 2 * x;
    noise = randn(N, 1);
    prev_decoded = zeros(length(ebno_vec), length(list_size_vec));
    for i_ebno = 1 : length(ebno_vec)
        sigma = 1/sqrt(2 * R) * 10^(-ebno_vec(i_ebno)/20);
        y = bpsk + sigma * noise;
        llr = 2/sigma^2*y;
        %*******Simulaion Accelaration*********
        for i_list = 1 : length(list_size_vec)
            if i_list ~= 1
                if bler(i_ebno, i_list) == max_err
                    continue;
                end
            else
                if all(bler(i_ebno, 2 : end) == max_err)
                    continue
                end
            end
            num_runs(i_ebno, i_list) = num_runs(i_ebno, i_list) + 1;
            run_sim = 1;
            for i_ebno2 = 1 : i_ebno
                for i_list2 = 1 : i_list
                    if prev_decoded(i_ebno2, i_list2)
                        run_sim = 0;
                    end
                end
            end
            if run_sim == 0
                continue;
            end
            %*******Simulaion Accelaration*********
            if list_size_vec(i_list) == 1
                polar_info_esti = SC_decoder(llr, frozen_bits, lambda_offset, llr_layer_vec, bit_layer_vec, info_bits);
%                 polar_info_esti = FastSCdecoder(llr, info_bits, node_type_matrix, lambda_offset, llr_layer_vec, psi_vec, bit_layer_vec);
            else
                polar_info_esti = FastSCL_decoder(llr, list_size_vec(i_list), info_bits, lambda_offset, llr_layer_vec, bit_layer_vec, psi_vec, node_type_matrix, H_crc);
            end
            if any(polar_info_esti ~= info_with_crc)
                bler(i_ebno, i_list) = bler(i_ebno, i_list) + 1;
                ber(i_ebno, i_list) = ber(i_ebno, i_list) + sum(polar_info_esti(1 : K - crc_length) ~= info_with_crc(1 : K - crc_length));
                %Caution, this is data bits error rate, regardless of CRC
                %bits.
            else
                prev_decoded(i_ebno, i_list) = 1;
            end

        end
    end
end
toc
end