function b = cptsl ( n, d, e, b )

%*****************************************************************************80
%
%% CPTSL solves a Hermitian positive definite tridiagonal linear system.
%
%  Discussion;
%
%    The system does not have to be factored first.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, complex D(N), the diagonal of the matrix.
%
%    Input, complex E(N), the superdiagonal of the matrix in E(1:N-1).
%
%    Input, complex B(N), the right hand side.
%
%    Output, complex B(N), the solution.
%

%
%  Check for 1 x 1 case.
%
  if ( n == 1 )
    b(1) = b(1) / d(1);
    return
  end

  nm1 = n - 1;
  nm1d2 = floor ( ( n - 1 ) / 2 );

  if ( n ~= 2 )

    kbm1 = n - 1;
%
%  Zero top half of subdiagonal and bottom half of superdiagonal.
%
    for k = 1 : nm1d2
      t1 = conj ( e(k) ) / d(k);
      d(k+1) = d(k+1) - t1 * e(k);
      b(k+1) = b(k+1) - t1 * b(k);
      t2 = e(kbm1) / d(kbm1+1);
      d(kbm1) = d(kbm1) - t2 * conj ( e(kbm1) );
      b(kbm1) = b(kbm1) - t2 * b(kbm1+1);
      kbm1 = kbm1 - 1;
    end

  end

  kp1 = nm1d2 + 1;
%
%  Clean up for possible 2 x 2 block at center.
%
  if ( mod ( n, 2 ) == 0 )
    t1 = conj ( e(kp1) ) / d(kp1);
    d(kp1+1) = d(kp1+1) - t1 * e(kp1);
    b(kp1+1) = b(kp1+1) - t1 * b(kp1);
    kp1 = kp1 + 1;
  end
%
%  Back solve starting at the center, going towards the top and bottom.
%
  b(kp1) = b(kp1) / d(kp1);

  if ( n ~= 2 )

    k = kp1 - 1;
    ke = kp1 + nm1d2 - 1;

    for kf = kp1 : ke
      b(k) = ( b(k) - e(k) * b(k+1) ) / d(k);
      b(kf+1) = ( b(kf+1) - conj ( e(kf) ) * b(kf) ) / d(kf+1);
      k = k - 1;
    end

  end

  if ( mod ( n, 2 ) == 0 )
    b(1) = ( b(1) - e(1) * b(2) ) / d(1);
  end

  return
end


%FindInInterval - Find values that fall in a given interval.
%
%  The equivalent Matlab code is trivial
%
%      i = find(values(:,1)>=interval(1)&values(:,1)<=interval(2));
%      indices = [i(1),i(end)];
%
%  but becomes extremely slow when dealing with very large lists.
%  This function can dramatically speed up things whenever one needs
%  to repeatedly find values in a long list of intervals.
%
%  USAGE
%
%    indices = FindInInterval(values,interval,from)
%
%    values         values to test, sorted in ascending order
%    interval       [start,stop] pair
%    from           optional initial index (see example below)
%
%  OUTPUT
%
%    indices        indices of the first and last values that fall
%                   in the interval
%
%  EXAMPLE
%
%    This code assumes that the variable 'interval' contains a list
%    of non-overlapping intervals sorted in ascending order,
%    i.e. interval(i+1,1) >= interval(i,2).
%
%    previous = 1;
%    for i = 1:n,
%       % Find values within this window
%       j = FindInInterval(values,interval(i,:),previous);
%       previous = j(1);
%       % ... do whatever computations here ...
%    end
%
%  SEE
%
%    See also ConsolidateIntervals, SubtractIntervals, ExcludeIntervals,
%    InIntervals, Restrict, CountInIntervals, PlotIntervals.

% Copyright (C) 2004-2010 by Michaël Zugaro
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.


function boxOut = bbox_scale2(boxIn,scale,szOut)
% Copyright (C) 2016 Hakan Bilen.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

if isempty(boxIn), boxOut = []; return; end

boxOut = scale * (boxIn-1) + 1;

boxOut = [max(1,round(boxOut(:,1))),...
  max(1,round(boxOut(:,2))),...
  min(szOut(1),round(boxOut(:,3))),...
  min(szOut(2),round(boxOut(:,4)))];


function c = tapas_hgf_jget_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the Hierarchical Gaussian Filter (HGF) model for the jumping
% Gaussian estimation task (JGET).
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, and Stephan KE. (2011). A Bayesian foundation
% for individual learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., the omegas). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., the sigmas).
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin at the j-th level are arbitrary. This is the case if the observation model does not
% contain the representations mu_j and sigma_j. A choice of scale and origin is then implied by
% fixing the initial value mu_j_0 of mu_j and either kappa_j-1 or omega_j-1.
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_hgf_jget_plotTraj(est)
%
% where est is the stucture returned by fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mux_0      row vector of initial values of mu_x (in ascending order of levels)
%         est.p_prc.sax_0      row vector of initial values of sigma_x (in ascending order of levels)
%         est.p_prc.mua_0      row vector of initial values of mu_alpha (in ascending order of levels)
%         est.p_prc.saa_0      row vector of initial values of sigma_alpha (in ascending order of levels)
%         est.p_prc.kau        kappa_u
%         est.p_prc.omu        omega_u
%         est.p_prc.kax        row vector of kappa_x (in ascending order of levels)
%         est.p_prc.omx        row vector of omega_x (in ascending order of levels)
%         est.p_prc.kaa        row vector of kappa_alpha (in ascending order of levels)
%         est.p_prc.oma        row vector of omega_alpha (in ascending order of levels)
%
%         est.traj.mux         mux (rows: trials, columns: levels)
%         est.traj.sax         sigma_x (rows: trials, columns: levels)
%         est.traj.muxhat      prediction of mu_x (rows: trials, columns: levels)
%         est.traj.saxhat      precisions of predictions of x (rows: trials, columns: levels)
%         est.traj.wx          weighting factors for x (rows: trials, columns: levels)
%         est.traj.dax         volatility prediction errors in x (rows: trials, columns: levels)
%         est.traj.mua         mu_alpha (rows: trials, columns: levels)
%         est.traj.saa         sigma_alpha (rows: trials, columns: levels)
%         est.traj.muahat      prediction of mu_alpha (rows: trials, columns: levels)
%         est.traj.saahat      precisions of predictions of alpha (rows: trials, columns: levels)
%         est.traj.wa          weighting factors for alpha (rows: trials, columns: levels)
%         est.traj.daa         volatility prediction errors in alpha (rows: trials, columns: levels)
%         est.traj.dau         input prediction error
%
%
%
% Tips:
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior means of the kappas, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
% - If the log-model evidence cannot be calculated because the Hessian poses problems, look at
%   est.optim.H and fix the parameters that lead to NaNs.
%
% - Your guide to all these adjustments is the log-model evidence (LME). Whenever the LME increases
%   by at least 3 across datasets, the adjustment was a good idea and can be justified by just this:
%   the LME increased, so you had a better model.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013-2014 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'hgf_jget';

% Number of levels (minimum: 2)
c.n_levels = 2;

% Sufficient statistics of Gaussian parameter priors

% PLACEHOLDER VALUES
% It is often convenient to set some priors to values
% derived from the inputs. This can be achieved by
% using placeholder values. The available placeholders
% are:
%
% 99991   Value of the first input
%         Usually a good choice for mux_0mu(1)
% 99992   Variance of the first 20 inputs
%         Usually a good choice for mux_0sa(1)
% 99993   Log-variance of the first 20 inputs
%         Usually a good choice for logsax_0mu(1) and mua_0mu(1)
% 99994   Log-variance of the first 20 inputs minus two
%         Usually a good choice for omxmu(1)

% Initial mus and sigmas
% Format: row vectors of length n_levels
% For all but the first level, this is usually best
% kept fixed to 1 (determines origin on x_i-scale).
c.mux_0mu = [99991, 1];
c.mux_0sa = [    0, 0];

c.logsax_0mu = [log(3), log(0.1)];
c.logsax_0sa = [      0,       0];

c.mua_0mu = [log(1), 1];
c.mua_0sa = [   0.1, 0];

c.logsaa_0mu = [log(3), log(0.1)];
c.logsaa_0sa = [      0,       0];

% Kappas
% Format: row vector of length n_levels-1 (except kappa_u: scalar)
% This should be fixed (preferably to 1) if the observation model
% does not use mu_i+1 (kappa then determines the scaling of x_i+1).
c.logkaumu = log(1);
c.logkausa = 0;

c.logkaxmu = [log(1)];
c.logkaxsa = [     0];

c.logkaamu = [log(1)];
c.logkaasa = [     0];

% Omegas
% Format: row vector of length n_levels (except omega_u: scalar)
c.omumu = 0;
c.omusa = 0;

c.omxmu = [  0,  -7];
c.omxsa = [5^2,   1];

c.omamu = [  0,  -7];
c.omasa = [5^2,   1];

% Gather prior settings in vectors
c.priormus = [
    c.mux_0mu,...
    c.logsax_0mu,...
    c.mua_0mu,...
    c.logsaa_0mu,...
    c.logkaumu,...
    c.logkaxmu,...
    c.logkaamu,...
    c.omumu,...
    c.omxmu,...
    c.omamu,...
         ];

c.priorsas = [
    c.mux_0sa,...
    c.logsax_0sa,...
    c.mua_0sa,...
    c.logsaa_0sa,...
    c.logkausa,...
    c.logkaxsa,...
    c.logkaasa,...
    c.omusa,...
    c.omxsa,...
    c.omasa,...
         ];

% Check whether we have the right number of priors
expectedLength = 8*c.n_levels;
if length([c.priormus, c.priorsas]) ~= 2*expectedLength;
    error('tapas:hgf:PriorDefNotMatchingLevels', 'Prior definition does not match number of levels.')
end

% Model function handle
c.prc_fun = @tapas_hgf_jget;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_hgf_jget_transp;

return;


function [ bboxes ] = scale_bboxes( bboxes, scale_factor )
% scale_bboxes: it scales the set bounding boxes bboxes by the scale_factor
% factor.
%
% INPUT:
% 1) bboxes: a N x 4 array with the input bounding box coordinates in the
% form of [x0,y0,x1,y1] (where (x0,y0) is the top-left corner and (x1,y1)
% the bottom left corner)
% 2) scale_factor: a 1 x 1 or 2 x 1 array with the scaling factor of the
% bounding boxes. If scale_factor is a 1 x 1 array then the same scaling
% factor will be applied on both the x and y axis. If scale_factor is a
% 2 x 1 (or 1 x 2) array then the bounding boxes will be scaled across the
% y dimension by scale_factor(1) and across the x dimension by
% scale_factor(2).
%
% OUTPUT:
% 1) bboxes: a N x 4 array with the output bounding box coordinates in the
% form of [x0,y0,x1,y1] (where (x0,y0) is the top-left corner and (x1,y1)
% the bottom left corners.
%
% This file is part of the code that implements the following paper:
% Title      : "LocNet: Improving Localization Accuracy for Object Detection"
% Authors    : Spyros Gidaris, Nikos Komodakis
% Institution: Universite Paris Est, Ecole des Ponts ParisTech
% ArXiv link : http://arxiv.org/abs/1511.07763
% code       : https://github.com/gidariss/LocNet
%
% AUTORIGHTS
% --------------------------------------------------------
% Copyright (c) 2016 Spyros Gidaris
%
% Title     : "LocNet: Improving Localization Accuracy for Object Detection"
% ArXiv link: http://arxiv.org/abs/1511.07763
% Licensed under The MIT License [see LICENSE for details]
% ---------------------------------------------------------

if numel(scale_factor) == 1, scale_factor(2) = scale_factor(1); end
assert(numel(scale_factor) == 2);
scale_factor = single(scale_factor);

bboxes_center      = [(bboxes(:,1)+bboxes(:,3)), (bboxes(:,2)+bboxes(:,4))]/2;
bboxes_width_half  = (bboxes(:,3) - bboxes(:,1))/2;
bboxes_width_half  = bboxes_width_half * scale_factor(2);

bboxes_height_half = (bboxes(:,4) - bboxes(:,2))/2;
bboxes_height_half = bboxes_height_half * scale_factor(1);

bboxes = round([bboxes_center(:,1) - bboxes_width_half, ...
                bboxes_center(:,2) - bboxes_height_half, ...
                bboxes_center(:,1) + bboxes_width_half, ...
                bboxes_center(:,2) + bboxes_height_half]);
end


function m_demo(index)
% M_DEMO  Demonstration program showing various maps in M_Map package
%         Dig into this to look for examples of things you want to do.
%
%         M_DEMO runs all the demos.
%         M_DEMO(NUM) runs examples NUM (1<=NUM<=10), pausing between examples.
%
%         Some demos may require you to install GSHHS or TerrainBase datafiles
%         (see documentation)

% Rich Pawlowicz (rich@ocgy.ubc.ca) 7/May/1997
% (thanks to Art Newhall for putting these examples into an m-file,
% and the Chuck Denham for enhancing the interface)
%
% 27/July/98 - more examples.
% 17/Aug/98     "
% 15/Nov/98  - another example, better interface.
% 23/Dec/98  - another example.

%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.

N_EXAMPLES=11;

if nargin==0
 index=1:N_EXAMPLES;
end

for i=index

clf;
switch i

  case 1

    m_proj('ortho','lat',48','long',-123');
    m_coast('patch','r');
    m_grid('linest','-','xticklabels',[],'yticklabels',[],'ytick',[-80:40:80]);
    xlabel('Orthographic Projection','visible','on');

  case 2

    m_proj('lambert','long',[-160 -40],'lat',[30 80]);
    m_coast('patch',[1 .85 .7]);
    m_elev('contourf',[500:500:6000]);
    m_grid('box','fancy','tickdir','in');
    colormap(flipud(copper));
    xlabel('Conic Projection of North America with elevations','visible','on');

  case 3

    m_proj('stereographic','lat',90,'long',30,'radius',25);
    m_elev('contour',[-3500:1000:-500],'edgecolor','b');
    m_grid('xtick',12,'tickdir','out','ytick',[70 80],'linest','-');
    m_coast('patch',[.7 .7 .7],'edgecolor','r');
    xlabel('Polar Stereographic Projection with bathymetry','visible','on');

  case 4

    subplot(211);
    Slongs=[-100 0;-75 25;-5 45; 25 145;45 100;145 295;100 290];
    Slats= [  8 80;-80  8; 8 80;-80   8; 8  80;-80   0;  0  80];
    for l=1:7
     m_proj('sinusoidal','long',Slongs(l,:),'lat',Slats(l,:));
     m_grid('fontsize',6,'xticklabels',[],'xtick',[-180:30:360],...
            'ytick',[-80:20:80],'yticklabels',[],'linest','-','color',[.9 .9 .9]);
     m_coast('patch','g');
    end
    xlabel('Interrupted Sinusoidal Projection of World Oceans');
    % In order to see all the maps we must undo the axis limits set by m_grid calls:
    set(gca,'xlimmode','auto','ylimmode','auto');

    subplot(212);
    Slongs=[-100 43;-75 20; 20 145;43 100;145 295;100 295];
    Slats= [  0  90;-90  0;-90   0; 0  90;-90   0;  0  90];
    for l=1:6
     m_proj('mollweide','long',Slongs(l,:),'lat',Slats(l,:));
     m_grid('fontsize',6,'xticklabels',[],'xtick',[-180:30:360],...
            'ytick',[-80:20:80],'yticklabels',[],'linest','-','color','k');
     m_coast('patch',[.6 .6 .6]);
    end
    xlabel('Interrupted Mollweide Projection of World Oceans');
    set(gca,'xlimmode','auto','ylimmode','auto');

  case 5

    %% Nice looking data
    [lon,lat]=meshgrid([-136:2:-114],[36:2:54]);
    u=sin(lat/6);
    v=sin(lon/6);

    m_proj('oblique','lat',[56 30],'lon',[-132 -120],'aspect',.8);

    subplot(121);
    m_coast('patch',[.9 .9 .9],'edgecolor','none');
    m_grid('tickdir','out','yaxislocation','right',...
	   'xaxislocation','top','xlabeldir','end','ticklen',.02);
    hold on;
    m_quiver(lon,lat,u,v);
    xlabel('Simulated surface winds');

    subplot(122);
    m_coast('patch',[.9 .9 .9],'edgecolor','none');
    m_grid('tickdir','out','yticklabels',[],...
	   'xticklabels',[],'linestyle','none','ticklen',.02);
    hold on;
    [cs,h]=m_contour(lon,lat,sqrt(u.*u+v.*v));
    clabel(cs,h,'fontsize',8);
    xlabel('Simulated something else');

  case 6

    % Plot a circular orbit
    lon=[-180:180];
    lat=atan(tan(60*pi/180)*cos((lon-30)*pi/180))*180/pi;

    m_proj('miller','lat',82);
    m_coast('color',[0 .6 0]);
    m_line(lon,lat,'linewi',3,'color','r');
    m_grid('linest','none','box','fancy','tickdir','out');

  case 7

    m_proj('lambert','lon',[-10 20],'lat',[33 48]);
    m_tbase('contourf');
    m_grid('linestyle','none','tickdir','out','linewidth',3);
    colormap(jet);

  case 8

    m_vec;

  case 9

    % Example showing the default coastline and all of the GSHHS coastlines.

    axes('position',[.35 .6 .37 .37]);
    m_proj('albers equal-area','lat',[40 60],'long',[-90 -50],'rect','on');
    m_coast('patch',[0 1 0]);
    m_grid('linest','none','linewidth',2,'tickdir','out','xaxisloc','top','yaxisloc','right');
    m_text(-69,41,'Standard coastline','color','r','fontweight','bold');

    axes('position',[.09 .5 .37 .37]);
    m_proj('albers equal-area','lat',[40 54],'long',[-80 -55],'rect','on');
    m_gshhs_c('patch',[.2 .8 .2]);
    m_grid('linest','none','linewidth',2,'tickdir','out','xaxisloc','top');
    m_text(-80,52.5,'GSHHS\_C (crude)','color','m','fontweight','bold','fontsize',14);

    axes('position',[.13 .2 .37 .37]);
    m_proj('albers equal-area','lat',[43 48],'long',[-67 -59],'rect','on');
    m_gshhs_l('patch',[.4 .6 .4]);
    m_grid('linest','none','linewidth',2,'tickdir','out');
    m_text(-66.5,43.5,'GSHHS\_L (low)','color','m','fontweight','bold','fontsize',14);

    axes('position',[.35 .05 .37 .37]);
    m_proj('albers equal-area','lat',[45.8 47.2],'long',[-64.5 -62],'rect','on');
    m_gshhs_i('patch',[.5 .6 .5]);
    m_grid('linest','none','linewidth',2,'tickdir','out','yaxisloc','right');
    m_text(-64.4,45.9,'GSHHS\_I (intermediate)','color','m','fontweight','bold','fontsize',14);

    axes('position',[.55 .23 .37 .37]);
    m_proj('albers equal-area','lat',[46.375 46.6],'long',[-64.2 -63.7],'rect','on');
    m_gshhs_h('patch',[.6 .6 .6]);
    m_grid('linest','none','linewidth',2,'tickdir','out','xaxisloc','top','yaxisloc','right');
    m_text(-64.18,46.58,'GSHHS\_H (high)','color','m','fontweight','bold','fontsize',14);

  case 10

    % Example showing a trackline plot

    clf
    m_proj('UTM','long',[-72 -68],'lat',[40 44]);
    m_gshhs_i('color','k');
    m_grid('box','fancy','tickdir','in');

    % fake up a trackline
    lons=[-71:.1:-67];
    lats=60*cos((lons+115)*pi/180);
    dates=datenum(1997,10,23,15,1:41,zeros(1,41));

    m_track(lons,lats,dates,'ticks',0,'times',4,'dates',8,...
           'clip','off','color','r','orient','upright');

  case 11

    % example showing range rings

    clf
    m_proj('hammer','clong',170);
    m_grid('xtick',[],'ytick',[],'linestyle','-');
    m_coast('patch','g');
    m_line(100.5,13.5,'marker','square','color','r');
    m_range_ring(100.5,13.5,[1000:1000:15000],'color','b','linewi',2);
    xlabel('1000km range rings from Bangkok');

  end

 if i<length(index)
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   disp('  hit return to continue');
   pause
   disp('        ...drawing');
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 end
end


function plotMicArray(mic_dirs_deg, R)
% PLOTMICARRAY plots the arrangement of the microphones in a spherical array
%
%   mic_dirs:   the directions of the microphones in [azi1 elev1; ...]
%       convention
%   R:          radius of the array
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% PLOTMICARRAY.M - 11/7/2013
% Archontis Politis, archontis.politis@aalto.fi
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

mic_dirs_rad = mic_dirs_deg*pi/180;
Nmic = size(mic_dirs_deg,1);

hold on
% set up unit sphere information
numSphereFaces = 20;
[unitSphereX, unitSphereY, unitSphereZ] = sphere(numSphereFaces);
% radius of each sphere
spheresRadius = ones(Nmic,1)*0.05*R;
%plot 3d axes
line([0 2*R],[0 0], [0 0],'color','r');
text(2*R,0,0,'x','Color','r','FontSize',24);
line([0 0],[0 2*R], [0 0],'color','g');
text(0,2*R,0,'y','Color','g','FontSize',24);
line([0 0],[0 0], [0 2*R],'color','b');
text(0,0,2*R,'z','Color','b','FontSize',24);

spheresX = R*cos(mic_dirs_rad(:,1)).*cos(mic_dirs_rad(:,2));
spheresY = R*sin(mic_dirs_rad(:,1)).*cos(mic_dirs_rad(:,2));
spheresZ = R*sin(mic_dirs_rad(:,2));
% for each given sphere, shift the scaled unit sphere by the
% location of the sphere and plot
for i=1:Nmic
    sphereX = spheresX(i) + unitSphereX*spheresRadius(i);
    sphereY = spheresY(i) + unitSphereY*spheresRadius(i);
    sphereZ = spheresZ(i) + unitSphereZ*spheresRadius(i);
    h = surf(sphereX, sphereY, sphereZ);
    set(h, 'FaceColor','b', 'FaceLighting', 'gouraud')

    text(1.1*spheresX(i),1.1*spheresY(i),1.1*spheresZ(i), num2str(i),'color','w','FontSize',24)
end
sphereX = unitSphereX*R;
sphereY = unitSphereY*R;
sphereZ = unitSphereZ*R;
h = surf(sphereX, sphereY, sphereZ);
set(h, 'FaceColor','c', 'FaceLighting', 'gouraud')
light('Position',[0 0 1],'Style','infinite');
material shiny
hold off
axis equal
grid on
view(-30,15)
set(gca,'visible','off')


classdef testHomogenizationLaminateForAnisotropicTensors < ...
         testShowingError

     properties (Access = protected)
         testName = 'HomogenizationLaminateForAnisotropicTensors';
         tol = 1e-6;
     end

     properties (Access = private)
         stiffTensor
         weakTensor
         ChForIso
         ChForAni
         laminateDirection
         theta
     end

     methods (Access = public)

         function obj = testHomogenizationLaminateForAnisotropicTensors()
             obj.init()
             obj.computeHomogenizerForIsotropicMaterials()
             obj.computeHomogenizerForAnisotropicMaterials()
         end

     end

     methods (Access = protected)

         function init(obj)
             obj.createTensors()
             obj.createLaminateDirection()
             obj.theta = 0.8;
         end

        function createLaminateDirection(obj)
            d = [0 1 0];
            dir = Vector3D;
            dir.setValue(d);
            dir.normalize()
            obj.laminateDirection = dir;
        end

         function createTensors(obj)
            E1 = 1;
            E0 = 1e-3;
            nu1 = 1/3;
            nu0 = 1/3;
            obj.stiffTensor = IsotropicConstitutiveTensor(E1,nu1);
            obj.weakTensor  = IsotropicConstitutiveTensor(E0,nu0);
         end

         function computeHomogenizerForIsotropicMaterials(obj)
            C0 = obj.weakTensor;
            C1 = obj.stiffTensor;
            dir{1} = obj.laminateDirection;
            m1 = 1;
            SeqHomog = VoigtPlaneStressHomogHomogenizer(C0,C1,dir,m1,obj.theta);
            obj.ChForIso  = SeqHomog.getPlaneStressHomogenizedTensor();
         end

         function computeHomogenizerForAnisotropicMaterials(obj)
            C0 = obj.weakTensor;
            C1 = obj.stiffTensor;
            dir = obj.laminateDirection;
            Lam = AnisotropicLaminateHomogenizer(C0,C1,dir,obj.theta);
            obj.ChForAni = Lam.getHomogenizedTensor();
         end

         function computeError(obj)
             Ch4Iso = obj.ChForIso.getValue();
             Ch4Ani = obj.ChForAni.getValue();
             obj.error = norm(Ch4Iso - Ch4Ani);
         end

     end

end


function plot_histogram (samples, pd, x_label, x_title)
% plot_histogram: plots histogram from samples (empirical PDF)  and
% compares to inferred probability density function (reference PDF). It
% also plots mean and median.
%
% INPUT
%   samples: Nx1 samples.
%   pd: probality distribution object from ProbabilityDistribution class.
%   x_label: label for X axis (string).
%   x_title: title for the figure (string).
%
% OUTPUT
%   figure with histogram, reference PDF, mean and median.
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% Reference:
%
%
% Version: 004
% Date:    2021/03/02
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego`

N = length(samples);

%% REFERENCE PDF

x = linspace(min(samples), max(samples), N );
ref_pdf = pdf(pd, x);

%% STATISTIC ANALYSIS

mu = mean(samples);
med = median(samples);

i = -5;
idx1 = [];
idx2 = [];

while (isempty(idx1) || isempty(idx2))

    EPS = 10^i;
    idx1 = find( x >= mu - EPS   & x < mu + EPS );
    idx2 = find( x >= med - EPS & x < med + EPS );
    i = i + 1;
end

if ( isempty(idx1) || isempty(idx2) )
    error('plot_histogram: no match for mean or median')
end

% Middlepoints
idx1 = idx1( ceil(end/2) );
idx2 = idx2( ceil(end/2) );

%% PLOT

bins = 100;
blue_new = [0 0.4470 0.7410];
orange_new = [0.8500 0.3250 0.0980];

figure

% Plot histogram from dataquiq
histogram(samples, bins, 'Normalization', 'pdf', 'FaceColor', [.9 .9 .9]);
hold on

% Plot the reference pdf
p0 = plot(x, ref_pdf, '-',  'LineWidth', 2);

% Plot lines
y = ref_pdf (idx1);
l1 = line( [mu, mu] , [0, y], 'Color', blue_new, 'LineWidth', 2, 'LineStyle','-.');

y = ref_pdf (idx2);
l2 = line( [med, med] , [0, y], 'Color', orange_new, 'LineWidth', 2, 'LineStyle','-.' );

legend([p0, l1, l2], 'Reference PDF', 'Mean', 'Median')

xlabel(x_label);
% ylabel('Probability density function (PDF)');
title(x_title)

grid on
hold off

end


classdef LSMOP8 < PROBLEM
% <multi/many> <real> <large/none>
% Large-scale benchmark MOP
% nk --- 5 --- Number of subcomponents in each variable group

%------------------------------- Reference --------------------------------
% R. Cheng, Y. Jin, and M. Olhofer, Test problems for large-scale
% multiobjective and many-objective optimization, IEEE Transactions on
% Cybernetics, 2017, 47(12): 4108-4121.
%------------------------------- 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(Access = private)
        nk = 5; % Number of subcomponents in each variable group
        sublen;	% Number of variables in each subcomponent
        len;    % Cumulative sum of lengths of variable groups
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            % Parameter setting
            obj.nk = obj.ParameterSet(5);
            if isempty(obj.M); obj.M = 3; end
            if isempty(obj.D); obj.D = 100*obj.M; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = [ones(1,obj.M-1),10.*ones(1,obj.D-obj.M+1)];
            obj.encoding = ones(1,obj.D);
            % Calculate the number of variables in each subcomponent
            c = 3.8*0.1*(1-0.1);
            for i = 1 : obj.M-1
                c = [c,3.8.*c(end).*(1-c(end))];
            end
            obj.sublen = floor(c./sum(c).*(obj.D-obj.M+1)/obj.nk);
            obj.len    = [0,cumsum(obj.sublen*obj.nk)];
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            [N,D] = size(PopDec);
            M     = obj.M;
            PopDec(:,M:D) = (1+repmat(cos((M:D)./D*pi/2),N,1)).*PopDec(:,M:D) - repmat(PopDec(:,1)*10,1,D-M+1);
            G = zeros(N,M);
            for i = 1 : 2 : M
                for j = 1 : obj.nk
                    G(:,i) = G(:,i) + Griewank(PopDec(:,obj.len(i)+M-1+(j-1)*obj.sublen(i)+1:obj.len(i)+M-1+j*obj.sublen(i)));
                end
            end
            for i = 2 : 2 : M
                for j = 1 : obj.nk
                    G(:,i) = G(:,i) + Sphere(PopDec(:,obj.len(i)+M-1+(j-1)*obj.sublen(i)+1:obj.len(i)+M-1+j*obj.sublen(i)));
                end
            end
            G      = G./repmat(obj.sublen,N,1)./obj.nk;
            PopObj = (1+G+[G(:,2:end),zeros(N,1)]).*fliplr(cumprod([ones(N,1),cos(PopDec(:,1:M-1)*pi/2)],2)).*[ones(N,1),sin(PopDec(:,M-1:-1:1)*pi/2)];
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,obj.M);
            R = R./repmat(sqrt(sum(R.^2,2)),1,obj.M);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            if obj.M == 2
                R = obj.GetOptimum(100);
            elseif obj.M == 3
                a = linspace(0,pi/2,10)';
                R = {sin(a)*cos(a'),sin(a)*sin(a'),cos(a)*ones(size(a'))};
            else
                R = [];
            end
        end
    end
end

function f = Griewank(x)
    f = sum(x.^2,2)./4000 - prod(cos(x./repmat(sqrt(1:size(x,2)),size(x,1),1)),2) + 1;
end

function f = Sphere(x)
    f = sum(x.^2,2);
end


function e = trimSegment(s,imSize,mrg)

% TRIMSEGMENT  Trim segment at image borders
%   TRIMSEGMENT(S,IMSIZE) trims the segment S at the image borders
%   specified by IMSIZE. S is a 4-vector containing the two segment's
%   endpoints. IMSIZE is a 2-vector with the image dimensions in pixels,
%   IMSIZE = [HSIZE,VSIZE]. The output segment has the same orientation as
%   the input one.
%
%   TRIMSEGMENT(...,MRG) restricts the image size to be smaller in MRG
%   pixels at its four borders.
%
%   See also PINHOLESEGMENT.

%   (c) 2008-2009 Joan Sola @ LAAS-CNRS

% input segment's endpoints
a = s(1:2);
b = s(3:4);

% image witdh and height
[w,h] = split(imSize);

if nargin < 3
    insq = inSquare([a b],[0 w 0 h]);
else
    insq = inSquare([a b],[0 w 0 h],mrg);
end

if all(insq) % both endpoints are in the image

    e = s; % return the segment unchanged

else % at least one endpoint is out of the image

    H = pp2hmgLin(a,b); % homogeneous line
    L = [1; 0; 0];  % left image border
    R = [1; 0;-w];  % right
    T = [0; 1; 0];  % top
    B = [0; 1;-h];  % bottom

    % intersections of infinite line with infinite borders
    HL = intersectHmgLin(H,L,1);
    HR = intersectHmgLin(H,R,1);
    HT = intersectHmgLin(H,T,1);
    HB = intersectHmgLin(H,B,1);

    % bring to image borders
    i = 1;
    if inInterval(HL(2),[0,h])
        e(i:i+1,1) = HL;
        i = 3;
    end
    if inInterval(HR(2),[0,h])
        e(i:i+1,1) = HR;
        i = 3;
    end
    if inInterval(HT(1),[0,w])
        e(i:i+1,1) = HT;
        i = 3;
    end
    if inInterval(HB(1),[0,w])
        e(i:i+1,1) = HB;
    end

    if insq(1) % endpoint a is in the image

        p = e(1:2);
        q = e(3:4);

        u = b - a;
        v = p - a;
        if any(u./v > 0)
            e(1:2) = a;
            e(3:4) = p;
        else
            e(1:2) = a;
            e(3:4) = q;
        end

    elseif insq(2) % endpoint b is in the image

        p = e(1:2);
        q = e(3:4);

        u = a - b;
        v = p - b;
        if any(u./v > 0)
            e(1:2) = p;
            e(3:4) = b;
        else
            e(1:2) = q;
            e(3:4) = b;
        end

    else % no endpoint is inside the image

        if i == 1 % no intersection with image borders
            % Segment is not visible
            e = [];

        else
            p = e(1:2);
            q = e(3:4);

            if i==3 && any((p-a)./(b-p) > 0)  % segment is visible
                % check orientations

                u = b - a;
                v = q - p;
                if any(u./v < 0)
                    e = e([3 4 1 2]); % match orientations
                end
            else % segment is not visible
                e = [];
            end
        end

    end
end

return

%% test
imsize = [10 10];
s{1}  = [1 2 3 4]';
s{2}  = [-1 2 3 4]';
s{3}  = [1 -2 3 4]';
s{4}  = [1 2 -3 4]';
s{5}  = [1 2 3 -4]';
s{6}  = [1 2 3 11]';
s{7}  = [1 2 11 4]';
s{8}  = [1 11 3 4]';
s{9}  = [11 2 3 4]';
s{10} = [-1 -2 13 14]';
s{11} = [13 14 -1 -2]';

for i=1:numel(s)
    s{i}'
    (trimSegment(s{i},imsize))'
end

%% test
lmin = 10;
mrg = 0;
imSize = [640;480];
cla
lh = line('color','c','linewidth',3);
yh = line('color','r','linestyle','--')

%%
s = randn(4,1)*600

set(lh,'xdata',s([1,3]),'ydata',s([2,4]))

sv = trimSegment(s,imSize)

if isempty(sv)
    set(yh,'xdata',[],'ydata',[])
else
    set(yh,'xdata',sv([1,3]),'ydata',sv([2,4]))
end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [y,nz] = ompdenoise1(params,msgdelta)
%OMPDENOISE1 OMP denoising of 1-D signals.
%  OMPDENOISE1 denoises a 1-dimensional signal using OMP denoising. The
%  function syntax is identical to OMPDENOISE, but it runs significantly
%  faster on 1-D signals. OMPDENOISE1 requires somewhat more memory than
%  OMPDENOISE (approximately the size of the input signal), so if memory is
%  limited, OMPDENOISE can be used instead.
%
%  See also OMPDENOISE.

%  Ron Rubinstein
%  Computer Science Department
%  Technion, Haifa 32000 Israel
%  ronrubin@cs
%
%  August 2009

% parse input arguments %

x = params.x(:);
D = params.dict;
blocksize = params.blocksize;

% maxval %
if (isfield(params,'maxval'))
  maxval = params.maxval;
else
  maxval = 1;
end

% gain %
if (isfield(params,'gain'))
  gain = params.gain;
else
  gain = 1.15;
end

% maxatoms %
if (isfield(params,'maxatoms'))
  maxatoms = params.maxatoms;
else
  maxatoms = floor(blocksize/2);
end

% stepsize %
if (isfield(params,'stepsize'))
  stepsize = params.stepsize;
else
  stepsize = 1;
end
if (any(stepsize<1))
  error('Invalid step size.');
end

% noise mode %
if (isfield(params,'noisemode'))
  switch lower(params.noisemode)
    case 'psnr'
      sigma = maxval / 10^(params.psnr/20);
    case 'sigma'
      sigma = params.sigma;
    otherwise
      error('Invalid noise mode specified');
  end
elseif (isfield(params,'sigma'))
  sigma = params.sigma;
elseif (isfield(params,'psnr'))
  sigma = maxval / 10^(params.psnr/20);
else
  error('Noise strength not specified');
end

% lambda %
if (isfield(params,'lambda'))
  lambda = params.lambda;
else
  lambda = maxval/(10*sigma);
end

% msgdelta %
if (nargin <2)
  msgdelta = 5;
end
if (msgdelta<=0)
  msgdelta = -1;
end

epsilon = sqrt(blocksize) * sigma * gain;   % target error for omp

MEM_LOW = 1;
MEM_NORMAL = 2;
MEM_HIGH = 3;

if (isfield(params,'memusage'))
  switch lower(params.memusage)
    case 'low'
      memusage = MEM_LOW;
    case 'normal'
      memusage = MEM_NORMAL;
    case 'high'
      memusage = MEM_HIGH;
    otherwise
      error('Invalid memory usage mode');
  end
else
  memusage = MEM_NORMAL;
end

% compute G %

G = [];
if (memusage >= MEM_NORMAL)
  G = D'*D;
end

% verify dictionary normalization %

if (isempty(G))
  atomnorms = sum(D.*D);
else
  atomnorms = diag(G);
end
if (any(abs(atomnorms-1) > 1e-2))
  error('Dictionary columns must be normalized to unit length');
end

% denoise the signal %

% process the signal in batches to conserve memory
% choose batchsize so im2colstep returns a matrix of approximately the same
% size as the signal
batchsize = ceil(length(x)*stepsize/blocksize + blocksize);

y = zeros(size(x));
ids = 1:min(batchsize,length(x));
nz = 0;

blocknum = floor((length(x)-blocksize)/stepsize) + 1;
processedblocks = 0;
tid = timerinit('ompdenoise', blocknum);
while (length(ids)>=blocksize)

  % extract the signal blocks
  blocks = im2colstep(x(ids),[blocksize 1],[stepsize 1]);

  % remove DC
  [blocks, dc] = remove_dc(blocks,'columns');

  % denoise the blocks
  if (memusage == MEM_LOW)
    gamma = omp2(D,blocks,[],epsilon,'maxatoms',maxatoms,'checkdict','off');
  else
    gamma = omp2(D'*blocks,sum(blocks.*blocks),G,epsilon,'maxatoms',maxatoms,'checkdict','off');
  end
  nz = nz + nnz(gamma);
  cleanblocks = add_dc(D*gamma, dc, 'columns');

  y(ids) = y(ids) + col2imstep(cleanblocks, [length(ids) 1], [blocksize 1], [stepsize 1]);
  ids = ids + floor((batchsize-blocksize)/stepsize)*stepsize + stepsize;
  if (ids(end)>length(x))
    ids = ids(ids<=length(x));
  end

  % display status
  if (msgdelta>0)
    processedblocks = processedblocks + size(blocks,2);
    timereta(tid, processedblocks, msgdelta);
  end

end

if (msgdelta>0)
  timereta(tid, blocknum);
end

cnt = countcover(size(x),[blocksize 1],[stepsize 1]);
y = (y+lambda*x)./(cnt + lambda);
y = reshape(y,size(params.x));


function y = solveLinearSystem(mode,A,b,para);

switch mode,
  case {'matlab','MATLAB'},
    y = A\b;
%     file = createFileName('prefix','JL');
%     save(file,'A','b');

  case 'MG',
    para.MGlevel      = log2(para.m(1))+1;
    para.MGcycle      = 1;
    para.MGomega      = 0.5;   %% !!! 0.5 should be better
    para.MGsmoother   = 'mfJacobi';
    para.MGpresmooth  = 3;
    para.MGpostsmooth = 1;
    para.dim = length(para.Omega);
    u = zeros(size(b));
    [y,res,r] = mfvcycle(para,u,b,1e-12,para.MGlevel,(max(para.m)>32));
end;


function [p] = eeg_linfit(p)

% eeg_linfit - returns slope/intercept of linear fit
%
% USEAGE: [p] = eeg_linfit(p)
%
% p is the eeg_toolbox struct.  For this function, the fields
% required are:
%
%   p.volt.data - voltage data matrix (Npoints,Nelec)
%   p.volt.timeArray - voltage sample points (msec)
%
% Returns slope and intercept matrices (Npoints,Nelec)
% for linear fit of each electrode into fields of p:
%
%   p.volt.fitslope
%   p.volt.fitintercept
%
% The column vectors of slope/intercept hold constant
% values.  So, the linear fit data can be generated by:
%
% y = p.volt.fitslope .* p.volt.timeArray + p.volt.fitintercept;
%
% Where necessary, the p.volt.timeArray is replicated
% across columns for each electrode to enable this calculation
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:52 $

% Licence:  GNU GPL, no implied or express warranties
% History:  07/00, Darren.Weber_at_radiology.ucsf.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% volt.timeArray is essentially a column vector, but
% operations in the eeg_toolbox may replicate across N columns
time = p.volt.timeArray(:,1);

slope = zeros(1,size(p.volt.data,2));
intercept = slope;

for elec = 1:size(p.volt.data,2),

    y = polyfit(time,p.volt.data(:,elec),1);
    slope(1,elec) = y(1);
    intercept(1,elec) = y(2);
end

% make sure that volt.timeArray is same size as volt.data
if ~isequal(size(p.volt.timeArray),size(p.volt.data)),
    p.volt.timeArray = repmat(p.volt.timeArray(:,1),1,size(p.volt.data,2));
end

p.volt.fitslope = repmat(slope,size(p.volt.data,1),1);
p.volt.fitintercept = repmat(intercept,size(p.volt.data,1),1);

return


function [varargout]=patchPointDist(V,F,Vp,dL)

methodOpt='near-norm';

switch methodOpt
    case 'near-norm'

        [Nn,Vn]=patchNormal(F,V); %Get face normals and normal vector origins
        [Dn,indMin]=minDist(Vp,Vn); %Get distances to origins and find closest faces

        %Attempt to find orthogonal projection to closest face
        N=Nn(indMin,:); %Order the normals
        W=Vp-Vn(indMin,:);
        Dm=(dot(N,W,2));
        D=abs(Dm);
        Dp=Dm(:,ones(1,3)).*N;
        Vpd=Vp-Dp;

        %Test if projection is valid
        TR = triangulation(F,V);
        B = cartesianToBarycentric(TR,indMin,Vpd);
        logicOutside=any(B<-dL,2);

        %Replace invalid projections with face centre points
        D(logicOutside)=Dn(logicOutside); %Replace distances
        Dp(logicOutside,:)=Vp(logicOutside,:)-Vn(indMin(logicOutside),:); %Replace difference vectors

    otherwise
        error('Wrong method option chosen');
end

switch nargout
    case 1
        varargout{1}=D;
    case 2
        varargout{1}=D;
        varargout{2}=Dp;
    case 3
        varargout{1}=D;
        varargout{2}=Dp;
        varargout{3}=logicOutside;
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


classdef prtClassMatchedSubspace < prtClass

    properties (SetAccess=private)

        name = 'Matched Subspace'
        nameAbbreviation = 'MatchedSubspace'
        isNativeMary = true;

    end

    properties
        inferH1subspace = true;
        inferH0subspace = false;

        nH1components = 2;
        nH0components = 2;

        proj1
        proj0
    end

    methods
        function self = prtClassKnn(varargin)
            self = prtUtilAssignStringValuePairs(self,varargin{:});
            self.verboseStorage = true;
        end
    end

    methods (Access=protected, Hidden = true)

        function self = trainAction(self,ds)

            if self.inferH1subspace
                ds1 = ds.retainClassesByInd(2);
                x = ds1.X;
                [u,s] = svds(x',self.nH1components);
                p = u*(u'*u)^(-1/2)*u';
            else
                p = eye(ds.nFeatures);
            end
            self.proj1 = p;

            if self.inferH0subspace
                ds0 = ds.retainClassesByInd(1);
                x = ds0.X;
                [u,s] = svds(x,self.nH0components);
                p = u*(u'*u)^(-1/2)*u';
            else
                p = eye(ds.nFeatures);
            end
            self.proj0 = p;

        end

        function yOut = runAction(self,ds)

            x = ds.X;
            h1 = diag(x*self.proj1*x');
            h0 = diag(x*self.proj0*x');
            yOut = ds;
            %             yOut.X = log(h1)-log(h0);
            h1 = abs(h1);
            h0 = abs(h0);
            yOut.X = h1./h0;

        end
    end
end


function [out_im,status ] = open_bitfield_bmp( filename )
% OPEN_BITFIELD_BMP open a bitfield compressed bitmap image.
%
%   IM = OPEN_BITFIELD_BMP( FILENAME ) opens a 16-bit bitmapped compressed file
%   named FILENAME. The output is formatted as three planes of Red, Green
%   and Blue data.
%
%   See also imread, imfinfo

% Get information about the image
info = imfinfo(filename);

% if we have a bitfield compressed R-G-B bitmap image
if ( strcmp(info.Format , 'bmp') && ...
     strcmp(info.CompressionType , 'bitfields') &&  ...
     (info.BitDepth == 16 || info.BitDepth == 32) )

    % indicate the input bmp file is not of 8-bit
    status = 0;

    % Open the file for reading
    fid = fopen(filename, 'r');

    % Extract relevvant image info
    data_offset = info.ImageDataOffset;
    width = info.Width;
    height = info.Height;

    % Create space for output image
    out_im = zeros(height, width, 3);

    % Seek to where the image data begins (i.e. skip the file header
    fseek(fid, data_offset, 'bof');

    % Read in the image data and format it into a matrix
    if (info.BitDepth == 16)
        % compressed_image = (fread(fid, [width + 1, height] , 'uint16'))';
        compressed_image = (fread(fid, [width, height] , 'uint16'))';
    else
        compressed_image = (fread(fid, [width, height] , 'uint32'))';
    end

    % Eliminate last column of junk data (scanline row terminators) in 16
    % bit mode
    if (info.BitDepth == 16)
        compressed_image = compressed_image(:, 1:(width));
    end;

    % Invert Row Order since it is flipped
    new_image = flipud(compressed_image);

    % Extract color bitmasks to decompress
    red_mask = info.RedMask;
    blue_mask = info.BlueMask;
    green_mask = info.GreenMask;

    % Extract color components and form output image
    out_im(:,:,1) = (bitand(new_image, red_mask) / red_mask * 255);
    out_im(:,:,2) = (bitand(new_image, green_mask) / green_mask * 255);
    out_im(:,:,3) = (bitand(new_image, blue_mask) / blue_mask * 255);

    % typecast
    out_im = uint8(out_im);

    % display image
    %imshow(out_im);

else

    % indicate the input bmp file is 8-bit
    status = 1;

    % general imread for all other cases
    out_im = imread(filename);
    %imshow(out_im);

end


function Whitener = bst_whitener(NoiseCov, ChannelFile, DataTypes, ChannelFlag)
% BST_WHITENER: Compute a whitener from a NoiseCov matrix
%
% USAGE:  Whitener = bst_whitener(NoiseCov);
%         Whitener = bst_whitener(NoiseCov, ChannelFile, DataTypes, ChannelFlag);
%         Whitener = bst_whitener(NoiseCov,     Channel, DataTypes, ChannelFlag);

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2009-2010

% If channels definition is not provided
if (nargin < 4)
    % Use all channels
    iChan = 1:length(NoiseCov);
else
    % If channel file provided: load ChannelFile
    if ischar(ChannelFile)
        ChannelMat = in_bst_channel(ChannelFile, 'Channel');
        Channel = ChannelMat.Channel;
    % If Channel structure provided
    elseif isstruct(ChannelFile)
        Channel = ChannelFile;
    end
    % Get good channels
    iChan = good_channel(Channel, ChannelFlag, DataTypes);
end

% Detect the rows with only zero values
iZeroRow = find(sum(NoiseCov .^ 2) == 0);
% Remove those channels from list of valid channels
iChan = setdiff(iChan, iZeroRow);

% Initialize output matrix
Whitener = zeros(size(NoiseCov));
% Decomposition
[U,S] = svd(NoiseCov(iChan,iChan));
% Check matrix rank
m = length(iChan);
r = sum(diag(S) > m * S(1) * eps('double'));
if (r < m)
    error(['You have deficient data. Please:' 10 10 ...
           '1) Look for bad channels in your recordings' 10 ...
           '2) Tag them bad channels in all your recordings' 10 ...
           '3) Try automatic detection of flat channels: Right click > Good/bad channels > ...' 10 ...
           '4) Recompute the noise covariance matrix' 10, ...
           '5) Restart this proces']);
end
% Create whitener
Whitener(iChan,iChan) = pinv(U * sqrt(S));


function [m] = pm2m(pm)
% Convert length from picometers to meters.
% Chad A. Greene 2012
m = pm*1e-12;


function sysmodel = SparseRegression(Y,Yp,U,dt, options_method, lambda)

Nstates = size(Y,1);
Ninputs = size(U,1);

% Sparse regression
switch options_method.sparsify
    case 'LASSO'
        % using the l1 norm to promote sparsity
        G = zeros(Nstates+Ninputs,Nstates);
        for i = 1:Nstates
            [G(:,i), FitInfo] = lasso([Y;U]',Yp(i,:)','Lambda',lambda); %,'Lambda',0.00000001
            %     lassoPlot(G{i},FitInfo);
        end
    case 'ILSTH'
        G = sparsifyDynamics([Y;U]',Yp',lambda,Nstates);
end

G = G';
A = G(1:Nstates,1:Nstates);
B = G(1:Nstates,Nstates+1:Nstates+Ninputs);
C = eye(Nstates,Nstates);
D = zeros(Nstates,1);
sysmodel = ss(A,B,C,D,dt);
end


clear all, close all
n1=10; n2=5;                       % number of data points from each class n1/n2
sc=1;                                  % change the scale of the initial problem

% 1) draw data from two Gaussians
S1 = eye(2); S2 = [1 0.95; 0.95 1];
m1 = [0.75;  0]; m2 = [-0.75; 0];
randn('seed',17); rand('seed',17)
% coordinates
x1 = chol(S1)'*randn(2,n1)+repmat(m1,1,n1);
x2 = chol(S2)'*randn(2,n2)+repmat(m2,1,n2);
x1(:,5) = x1(:,2); x1(:,1) = x1(:,4); x2(:,1) = x2(:,2);  % rank deficiency in K
xtr = [x1 x2]';
% class labels
ytr = [repmat(-1,[1,n1]) repmat(1,[1,n2])]';
tt = (-5:.3:5)*sc; [t1 t2] = meshgrid(tt,tt);
x1 = x1*sc; x2=x2*sc; xtr=xtr*sc; m1=m1*sc; m2=m2*sc;
t  = [t1(:) t2(:)];
z1 = exp(-sum((t-repmat(m1',length(t),1))*inv(S1).*(t-repmat(m1',length(t),1)),2)/2);
z2 = exp(-sum((t-repmat(m2',length(t),1))*inv(S2).*(t-repmat(m2',length(t),1)),2)/2);
xte = t; clear t
xtr(end,:)=[2.9,2.3]; ytr(end)=-1;                                       % modif

% 2) set GP parameters
cov = {@covSum,{@covSEiso,@covNoise}}; hyp.cov = [0; 2; -Inf];       % ell,sf,sn
lik =  {'likLogistic'};  hyp.lik  = [];                  % likLogistic or likErf
mn  = {'meanZero'};      hyp.mean = [];
inf = 'infEP';

% 3) predict using approximated inference
[nlZ,dnlZ,post] = gp(hyp, inf, mn, cov, lik, xtr, ytr);
[ymu,ys2,fmu,fs2,junk,post] = gp(hyp,inf,mn,cov,lik,xtr,post,xte);

% 4) prediction using MCMC sampling
% set MCMC parameters, see some more details in inf/infMCMC.m
% We have two samplers implemented, namely
%  hmc - Hybrid Monte Carlo, and
%  ess - Elliptical Slice Sampling.
% par.sampler = 'hmc'; par.Nsample = 20;
par.sampler = 'ess'; par.Nais = 5; par.Nsample = 20; par.Nburnin = 20; par.Nskip = 2;

hyp = gpminimize(hyp,@gp,-200,0,inf,mn,cov,lik,xtr,ytr);
tic
[posts,nlZs,dnlZs] = infMCMC(hyp,mn,cov,lik,xtr,ytr,par);
posts
toc
[ymus,ys2s,fmus,fs2s,junk,posts] = gp(hyp,@infMCMC,mn,cov,lik,xtr,posts,xte);

% 5a) echo results
fprintf('nlZ-EP=%f, nlZ-AIS=%f\n', nlZ, nlZs)
fprintf('acceptance rate (MCMC) = %1.2f%%\n',100*posts.acceptance_rate_MCMC)
for r=1:length(posts.acceptance_rate_AIS)
  fprintf('acceptance rate (AIS) = %1.2f%%\n',100*posts.acceptance_rate_AIS(r))
end
% 5b) print results
figure
subplot(221)
  plot([-12,12],[-12,12],'r'), hold on, plot(fmus,fmu,'k.'), title('\mu_f')
  xlabel('MCMC'), ylabel(inf)
subplot(222)
  plot([0,10],[0,10],'r'), hold on, plot(sqrt(fs2s),sqrt(fs2),'k.')
  title('\sigma_f')
  xlabel('MCMC'), ylabel(inf)
subplot(223)
  plot([-1,1],[-1,1],'r'), hold on, plot(ymus,ymu,'k.'), title('\mu_y')
  xlabel('MCMC'), ylabel(inf)
subplot(224)
  plot([0,1],[0,1],'r'), hold on, plot(sqrt(ys2s),sqrt(ys2),'k.')
  title('\sigma_y')
  xlabel('MCMC'), ylabel(inf)


function blas2_test01 ( )

%*****************************************************************************80
%
%% BLAS2_TEST01 tests DGEMV.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 February 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BLAS2_TEST01\n' );
  fprintf ( 1, '  For a general matrix A,\n' );
  fprintf ( 1, '  DGEMV computes y := alpha * A * x + beta * y\n' );
  fprintf ( 1, '  or             y := alpha * A'' * x + beta * y.\n' );
%
%  y = alpha * A * x + beta * y
%
  trans = 'N';
  m = 5;
  n = 4;
  alpha = 2.0;
  lda = m;
  a = r8mat_test ( trans, lda, m, n );
  x = zeros ( n, 1 );
  for i = 1 : n
    x(i) = i;
  end
  incx = 1;
  beta = 3.0;
  y = zeros ( m, 1 );
  for i = 1 : m
    y(i) = 10 * i;
  end
  incy = 1;

  r8mat_print ( m, n, a, '  Matrix A:' );
  r8vec_print ( n, x, '  Vector X:' );
  r8vec_print ( m, y, '  Vector Y:' );

  y = dgemv ( trans, m, n, alpha, a, lda, x, incx, beta, y, incy );

  r8vec_print ( m, y, '  Result Y = alpha * A  * x + beta * y' );
%
%  y = alpha * A' * x + beta * y
%
  trans = 'T';
  m = 5;
  n = 4;
  alpha = 2.0;
  lda = m;
  a = r8mat_test ( trans, lda, n, m );
  x = zeros ( m, 1 );
  for i = 1 : m
    x(i) = i;
  end
  incx = 1;
  beta = 3.0;
  y = zeros ( n, 1 );
  for i = 1 : n
    y(i) = 10 * i;
  end
  incy = 1;

  r8mat_print ( m, n, a, '  Matrix A:' );
  r8vec_print ( m, x, '  Vector X:' );
  r8vec_print ( n, y, '  Vector Y:' );

  y = dgemv ( trans, m, n, alpha, a, lda, x, incx, beta, y, incy );

  r8vec_print ( n, y, '  Result Y = alpha * A'' * x + beta * y' );

  return
end


function T = sptoeplitz(col, row)
%SPTOEPLITZ   Sparse Toeplitz matrix.
%   SPTOEPLITZ(C, R) produces a sparse nonsymmetric Toeplitz matrix having
%   C as its first column and R as its first row. Neither C nor R needs to
%   be sparse. No full-size dense matrices are formed.
%
%   SPTOEPLITZ(R) is a sparse symmetric/Hermitian Toeplitz matrix.
%
%   Examples:
%     sptoeplitz( real( (1i).^(0:8) ) )   % 9x9, 41 nonzeros
%     sptoeplitz( [-2 1 zeros(1,9998)] ); % classic 2nd difference
%
% See also TOEPLITZ, SPDIAGS.

% Based on SPTOEPLITZ.M on the Mathworks File Exchange,
% Copyright (c) 2006 by Tobin Driscoll (tobin.driscoll@gmail.com).

% Developer note: This is needed by multiplication operators.

% This part is borrowed from built-in Toeplitz.
if ( nargin < 2 ) % Symmetric case
  col(1) = conj(col(1));
  row = col;
  col = conj(col);
else
  if ( col(1) ~= row(1) )
    warning('MATLAB:sptoeplitz:DiagonalConflict',['First element of ' ...
      'input column does not match first element of input row. ' ...
      '\n         Column wins diagonal conflict.'])
  end
end

% Size of result.
m = length(col(:));
n = length(row(:));

% Only use toeplitz if you have too... fairly slow.
if ( (m < 2e3) && (n < 2e3) )
    % Note: nnz(col) -- Number of nonzero elements in col
    if ( nnz(col) == 1 && nnz(row) == 1 )
        Ic = find(col);
        Ir = find(row);
        if ( Ic == 1 )
            T = spdiags(col(Ic)*ones(m, 1), 0, m, n);
        else
            T = spdiags([col(Ic)*ones(m, 1) row(Ir)*ones(n , 1)], ...
                [-Ic + 1, Ir - 1], m, n);
        end
    else
        T = toeplitz(col, row);
        T = sparse(T);
    end
else
    % Locate the nonzero diagonals.
    [ic, jc, sc] = find(col(:));
    row(1) = 0;  % not used
    [ir, jr, sr] = find(row(:));

    % Use spdiags for construction.
    d = [ ir - 1; 1 - ic ];
    B = repmat( [ sr; sc ].', min(m, n), 1 );
    T = spdiags(B, d, m, n);
end

end


function bk = r4_besks ( xnu, x, nin )

%*****************************************************************************80
%
%% R4_BESKS evaluates a sequence of K Bessel functions at X.
%
%  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 XNU, the order of the first function.
%    |XNU| < 1.
%
%    Input, real X, the argument.
%
%    Input, integer NIN, the absolute value of NIN indicates
%    the number of terms to compute.
%    If NIN < 0, successive values of NU count DOWN from XNU.
%    If NIN > 0, successive values of NU count UP from XNU.
%
%    Output, real BK(abs(NIN)), the K Bessel functions.
%
  persistent xmax

  if ( isempty ( xmax ) )
    xmax = - log ( r4_mach ( 1 ) );
    xmax = xmax + 0.5 * log ( 3.14 * 0.5 / xmax );
  end

  bk = r4_beskes ( xnu, x, nin );

  expxi = exp ( - x );
  n = abs ( nin );

  bk(1:n) = expxi * bk(1:n);

  return
end


function chebyshev_discrete_test ( )

%*****************************************************************************80
%
%% CHEBYSHEV_DISCRETE_TEST tests CHEBYSHEV_DISCRETE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2009
%
%  Author:
%
%    John Burkardt
%
  OFFSET = 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBYSHEV_DISCRETE_TEST:\n' );
  fprintf ( 1, '  CHEBYSHEV_DISCRETE evaluates discrete Chebyshev polynomials.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '       N      M         X        T(N,M,X)\n' );

  n = 5;
  m = 5;

  for j = 0 : 5

    x = j / 2.0;

    value = chebyshev_discrete ( n, m, x );

    fprintf ( 1, '\n' );

    for i = 0 : n

      fprintf ( 1, '  %8d  %8d  %8f  %14f\n', i, m, x, value(i+OFFSET) );

    end

  end

  return
end


function M = legendreMass(k)
% Compute the mass matrix using legendre polinomials of order 'k' as a base
% for our expansion.

M = diag(2./(2*(0:k)+1));


%PRINCSE Compute principal stresses and strains.
%
%   [ SE_P ] = PRINCSE( SE_X, SE_Y, SE_Z, SE_XY, SE_YZ, SE_XZ, ICOMP ) Computes
%   principal stresses and strains (for the ICOMP component, default 1). In two
%   dimensions only inputs SE_X, SE_Y, SE_Z, SE_XY, and ICOMP are required.

% Copyright 2013-2022 Precise Simulation, Ltd.


function c=comp_nonsepdgtreal_quinqux(f,g,a,M)
%COMP_NONSEPDGTREAL_QUINQUX  Compute Non-separable Discrete Gabor transform
%   Usage:  c=comp_nonsepdgtreal_quinqux(f,g,a,M);
%
%   This is a computational subroutine, do not call it directly.

%   AUTHOR : Nicki Holighaus and Peter L. Søndergaard
%   TESTING: TEST_NONSEPDGT
%   REFERENCE: REF_NONSEPDGT

lt=[1 2];

L=size(f,1);
W=size(f,2);
N=L/a;
M2=floor(M/2)+1;

% ----- algorithm starts here, split into sub-lattices ---------------

c=zeros(M,N,W,assert_classname(f,g));

mwin=comp_nonsepwin2multi(g,a,M,[1 2],L);

% simple algorithm: split into sublattices

for ii=0:1
    c(:,ii+1:2:end,:)=comp_dgt(f,mwin(:,ii+1),2*a,M,[0 1],0,0,0);
end;

% Phase factor correction
E = zeros(1,N,assert_classname(f,g));
for win=0:1
    for n=0:N/2-1
        E(win+n*2+1) = exp(-2*pi*i*a*n*rem(win,2)/M);
    end;
end;

c=bsxfun(@times,c(1:M2,:,:),E);


function [kPa] = ftH2O2kPa(ftH2O)
% Convert pressure from feet of water column at 4 degrees to kilopascals
% Chad Greene 2012
kPa = ftH2O*2.98907;


function overlayPlot(varargin)
%IMAGEOVERLAY  Overlay two images
%
% DESCRIPTION:
%       overlayPlot overlays two 2D images. The background is displayed
%       using a grayscale map. The foreground is log compressed and
%       thresholded to a particular dynamic range, and overlayed using an
%       alpha value of 0.5.
%
%       Example:
%           x = rand(128);
%           y = peaks(128);
%           overlayPlot(x, y, 30);
%
% USAGE:
%       overlayPlot(bg, fg)
%       overlayPlot(bg, fg, fg_dnr)
%       overlayPlot(x, y, bg, fg, fg_dnr)
%
% INPUTS:
%       x, y        - vectors describing the position of the pixels in the
%                     image equivalent to image(x, y, c)
%       bg          - background image
%       fg          - foreground image
%       fg_dbr      - dynamic range in dB of foreground image
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 17th October 2012
%       last update - 4th June 2013
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox

% This file is part of k-Wave. k-Wave 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.
%
% k-Wave 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 Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% set the literals
num_colors = 128;
transparency = 0.5;
fg_dnr = 30;

% extract the inputs
if nargin == 2 || nargin == 3
    bg = varargin{1};
    fg = varargin{2};
    if nargin == 3
        fg_dnr = varargin{3};
    end
elseif nargin == 5
    x_vec = varargin{1};
    y_vec = varargin{2};
    bg = varargin{3};
    fg = varargin{4};
    fg_dnr = varargin{5};
else
    error('incorrect number of inputs');
end

% scale the background image from 0 to num_colors
bg = bg - min(bg(:));
bg = round(num_colors*bg/max(bg(:)));

% convert the background image to true color
bg = ind2rgb(bg, gray(num_colors));

% plot the background image
if nargin == 5
    image(x_vec, y_vec, bg);
else
    image(bg);
end

% if a value for the dynamic range is given, log compress and threshold
if fg_dnr
    fg(fg <= 0) = 0;
    fg = 20*log10(fg./max(fg(:)));
    fg = fg + fg_dnr;
    fg(fg <= 0) = 0;
end

% scale the background image from 0 to num_colors
fg = fg - min(fg(:));
fg = round(num_colors*fg/max(fg(:)));

% compute the alpha channel
alpha = transparency*ones(size(fg));
alpha(fg == 0) = 0;

% convert the background image to true color
fg = ind2rgb(fg, jet(num_colors));

% plot the foreground image and set the alpha channel
hold on;
if nargin == 5
    fg_im = image(x_vec, y_vec, fg);
else
    fg_im = image(fg);
end
set(fg_im, 'AlphaData', alpha);


% test for level set on meshes

path(path, '../toolbox_graph_data/off/');
if not(exist('name'))
name = 'elephant-50kv';
name = 'bunny';
name = 'david50kf';
name = 'hand';
end

options.name = name;
[vertex,face] = read_off([name '.off']);

func = 'xaxis';
func = 'eigen';

switch func
    case 'xaxis'
        F = rescale(vertex(1,:))';
        tau = linspace(0.05,.95, 10);
    case 'eigen'
        options.symmetrize = 1;
        options.normalize = 1;
        disp('--> Computing Laplacian');
        L = compute_mesh_laplacian(vertex,face,'conformal',options);
        opts.disp = 0;
        p = 150;
        disp('--> Extracting Eigenvectors');
        [U,D] = eigs(L,p, 'SM', opts);
        D = diag(abs(D)); [D,I] = sort(D); U = U(:,I);
        nb = 10; sel = round(linspace(10,p,nb));
        F = U(:, sel);
        tau = 0;
end

rep = 'results/levelsets-meshes/';
if not(exist(rep))
    mkdir(rep);
end

disp('--> Displaying');
for it=1:size(F,2)
    progressbar(it,size(F,2));
    f = F(:,it);
    options.niter_averaging = 3;
    f = perform_mesh_smoothing(face, vertex, f, options);
    % extract level set
    [v1,v2] = compute_levelset_mesh(vertex,face,f,tau,options);

    % display
    lw = 3;
    fw = perform_histogram_matching(f, linspace(0,1,length(f)));
    options.face_vertex_color = fw;
    clf;
    hold on;
    plot_mesh(vertex,face,options);
    for i=1:size(v1,2)
        h = plot3( [v1(1,i) v2(1,i)], [v1(2,i) v2(2,i)], [v1(3,i) v2(3,i)], 'k' );
        set(h, 'LineWidth', lw);
    end
    hold off;
    colormap jet(256);
    saveas(gcf, [rep name '-' func '-' num2string_fixeddigit(it,2), '.png'], 'png');
end


function [Acc]=parameter_sensitivity_test_loocv(All_Feat,nSubj,label,meth_Net,lambda_lasso)
% This function performs parameter sensitivity test using all the subject data with LOOCV, and
% it is run across all the combination of parameters.
% Input:
%       All_Feat: the constructed brain network using one combination of parameter;
%       nSubj: number of subjects;
%       label: the label for each subject; e.g., -1 for normal controls and 1 for patients;
%       lambda_lasso: lambda in the lasso feature selection;
%
% Written by Zhen Zhou, zzstefan@email.unc.edu
% IDEA lab, https://www.med.unc.edu/bric/ideagroup
% Department of Radiology and BRIC, University of North Carolina at Chapel Hill

e = 1:nSubj;
cpred = zeros(nSubj,1);
score = zeros(nSubj,1);

% LOOCV for testing
for i=1:nSubj
    Tst_ind = i;
    telabel = label(i);
    Trn_ind = e;
    Trn_ind(i) = [];
    trlabel = label;
    trlabel(i) = [];

    Feat = All_Feat;
    trFe = Feat(Trn_ind,:);
    teFe = Feat(Tst_ind,:);

    % Feature selection ag
    if ~strcmpi(meth_Net,'dHOFC')
        pval=0.05;  % generally use pval<0.05 as a threshold for feature selection
        [~,p]=ttest2(trFe(trlabel==-1,:),trFe(trlabel==1,:));
        trFe=trFe(:,p<pval);
        teFe=teFe(:,p<pval);

        midw=lasso(trFe,trlabel,'Lambda',lambda_lasso);  % parameter lambda for sparsity
        trFe=trFe(:,midw~=0);
        teFe=teFe(:,midw~=0);
    else
        midw=lasso(trFe,trlabel,'Lambda',lambda_lasso);  % parameter lambda for sparsity
        trFe=trFe(:,midw~=0);
        teFe=teFe(:,midw~=0);
    end

    % Feature normalization ag
    Mtr=mean(trFe);
    Str=std(trFe);
    trFe=trFe-repmat(Mtr,size(trFe,1),1);
    trFe=trFe./repmat(Str,size(trFe,1),1);
    teFe=teFe-Mtr;
    teFe=teFe./Str;

    % train SVM model ag
    classmodel=svmtrain(trlabel,trFe,'-t 0 -c 1 -q'); % linear SVM (require LIBSVM toolbox)
    % classify ag
    [cpred(i),~,score(i)]=svmpredict(telabel,teFe,classmodel,'-q');
end

Acc=100*sum(cpred==label)/nSubj;
%[AUC,SEN,SPE,F1]=perfeval(label,cpred,score);


function vbrfa2011_artificial_experiment(seed)

if nargin < 1
  seed = 10;
end
rand('state', seed);
randn('state', seed);

% Observations:
%
% - When increasing M, Laplace -> independent t
%
% - Variance of corruption makes no big difference between Laplace and
% independent t

% From Bishop, standard deviations 5,4,3,2,1,1,1,1,... :

M = 10;
N = 100;
D = 4;

% Covariance matrix singular values:

R = orth(randn(M));
eig = ([D:-1:1,zeros(1,M-D)]).^2;
Cov = R*diag(eig)*R';
% $$$ eig = (1+[D:-1:1,zeros(1,M-D)]).^2;
% $$$ Cov = R*diag(eig-1)*R';

mu = randn(M,1);

% Noiseless data
Y = mvnrnd(mu, Cov, N)';

% Noisy data
Yn = Y + 1*randn(M,N);

% Corrupted data
I = rand(M,N) < 0.02;
Yno = Yn + I.*unifrnd(-30,30,M,N);

%
% Construct the model
%

Dh = M-1;

% $$$ tsplot(Yno, 'k')
% $$$ addtsplot(Y, 'b');
% $$$ return

for model=1:4

  % It seems that (at least in some cases) it would be better to combine the
  % isotropic noise with X instead W for better and more stable results..?

  noise_iso = noise_module_isotropic(M, 1, ...
                                     'init', struct('a_tau',1, ...
                                                    'b_tau',1));

  % ARD for W
  W_module = factor_module_ard(Dh+1, M, ...
                               'update_alpha', 1, ...
                               'noise_module', noise_iso);

  % IID for X
  mu = [1; zeros(Dh,1)];
  Cov = diag([1e-6; ones(Dh,1)]);
  X_module = factor_module_iid(Dh+1, N, ...
                               'prior', struct('mu', mu, ...
                                               'CovX', Cov), ...
                               'noise_module', []);

  switch model

   case 1 % GAUSSIAN
    noise = noise_module_fixed(M, N, 1);
    id = 'gaussian';

   case 2 % MULTIVARIATE T
    noise = noise_module_multivariate_t(M, N, ...
                                        'update_nu', [1], ...
                                        'init', struct('nu', 0.1));
    id = 'multi-t';

   case 3 % INDEPENDENT T
    noise = noise_module_independent_t(M, N, ...
                                       'update_nu', [1], ...
                                       'nu', 'pooled', ...
                                       'init', struct('nu', 0.1));
    id = 'ind-t';

   case 4 % LAPLACE
    noise = noise_module_laplace(M, N);
    id = 'laplace';

   otherwise
    error('Unknown model requested')

  end

  %
  % Run the method
  %

  % General function call for running the method
  Q = vbfa(Dh+1, ...
           Yno, ...
           W_module, ...
           X_module, ...
           noise, ...
           'maxiter', 50, ...
           'rotate', [1:10, 10:5:100], ...
           'rotation_maxiter', 20, ...
           'update_x', 1, ...
           'update_w', 1, ...
           'update_noise', 1);

  Yh = Q.W'*Q.X;

  rmse1_train(model) = rmse(Yh, Yno)
  rmse2_test(model) = rmse(Yh(~I), Y(~I))
  rmse3_outliers(model) = rmse(Yh(I), Y(I))

% $$$   tsplot(Yno(1:10,:), 'k')
% $$$   addtsplot(Y(1:10,:), 'b');
% $$$   addtsplot(Yh(1:10,:), 'r');
end

rmse1_train
rmse2_test
rmse3_outliers

%Q.W*diag(Q.rhow)*Q.W' + sum(Q.CovW,3)


% A script for visualizing the orientations in the menpo data, allows to
% see the distributions of the data
clear
load('../menpo_68_pts.mat');
addpath('../../PDM_helpers');

xs = all_pts(1:end/2,:);
ys = all_pts(end/2+1:end,:);
num_imgs = size(xs, 1);

rots = zeros(3, num_imgs);
errs = zeros(1,num_imgs);

pdmLoc = ['../pdm_68_aligned_menpo.mat'];

load(pdmLoc);

pdm = struct;
pdm.M = double(M);
pdm.E = double(E);
pdm.V = double(V);
errs_poss = [];
for i=1:num_imgs

    labels_curr = cat(2, xs(i,:)', ys(i,:)');
    labels_curr(labels_curr==-1) = 0;

    [ a, R, T, ~, l_params, err, shapeOrtho] = fit_PDM_ortho_proj_to_2D(pdm.M, pdm.E, pdm.V, labels_curr);
    errs(i) = err/a;
    rots(:,i) = Rot2Euler(R);

end

hist(rots', 100);


function pde = TorusTime(mum,mup,sigm,sigp,epsm,epsp,omega,x0,y0,z0,r1,r2)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 02/21/2020 by Xu Zhang

%% PDE Structure
pde = struct('intf',@intf,...
    'f1',@f1,'f2',@f2,'f3',@f3,...
    'fm1',@fm1,'fm2',@fm2,'fm3',@fm3,...
    'fp1',@fp1,'fp2',@fp2,'fp3',@fp3,...
    'Mu',@Mu,'Mum',@Mum,'Mup',@Mup,'one',@one,...
    'Epslon',@Epslon,'Epslonm',@Epslonm,'Epslonp',@Epslonp,...
    'Sig',@Sig,'Sigm',@Sigm,'Sigp',@Sigp);

pde.mum = mum;
pde.mup = mup;
pde.sigm = sigm;
pde.sigp = sigp;
pde.epsm = epsm;
pde.epsp = epsp;
%% interface function
    function u = intf(x,y,z)
        u = (sqrt((x-x0).^2+(y-y0).^2)-r2).^2 + (z-z0).^2 - r1^2;
    end

%% exact solution

%% right hand side function
    function u = f1(x,y,z,t)
        u = fm1(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = fp1(x(id),y(id),z(id),t);
    end
    function u = f2(x,y,z,t)
        u = fm2(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = fp2(x(id),y(id),z(id),t);
    end
    function u = f3(x,y,z,t)
        u = fm3(x,y,z,t);
        id = intf(x,y,z) > 0;
        u(id) = fp3(x(id),y(id),z(id),t);
    end

    function u = fm1(x,y,z,t)
        u = -2*pi*omega*sin(myacos(x,y))*cos(2*pi*omega*t);
    end
    function u = fm2(x,y,z,t)
        u = 2*pi*omega*cos(myacos(x,y))*cos(2*pi*omega*t);
    end
    function u = fm3(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fp1(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fp2(x,y,z,t)
        u = zeros(size(x));
    end
    function u = fp3(x,y,z,t)
        u = zeros(size(x));
    end
    function theta = myacos(x,y)
        theta = (2*pi*(1-sign(y))/2 + sign(y).*acos(x./sqrt(x.^2+y.^2))).*abs(sign(y))+...
            (1-abs(sign(y))).*((1-x)/2*pi);
    end

%% Diffusion coefficient function
    function u = Mu(x,y,z)
        u = Mum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Mup(x(id),y(id),z(id));
    end
    function u = Mum(x,y,z)
        u = mum^(-1)*ones(size(x));
    end
    function u = Mup(x,y,z)
        u = mup^(-1)*ones(size(x));
    end

%% Mass coefficient function
    function u = Epslon(x,y,z)
        u = Epslonm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Epslonp(x(id),y(id),z(id));
    end
    function u = Epslonm(x,y,z)
        u = epsm*ones(size(x));
    end
    function u = Epslonp(x,y,z)
        u = epsp*ones(size(x));
    end

    function u = Sig(x,y,z)
        u = Sigm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Sigp(x(id),y(id),z(id));
    end
    function u = Sigm(x,y,z)
        u = sigm*ones(size(x));
    end
    function u = Sigp(x,y,z)
        u = sigp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
end


function rs = acf(s, len)

%tstoolbox/@signal/acf
%   Syntax:
%     * acf(s, len)
%
%   Input arguments:
%     * len -length of the fft (optional)
%
%   Autocorrelation function for real scalar signals, using fft (of length
%   len). If len is ommited a default value is calculated. The maximum of
%   the calculated length is 128.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

narginchk(1,2);

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

if (nargin < 2)
	if dlens(s,1) > 256
		len = 128;
	else
		len = nextpow2(dlens(s,1)/4);
	end
end

c = acf(s.core, len);
rs = signal(c, s);	% special constructor calling syntax for working routines
a = getaxis(s, 1);
dl = delta(a);
a = setfirst(a, 0);
rs = setaxis(rs, 1, a);
rs = setyunit(rs, unit);		% acf values are scalars without unit
rs = addhistory(rs, 'Autocorrelation function');
rs = setlabel(rs, 'Autocorrelation function');
rs = addcommandlines(rs, 's = acf(s', len);


function A = projection_precon_mnls(X, B, A)

    s = svd(B'*B);
    L = max(s);
    %eigen_values = eig(B'*B);
    %L = max(eigen_values);

    grad = - X * B + A*(B' * B);
    A = A - 1/L * grad / (B'*B);
    A = max(A, 0);
end


function params = get_params_value()
% constant parameters
params.c = physconst('LightSpeed');% Speed of light in air (m/s)
params.fc = 77e9; % Center frequency (Hz)
params.lambda = params.c/params.fc;
params.Rx = 4;
params.Tx = 2;

% configuration parameters
params.Fs = 4*10^6;
params.sweepSlope = 21.0017e12;
params.samples = 128;
params.loop = 255;

params.Tc = 120e-6; % us
params.fft_Rang = 134; % 134=>128
params.fft_Vel = 256;
params.fft_Ang = 128;
params.num_crop = 3;
params.max_value = 1e+04; % data WITH 1843

% Creat grid table
freq_res = params.Fs/params.fft_Rang; % range_grid
freq_grid = (0:params.fft_Rang-1).'*freq_res;
params.rng_grid = freq_grid*params.c/params.sweepSlope/2; % d=frediff_grid*c/sweepSlope/2;

w = linspace(-1,1,params.fft_Ang); % angle_grid
params.agl_grid = asin(w)*180/pi; % [-1,1]->[-pi/2,pi/2]

% velocity_grid
dop_grid = fftshiftfreqgrid(params.fft_Vel,1/params.Tc); % now fs is equal to 1/Tc
params.vel_grid = dop_grid*params.lambda/2;   % unit: m/s, v = lamda/4*[-fs,fs], dopgrid = [-fs/2,fs/2]

end


function max_pr = trace_fit_extreme(C,fr,t_int,fac)

if nargin < 4 || isempty(fac); fac = 1; end
if nargin < 3 || isempty(t_int); t_int = 0.25; end
if nargin < 2 || isempty(fr); fr = 30; end

Np = round(t_int*fr);
[K,T] = size(C);
bas = zeros(K,1);
sn = zeros(K,1);
for i = 1:K
    [~,density,xmesh] = kde(C(i,:));
    [~,ind] = max(density);
    bas(i) = xmesh(ind);
    sn(i) = std(C(i,C(i,:)<bas(i)))/sqrt(1-2/pi);
end

mu = norminv(1-1/T);
sig = norminv(1-exp(-1)/T) - mu;

z = bsxfun(@times, 1./(fac*sn(:)), bsxfun(@minus, C, bas));  % normalized z scores
z_pr = exp(-exp(-(z-mu)/sig));

filt_z = filter(ones(1,Np),1,log(z_pr),[],2)/sqrt(Np);
max_pr = exp(max(filt_z,[],2));


function [P,T] = cubic_flat_eval(C,tol)
  % CUBIC_FLAT_EVAL Recursively subdivide a cubic Bezier curve until each
  % segment spans a region of the curve that is locally flat up to a given
  % tolerance (i.e., this computes an adaptive refinement).
  %
  % [P,T] = cubic_flat_eval(C,tol)
  %
  % Inputs:
  %   C  4 by dim list of control points
  % Outputs:
  %   P  #P by dim list of evaluated points
  %   T  #T list of corresponding parameter values
  %
  % See also: cubic_eval, cubic_is_flat, cubic_split
  %
  if cubic_is_flat(C,tol)
    P = C([1 4],:);
    T = [0 1];
  else
    [C1,C2] = cubic_split(C,0.5);
    [P1,T1] = cubic_flat_eval(C1,tol);
    [P2,T2] = cubic_flat_eval(C2,tol);
    P = [P1;P2(2:end,:)];
    T = [T1*0.5;0.5+T2(2:end,:)];
  end
end


function X = cs_qleft (V, Beta, p, Y)
%CS_QLEFT apply Householder vectors on the left.
%   X = cs_qleft(V,Beta,p,Y) computes X = Hn*...*H2*H1*P*Y = Q'*Y where Q is
%   represented by the Householder vectors V, coefficients Beta, and
%   permutation p.  p can be [], which denotes the identity permutation.
%
%   Example:
%       Prob = UFget ('HB/well1033') ; A = Prob.A ; [m n] = size (A) ;
%       b = rand (m,1) ;
%       [V,beta,p,R] = cs_qr (A) ; % QR factorization of A(p,:)
%       b1 = cs_qleft (V, beta, p, b) ;
%       x1 = R (1:n,1:n) \ b1 (1:n) ;
%       x2 = A\b ;
%       norm (x1-x2)
%
%   See also CS_QR, CS_QRIGHT.

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

[m2 n] = size (V) ;
[m ny] = size (Y) ;
X = Y ;
if (m2 > m)
    if (issparse (Y))
        X = [X ; sparse(m2-m,ny)] ;
    else
        X = [X ; zeros(m2-m,ny)] ;
    end
end
if (~isempty (p))
    X = X (p,:) ;
end
for k = 1:n
    X = X - V (:,k) * (Beta (k) * (V (:,k)' * X)) ;
end


function partition=new_partitions(zones,sigma,r,N,x)
%sigma=1;
%r=8.4;
T=r/2/sigma;
kappa=size(zones);
kappa=kappa(1);
%width=sigma*(2*N/r-1);
%x=-width:1/200:width;

partition=zeros(kappa,length(x));

if kappa==1
  k=kappa;
  for j=1:length(x)
      if x(j)<=(zones(k,1)-1)/T+sigma
        partition(k,j)=0;
      elseif (zones(k,1)-1)/T+sigma<x(j) & x(j)<-sigma
        partition(k,j)=rho((-x(j)-sigma)/(-2*sigma-(zones(k,1)-1)/T));
      elseif -sigma<=x(j) & x(j)<=sigma
        partition(k,j)=1;
      elseif sigma<x(j) & x(j)<(zones(k,2)+1)/T-sigma
        partition(k,j)=rho((x(j)-sigma)/((zones(k,2)+1)/T-2*sigma));
      else
        partition(k,j)=0;
      end
  end
else
for k=1:kappa
  if k==1
    for j=1:length(x)
      if x(j)<=(zones(k,1)-1)/T+sigma
        partition(k,j)=0;
      elseif (zones(k,1)-1)/T+sigma<x(j) & x(j)<-sigma
        partition(k,j)=rho((-x(j)-sigma)/(-2*sigma-(zones(k,1)-1)/T));
      elseif -sigma<=x(j) & x(j)<=(zones(k+1,1)-1)/T+sigma
        partition(k,j)=1;
      elseif (zones(k+1,1)-1)/T+sigma<x(j) & x(j)<(zones(k,2)+1)/T-sigma
	partition(k,j)=rho((x(j)-((zones(k+1,1)-1)/T+sigma))/((zones(k,2)-zones(k+1,1)+2)/T-2*sigma));
%      elseif (1-N)/T+sigma<x(j) & x(j)<(1-N/2)/T
%        partition(k,j)=1-rho((-x(j)+(1-N/2)/T)/(N/T/2-sigma))/2;
%      elseif x(j)==(1-N/2)/T
%        partition(k,j)=1/2;
%      elseif (1-N/2)/T<x(j) & x(j)<1/T-sigma
%        partition(k,j)=rho((x(j)-(1-N/2)/T)/(N/T/2-sigma))/2;
      else
        partition(k,j)=0;
      end
    end
  elseif k==kappa
    for j=1:length(x)
      if x(j)<=(zones(k,1)-1)/T+sigma
        partition(k,j)=0;
      elseif (zones(k,1)-1)/T+sigma<x(j) & x(j)<(zones(k-1,2)+1)/T-sigma
        partition(k,j)=1-rho((x(j)-((zones(k,1)-1)/T+sigma))/((zones(k-1,2)-zones(k,1)+2)/T-2*sigma));
%      elseif sigma-1/T<x(j) & x(j)<(N/2-1)/T
%        partition(k,j)=rho((-x(j)+(N/2-1)/T)/(N/T/2-sigma))/2;
%      elseif x(j)==(N/2-1)/T
%        partition(k,j)=1/2;
%      elseif (N/2-1)/T<x(j) & x(j)<(N-1)/T-sigma
%        partition(k,j)=1-rho((x(j)-(N/2-1)/T)/(N/T/2-sigma))/2;
      elseif (zones(k-1,2)+1)/T-sigma<=x(j) & x(j)<=sigma
        partition(k,j)=1;
      elseif sigma<x(j) & x(j)<(zones(k,2)+1)/T-sigma
        partition(k,j)=rho((x(j)-sigma)/((zones(k,2)+1)/T-2*sigma));
      else
        partition(k,j)=0;
      end
    end
  else
    for j=1:length(x)
      if x(j)<=(zones(k,1)-1)/T+sigma
        partition(k,j)=0;
      elseif (zones(k,1)-1)/T+sigma<x(j) & x(j)<(zones(k-1,2)+1)/T-sigma
        partition(k,j)=1-rho((x(j)-((zones(k,1)-1)/T+sigma))/((zones(k-1,2)-zones(k,1)+2)/T-2*sigma));
%      elseif (-N+k-1)/T+sigma<x(j) & x(j)<(k-1-N/2)/T
%        partition(k,j)=rho((-x(j)+(k-1-N/2)/T)/(N/T/2-sigma))/2;
%      elseif x(j)==(k-1-N/2)/T
%        partition(k,j)=1/2;
%      elseif (k-1-N/2)/T<x(j) & x(j)<(k-1)/T-sigma
%        partition(k,j)=1-rho((x(j)-(k-1-N/2)/T)/(N/T/2-sigma))/2;
      elseif (zones(k-1,2)+1)/T-sigma<=x(j) & x(j)<=(zones(k+1,1)-1)/T+sigma
        partition(k,j)=1;
      elseif (zones(k+1,1)-1)/T+sigma<x(j) & x(j)<(zones(k,2)+1)/T-sigma
        partition(k,j)=rho((x(j)-((zones(k+1,1)-1)/T+sigma))/((zones(k,2)-zones(k+1,1)+2)/T-2*sigma));
%      elseif (-N+k)/T+sigma<x(j) & x(j)<(k-N/2)/T
%        partition(k,j)=1-rho((-x(j)+(k-N/2)/T)/(N/T/2-sigma))/2;
%      elseif x(j)==(k-N/2)/T
%        partition(k,j)=1/2;
%      elseif (k-N/2)/T<x(j) & x(j)<k/T-sigma
%        partition(k,j)=rho((x(j)-(k-N/2)/T)/(N/T/2-sigma))/2;
      else
        partition(k,j)=0;
      end
    end
  end
%  plot(x,partition(k,:))
%  axis([min(x) max(x) 0 1])
%  [k kappa]
%  zones(k,:)
%  pause
end
end
%plot(x,partition'); %'
%pause

%total=zeros(size(x));
%for j=1:N
%total=total+partition(j,:);
%end
%spy(1-total);


classdef NSGAIIARSBX < ALGORITHM
% <multi> <real/integer> <constrained/none>
% NSGA-II with adaptive rotation based simulated binary crossover

%------------------------------- Reference --------------------------------
% L. Pan, W. Xu, L. Li, C. He, and R. Cheng, Adaptive simulated binary
% crossover for rotated multi-objective optimization, Swarm and
% Evolutionary Computation, 2021, 60: 100759.
%--------------------------------------------------------------------------
% 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".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Generate random population
            Population = Problem.Initialization();
            [~,FrontNo,CrowdDis] = EnvironmentalSelection(Population,Problem.N);
            B  = eye(Problem.D);
            m  = 0.5*(Problem.upper - Problem.lower);
            ps = 0.5;

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = TournamentSelection(2,Problem.N,FrontNo,-CrowdDis);
                Offspring  = ARSBX(Problem,Population(MatingPool),{B,m,ps});
                [Population,FrontNo,CrowdDis] = EnvironmentalSelection([Population,Offspring],Problem.N);
                [B,m,ps,Population] = UpdateParameter(Problem,Population);
            end
        end
    end
end


function A = affineBuild(trans, rot, scale, skew)
%
% A = affineBuild(trans, rot, scale, skew)
%
% Builds an affine (orthogonal linear) transformation matrix from the four
% componets: translations, rotations scales and skews.
%
% trans, rot, scale, skew should all be 1x3 vectors specifying x,y,z.
% Rotations are in radians, and can be described as pitch (x-rotation),
% roll (y-rotation) and yaw (z-rotation).
%
% The transformations are applied in the following order:
%
% 1) translations
% 2) rotations
% 3) scaling
% 4) skews
%
% The form of the transform assumes a PRE-multiplication format:
% Y = A*X where X and Y are 4 x n arrays of n coordinates.
%
% HISTORY:
%   2004.03.12 RFD (bob@white.stanford.edu) shamelessly copied the core
%   algorithm from spm99's spm_matrix.m.
%   2005.07.08 ras (sayres at stanford edu) imported into mrVista 2.0
%   2005.07.28 ras enorces double class for inputs
if ~exist('skew','var') | isempty(skew), skew = [0 0 0];    end
if ~exist('scale','var') | isempty(scale), scale = [1 1 1]; end
if ~exist('rot','var') | isempty(rot), rot = [0 0 0];       end
if ~exist('trans','var') | isempty(trans), trans = [0 0 0]; end
if ~isa(trans,'double'), trans = double(trans);             end
if ~isa(rot,'double'), rot = double(rot);                   end
if ~isa(scale,'double'), scale = double(scale);             end
if ~isa(skew,'double'), skew = double(skew);                end

A  = eye(4);

A  = A*[1 	0 	0  trans(1);
        0 	1 	0  trans(2);
        0 	0 	1  trans(3);
        0 	0 	0  1];

A  = A*[1   0            0            0;
        0   cos(rot(1))  sin(rot(1))  0;
        0  -sin(rot(1))  cos(rot(1))  0;
        0   0    	      0           1];

A  = A*[cos(rot(2))  0  sin(rot(2))  0;
        0    	     1  0            0;
       -sin(rot(2))  0  cos(rot(2))  0;
        0            0  0            1];

A  = A*[cos(rot(3))  sin(rot(3))  0  0;
       -sin(rot(3))  cos(rot(3))  0  0;
        0            0            1  0;
        0     	     0    	      0  1];

A  = A*[scale(1) 0         0         0;
        0        scale(2)  0         0;
        0        0         scale(3)  0;
        0        0         0         1];

A  = A*[1  skew(1)  skew(2)  0;
        0  1        skew(3)  0;
        0  0        1        0;
        0  0        0        1];

return;


function dX = Motor2D(X,K,isForceField)

% Parameters
m   = 2;%1;  % mass
b   = 10; % viscosity
c1  = 0.15/2;
c2  = 0.05/2;
dt  = 0.005;
tau = 0.05;
A = [ 0 0    1    0;
      0 0    0    1;
      0 0 -b/m    0;
      0 0    0 -b/m];
B = [0   0;
     0   0;
     1/m 0;
     0 1/m];
B1 = [1/tau 0;
     0  1/tau];

x = X(1:6);
z = X(7:8);

dx = x;
x = x(:);
u = -K*x;

w = randn(2,1)*sqrt(dt);
M = [c1*u(1) c2*u(2); -c2*u(1) c1*u(2)];

VF=[-10.1 -11.2; -11.2 11.1];

v = M*w; % control dependent noise
f = z;

dx(1:4) = A * x(1:4)+B*(x(5:6)+f);
dx(5:6) = B1 * (-x(5:6)+u+v./dt);

if isForceField
	dz = -1/0.01*(z-VF*[x(3);x(4)]);
else
	dz=[0;0];
end

dX=[dx;dz];
end


classdef SomeOptimalSuperEllipseExponentVsStress < handle

    properties (Access = private)
        samplePoints
        qOpt
        phiMin
        phiMax
        nPhi
        rhoV
        xiV
        rho
        xi
        outputPath
        phiDV
        phiV
        iIn
        iOut
        qMin
        qMax
        qMean
    end

    methods (Access = public)

        function obj = SomeOptimalSuperEllipseExponentVsStress()
            obj.init();
            for iTest = 1:length(obj.xiV)
                obj.rho   = obj.rhoV(iTest);
                obj.xi   = obj.xiV(iTest);
                obj.phiV  = obj.createPhi();
                obj.phiDV = obj.createPhiInInterval();
                obj.createSamplePoints();
                obj.computeOptimalSuperEllipseExponent();
                obj.computeMeanSuperEllipseExponent();
                obj.plotQoptAndGaussianVsPhi(iTest);
            end
        end

    end

    methods (Access = private)

        function init(obj)
            obj.outputPath = '/home/alex/Dropbox/PaperStress/';
            obj.phiMin = 0;
            obj.phiMax = pi;
            obj.nPhi = 3;%50;
            obj.rhoV  = [0.9,0.9,0.5,0.5];
            obj.xiV  = pi/2 - [0.1083,0.557,0.88974,1.0984];
        end

        function itIs = isInInterval(obj,phi)
            itIs = phi>= obj.phiMin & phi <= obj.phiMax;
        end

        function phiD = createPhiInInterval(obj)
            phi = obj.phiV;
            iI  = obj.isInInterval(phi);
            iO  = ~iI;
            phiD(iI) = phi(iI);
            phiD(iO) = sign(phi(iO)).*(phi(iO)-pi/2);
        end

        function phi = createPhi(obj)
            phi = linspace(obj.phiMin,obj.phiMax,obj.nPhi);
        end

        function createSamplePoints(obj)
            s.type = 'FromFixedRhoAndTxi';
            s.rho0 = obj.rho;
            s.txi  = obj.xi;
            s.phi  = obj.phiV;
            sample = SamplePointsCreatorForOptimalExponentComputer.create(s);
            sample.compute();
            obj.samplePoints = sample;
        end

        function computeOptimalSuperEllipseExponent(obj)
            s.samplePoints = obj.samplePoints;
            rhoT = strrep(num2str(obj.rho),'.','_');
            txiT = strrep(num2str(round(obj.xi,3)),'.','_');
            fN = ['AveragingSuperEllipseRho',rhoT,'Txi',txiT];
            s.fileName = fN;
            exponentComputer = OptimalExponentComputer(s);
            exponentComputer.compute();
            obj.qOpt = exponentComputer.qOpt;
            obj.qMax = exponentComputer.qMax;
            obj.qMin = exponentComputer.qMin;
        end

        function computeMeanSuperEllipseExponent(obj)
            s.phiV = obj.phiV;
            qMeanC    = SuperEllipseMeanAndDesvExponentComputer(s);
            obj.qMean = qMeanC.computeMean(obj.qOpt,obj.xi);
        end

        function plotQoptAndGaussianVsPhi(obj,itxi)
            s.phiV = obj.phiV;
            s.phiMin = obj.phiMin;
            s.phiMax = obj.phiMax;
            s.qOpt = obj.qOpt;
            s.qMin = obj.qMin;
            s.qMax = obj.qMax;
            s.qMean = obj.qMean;
            s.xi = obj.xi;
            s.rho = obj.rho;
            p = OptimalSuperEllipseExponentVsGaussianPlotter(s);
            p.plot(itxi);
        end

    end

end


function morph_profile=GetMP(img,SEs)
% Extract Morphological Profiles of the input image with the given SEs
%2016-10-20, jlfeng
[nr,nc]=size(img);
numSE=length(SEs);
morph_profile=zeros(nr,nc,numSE*8);
%%
% Opening and Closing
mp1=zeros(nr, nc,numSE);mp2=zeros(nr, nc,numSE);
for kk=1:numSE
    mp1(:,:,kk)=imclose(img,SEs{kk});
    mp2(:,:,kk)=imopen(img,SEs{numSE+1-kk});
end
dmp=diff(cat(3,mp1,img,mp2),[],3);
morph_profile(:,:,1:numSE*4)=cat(3,mp1,mp2,dmp);
% OpenRec and CloseRec
for kk=1:numSE
    marker=imerode(img,SEs{numSE+1-kk});
    mp1(:,:,kk)=imreconstruct(marker,img);
    marker=imdilate(img,SEs{kk});
    mp2(:,:,kk)=imreconstruct(imcomplement(marker),imcomplement(img));
end
dmp=diff(cat(3,mp1,img,mp2),[],3);
morph_profile(:,:,(numSE*4+1):numSE*8)=cat(3, mp1,mp2,dmp);


classdef LHSintegrator_StiffnessColumn < LHSintegrator

    properties (Access = public)
        geometry
        stiffnessMatrix
    end

    properties (Access = private)
        freeNodes
    end

    methods (Access = public)

        function obj = LHSintegrator_StiffnessColumn(cParams)
            obj.init(cParams)
            obj.initStiffnessColumn(cParams);
            obj.createQuadrature();
            obj.createInterpolation();
            obj.createGeometry();
        end

        function LHS = compute(obj)
            lhs = obj.computeElementalLHS();
            LHS = obj.assembleMatrix(lhs);
            obj.stiffnessMatrix = LHS;
        end

        function [Kfree,free] = provideFreeStiffnessMatrix(obj)
            free = obj.freeNodes;
            K = obj.stiffnessMatrix;
            Kfree  = K(free,free);
        end

    end

    methods (Access = protected)

        function lhs = computeElementalLHS(obj)
            d = obj.dim;
            nElem = obj.mesh.nelem;
            Edof = d.ndofPerElement;
            Ke = zeros(Edof,Edof,nElem);
            l = obj.computeLength();
            [c1,c2,c3,c4,c5] = obj.coeffsStiffness(l);
            Ke(1,1,:) = c1.*c2;
            Ke(1,2,:) = c1.*c3;
            Ke(1,3,:) = -c1.*c2;
            Ke(1,4,:) = c1.*c3;
            Ke(2,1,:) = c1.*c3;
            Ke(2,2,:) = c1.*c4;
            Ke(2,3,:) = -c1.*c3;
            Ke(2,4,:) = -c1.*c5;
            Ke(3,1,:) = -c1.*c2;
            Ke(3,2,:) = -c1.*c3;
            Ke(3,3,:) = c1.*c2;
            Ke(3,4,:) = -c1.*c3;
            Ke(4,1,:) = c1.*c3;
            Ke(4,2,:) = -c1.*c5;
            Ke(4,3,:) = -c1.*c3;
            Ke(4,4,:) = c1.*c4;
            lhs = Ke;
        end

    end

    methods (Access = private)

        function initStiffnessColumn(obj,cParams)
            obj.freeNodes = cParams.freeNodes;
        end

        function createGeometry(obj)
            q   = obj.quadrature;
            int = obj.interpolation;
            int.computeShapeDeriv(q.posgp);
            s.mesh = obj.mesh;
            g = Geometry.create(s);
            g.computeGeometry(q,int);
            obj.geometry = g;
        end

        function l = computeLength(obj)
            g = obj.geometry;
            l = sum(g.dvolu,2);
        end

        function [c1,c2,c3,c4,c5] = coeffsStiffness(obj,l)
            c1 = (1./(30*l))';
            c2 = 36*ones(1,length(l));
            c3 = (3*l)';
            c4 = (4*l.^2)';
            c5 = (l.^2)';
        end

    end

end


% FUNCTION kdtree = kdtree_create(points)
%
% AUTHOR:     Steven Michael
%             (smichael@ll.mit.edu)
%
% DATE:       2/17/05
%
% DESCRIPTION:
%
%  This function creates a KD Tree from the given points
%  and outputs it in the abstract object "kdtree"
%  The "kdtree" object can then be used for range finding
%  and nearest neighbor searching.
%
% INPUTS:
%
%   points   :     A (npoints X ndim) array of points, where "npoints"
%                  is the number of points and "ndim" is the number
%                  of dimensions.  Note that the points, even if they
%                  are double precision, will be converted to single
%                  precision when the tree is populated.  This is for
%                  speed -- most kdtree search applications don't
%                  necessitate double precision data.
%
% OUTPUTS:
%
%   kdtree   :     The abstract KD Tree object.
%
%
% Example:
%
%    % Create a list of 1000 random points in 3d space
%    r = rand(1000,3);
%
%    % Create a tree from this list
%    tree = kdtree(r);
%
%    % Find the point closest to the origin
%    [pntidx,pntval] = kdtree_closestpoint(tree,[0 0 0]);
%
%    % Create a list "r2" of 100 random points in 3d space and
%    % find the points in "r" that are closest to each point in "r2"
%    [pntidx,pntval] = kdtree_closestpoint(tree,r2);
%
%    % Find all the points within the cube defined by "rng"
%    rng = [ [.45 .55]; [.45 .55]; ; [.45 .55] ];
%    pntidx = kdtree_range(tree,rng);


function check_WC_params(par)

if isfield(par,'features') && strcmp('features','wav')

    if isfield(par,'scales')&&  isfield(par,'par.w_post') &&  isfield(par,'par.w_pre')
         L = wmaxlev(par.w_pre+par.w_post,'haar');
         if L<scale
             error('[par.scale] exceeds maximum wavelet decomposition level for a waveform of length [par.w_pre+par.w_post]')
         end
    end
    if isfield(par,'par.w_post') &&  isfield(par,'par.w_pre')
        x = log2(par.w_pre+par.w_post);
        if floor(x)~=x
            error('The length [par.w_pre+par.w_post] should be a power of 2')
        end
    end
end
end


clf
set(gcf, 'Position', [90   410   651   545]);
goal = [4 8];
start = [7 2];
opt.metric = 'euclidean'
opt.show = 1
opt.movie = [];%'dxform2.mp4'

% make a simple map
occgrid = zeros(10,10);
occgrid(4:6,3:7) = 1;
%occgrid(7:8,7) = 1;  % extra bit

cost0 = occgrid;
cost0(cost0==1) = NaN;

cost = cost0;
cost(cost0==0) = Inf;
cost(goal(2), goal(1)) = 0;

if ~isempty(opt.movie)
    anim = Animate(opt.movie);
end

showpixels(cost, 'contrast', 6, 'fmt', '%.2g', 'cscale', [0 12], 'fontsize', 20, 'infsymbol', 'nancolor', 'nohidenan', 'nohideinf', 'infcolor')

if ~isempty(opt.movie)
    anim.add();
end

switch opt.metric
    case 'cityblock'
        m = [inf 1 inf
              1  0  1
             inf 1 inf];
    case 'euclidean'
        r2 = sqrt(2);
        m = [r2 1 r2
              1 0  1
             r2 1 r2];
    otherwise
        error('unknown distance metric');
end

count = 0;
ninf = 0;

while 1

    cost = imorph(cost, m, 'plusmin');
    count = count+1;
    if opt.show
        % transfer over the finite values
%         k = ~isinf(cost0);
%         cost(k) = cost0(k);

        showpixels(cost, 'contrast', 6, 'fmt', '%.2g', 'cscale', [0 14], 'fontsize', 20, 'nancolor', 'nohideinf', 'infsymbol', 'nohidenan', 'infcolor', 'here')
        xlabel('x', 'FontSize', 20); ylabel('y', 'FontSize', 20)
        %         cmap = [1 0 0; gray(count)];
        %         colormap(cmap)
        %         image(occgrid+1, 'CDataMapping', 'direct');
        %         set(gca, 'Ydir', 'normal');
        %         xlabel('x');
        %         ylabel('y');
        if ~isempty(opt.movie)
            anim.add();
        end
        pause(opt.show);
    end

    ninfnow = sum( isinf(cost(:)) ); % current number of Infs
    if ninfnow == ninf
        % stop if the number of Infs left in the map had stopped reducing
        % it may never get to zero if there are unreachable cells in the map
        break;
    end
    ninf = ninfnow;
end
count
if ~isempty(opt.movie)
    anim.close();
end