% Generate analysis time instants, optionaly using a given f0 curve
%
% Octave compatible
%
% Inputs
%  wav            : Wavform vector
%  fs             : Sampling frequency
%  tmargin        : Margin at start and end
%  usesampletimes : Time instants are thoses of samples.
%                   Time instants can't be between two sample times,
%                   such as window centers correspond to samples times
%  method
%     0: Regular instants.
%        varargin{1}: step size [s]
%     1: Keep f0sin time instants and fill with interpolated values.
%        varargin{1}: An f0 curve
%        varargin{2}: number of instants per period
%     2: Generate new instants from the given f0 curve.
%        varargin{1}: An f0 curve
%        varargin{2}: number of instants per period
%        Skip nan, inf and zero values
%
% Outputs
%  f0sout : f0 feature with the generated times
%
% Copyright (c) 2011 University of Crete - Computer Science Department (UOC-CSD)
%
% License
%  This file is under the LGPL license,  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. This file 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.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function at = gen_analysis_times(wav, fs, tmargin, usesampletimes, method, varargin)

    if nargin<5;    usesampletimes = true; end
    if nargin<6;    method = 3; end

    wavdur = (length(wav)-1)/fs;

    if method==0
        step = varargin{1};
        disp('    Generate regular analysis time');
        if usesampletimes
            at = round(tmargin*fs)/fs:round(step*fs)/fs:round((wavdur-tmargin)*fs)/fs;
        else
            at = tmargin:step:(wavdur-tmargin);
        end
        at = at';

    elseif method==1
        f0sin = varargin{1};
        nbperper = varargin{2};
        if nbperper>1
            disp(['    Add extra ' num2str(nbperper) ' analysis instants between each f0 value']);
            f0sin = f0sin(find(~isnan(f0sin(:,2)) & ~isinf(f0sin(:,2)) & f0sin(:,2)~=0),:);
            at = [];
            for n=1:size(f0sin,1)-1
                for m=0:nbperper-1
                    t = f0sin(n,1) + (m/nbperper)*(f0sin(n+1,1)-f0sin(n,1));
                    at = [at; t];
                end
            end
        else
            at = f0sin(:,1);
        end

    elseif method==2
        f0sin = varargin{1};
        nbperper = varargin{2};
        disp(['    Adapt analysis instants to f0 curve with ' num2str(nbperper) ' analysis intants per period']);
        f0sin = f0sin(find(~isnan(f0sin(:,2)) & ~isinf(f0sin(:,2)) & f0sin(:,2)~=0),:);
        at = [];
        nt = tmargin;
        while nt < wavdur - tmargin
            at = [at; nt];
            nt = nt + (1/nbperper)/interp1td(f0sin, nt);
        end

    end
    idx = find(at>tmargin & at<wavdur-tmargin);
    at = at(idx);

    if usesampletimes; at = round(fs*at)/fs; end

return


function mgdata = mg_ns_diff(Ap,domain,flowsol,qmethod,outbnd)
%mg_ns_diff   GMG for diffusion problem (Navier-Stokes)
%   mgdata = mg_ns_diff(Ap,domain,flowsol,qmethod,outbnd)
%   input
%          Ap           top level discrete diffusion operator
%          domain       domain information, 1 for cavity, 3 for step
%          flowsol      current velocity solution iterate
%          qmethod      discretization method, 0 for Q1-Q1, 2 for Q2-Q1
%          outbnd       location of outflow boundary (for step)
%   output
%          mgdata       structure containing GMG data at all levels
%             matrix    discrete diffusion operators
%             prolong   grid transfer operators
%             smoother  structure containing smoothing operators
%             nc        grid parameter identifying finest level
%
%   IFISS function: HCE; 18 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

if domain==1,      % Cavity
   np = sqrt(length(Ap));
   nc = log2(np-1);
   grid_type = 1;

% Smoothing strategy
   sweeps=1; smooth=2; stype=2;  % Means one (sweeps)-directional line (stype) GS (smooth)

% Top level
% Note (19 April 2004):  Fine level matrix created here is not pinned down
   [x,y,xy,mv,mp,bound] = mg_cavity_domain(nc+1,grid_type);
   [x,y,xy,xyp] = mg_diff_q2q1grid(x,y,xy,mv,mp,bound);
   mgdata(nc).matrix = mg_apsetup_q1(xy,xyp,mv,mp,domain);
   if qmethod==2,
      [x,y,xy,mv,mp,bound] = mg_cavity_domain(nc,grid_type);
      [x,y,xy,xyp] = mg_diff_q2q1grid(x,y,xy,mv,mp,bound);
      mgdata(nc).prolong = mg_prolong(2^nc,2^nc,x,y);
   else % qmethod=0
      mgdata(nc).prolong=mg_prolong(2^nc,2^nc,x,y);
   end
   mgdata(nc).smoother = mg_ns_smooth(mgdata(nc).matrix,sweeps,smooth,stype);
   mgdata(nc).nc = nc;  % Not used, saved to be consistent with domain=3

   % Loop over remaining levels
   for level = nc-1:-1:2;
      if qmethod==2,
         %%% construct matrix
         mgdata(level).matrix = mg_apsetup_q1(xy,xyp,mv,mp,domain);
         %%% construct prolongation operator
         [x,y,xy,mv,mp,bound] = mg_cavity_domain(level,grid_type);
         [x,y,xy,xyp] = mg_diff_q2q1grid(x,y,xy,mv,mp,bound);
         mgdata(level).prolong = mg_prolong(2^level,2^level,x,y);
      else % qmethod=0
         [x,y,xy,mv,mp,bound] = mg_cavity_domain(level+1,grid_type);
         [x,y,xy,xyp] = mg_diff_q2q1grid(x,y,xy,mv,mp,bound);
         mgdata(level).matrix = mg_apsetup_q1(xy,xyp,mv,mp,domain);
         mgdata(level).prolong=mg_prolong(2^level,2^level,x,y);
      end
      mgdata(level).smoother = mg_ns_smooth(mgdata(level).matrix,sweeps,smooth,stype);
      mgdata(level).nc = nc;
   end

elseif domain==3,     % Step
   nu = length(flowsol)/2;
   xi = (-(outbnd+3)+sqrt((outbnd+3)^2+4*(2*outbnd+1)*(nu-1))) ...
          / (2*(2*outbnd+1));
   nc = log2(2*xi);

% Smoothing strategy
   sweeps=1; smooth=3; stype=1; % Means one (sweeps)-directional point (stype) ILU (smooth)

% top level
   [x,y,xy,mv,mp,bound] = mg_step_domain(nc,outbnd);
   mgdata(nc).matrix = Ap;
   if qmethod==2,
      x=x(1:2:end)'; y=y(1:2:end)'; mgdata(nc).prolong=mg_ns_prolong_step(nc-1,x,y,outbnd);
   else % qmethod=0
      x=x'; y=y';
      mgdata(nc).prolong=mg_ns_prolong_step(nc,x,y,outbnd);
   end
   mgdata(nc).smoother = mg_ns_smooth(mgdata(nc).matrix,sweeps,smooth,stype);
   mgdata(nc).nc = nc;

%%% loop over remaining levels
   for level = nc-1:-1:2;
      if qmethod==2,
         [x,y,xy,mv,mp,bound] = mg_step_domain(level,outbnd);
         [x,y,xy,xyp] = mg_diff_q2q1grid(x,y,xy,mv,mp,bound);
         mgdata(level).matrix = mg_apsetup_q1(xy,xyp,mv,mp,domain);
         if level>2,
            x=x(1:2:end)'; y=y(1:2:end)';
            mgdata(level).prolong=mg_ns_prolong_step(level-1,x,y,outbnd);
         end
      else % qmethod=0
         [x,y,xy,xyp] = mg_diff_q2q1grid(x,y,xy,mv,mp,bound);
         mgdata(level).matrix = mg_apsetup_q1(xy,xyp,mv,mp,domain);
         [x,y,xy,mv,mp,bound] = mg_step_domain(level,outbnd);
         x=x'; y=y';
         mgdata(level).prolong = mg_ns_prolong_step(level,x,y,outbnd);
      end
      mgdata(level).smoother = mg_ns_smooth(mgdata(level).matrix,sweeps,smooth,stype);
      mgdata(level).nc = nc;
   end
else
   error('Error, mg_ns_diff_data is defined only for cavity and step\n');
end


function [ position, velocity, acceleration ] = interpolate( s, nrl, target, t )
% This function differentiates and interpolates a set of Chebyshev coefficients to give position and velocity.
%
% void interpolate (double *buf, double *t, long int ncf, long int na,
%
%                   double *position, double *velocity)
%
% ------------------------------------------------------------------------
%
%    PURPOSE:
%       This function differentiates and interpolates a set of
%       Chebyshev coefficients to give position and velocity.
%
%    REFERENCES:
%       Standish, E.M. and Newhall, X X (1988). "The JPL Export
%          Planetary Ephemeris"; JPL document dated 17 June 1988.
%
%    INPUT
%    ARGUMENTS:
%       *buf (double)
%          Array of Chebyshev coefficients of position.
%       *t (double)
%          t[0] is fractional time interval covered by coefficients at
%          which interpolation is desired (0 <= t[0] <= 1).
%          t[1] is length of whole interval in input time units.
%       ncf (long int)
%          Number of coefficients per component.
%       na (long int)
%          Number of sets of coefficients in full array
%          (i.e., number of sub-intervals in full interval).
%
%    OUTPUT
%    ARGUMENTS:
%       *position (double)
%          Position array of requested object.
%       *velocity (double)
%          Velocity array of requested object.
%
%    RETURNED
%    VALUE:
%       None.
%
%    GLOBALS
%    USED:
%       NP                eph_manager.h
%       NV                eph_manager.h
%       PC                eph_manager.h
%       VC                eph_manager.h
%       TWOT              eph_manager.h
%
%    FUNCTIONS
%    CALLED:
%       fmod              math.h
%
%    VER./DATE/
%    PROGRAMMER:
%       V1.0/03-93/WTH (USNO/AA): Convert FORTRAN to C.
%       V1.1/07-93/WTH (USNO/AA): Update to C standards.
%       V1.2/07-98/WTH (USNO/AA): Modify to make position and velocity
%                                 two distinct vector arrays.
%       V1.3/11-07/WKP (USNO/AA): Updated prolog.
%       V1.4/12-07/WKP (USNO/AA): Changed ncf and na arguments from short
%                                 int to long int.
%       V1.5/10-10/WKP (USNO/AA): Renamed function to lowercase to
%                                 comply with coding standards.
%
%    NOTES:
%       None.
%
% ------------------------------------------------------------------------

  persistent PC VC AC NP TWOT;
  if isempty(PC)
    PC = zeros(1,15); % ncf <= 15
    VC = zeros(1,15); % ncf <= 15
    AC = zeros(1,15); % ncf <= 15
    TWOT = 0.0;
    PC(1) = 1.0; % T_0(x) = 1
    PC(2) = 0.0; % T_1(x) = x
    VC(1) = 0.0; % T'_0(x) = 0
    VC(2) = 1.0; % T'_1(x) = 1
    AC(1) = 0.0; % T"_0(x) = 0
    AC(2) = 0.0; % T"_1(x) = 0
    AC(3) = 4.0; % T"_2(x) = 4
    NP = 2;
  end
  if target == Ephem.TTState
    NDIM = 1;
  elseif target == Ephem.NutationsState
    NDIM = 2;
  else
    NDIM = 3;
  end

  buf = s.IPT(1,target) + nrl*s.indexInc + s.indexOffset;
  ncf = s.IPT(2,target);
  na  = s.IPT(3,target);
  table = s.scan;

  position = zeros(1,NDIM); % row vector
  velocity = zeros(1,NDIM); % row vector
  acceleration = zeros(1,NDIM); % row vector
  if ncf <= 0
    if s.output > 0
      fprintf(s.output,'ERROR: interpolate DE%s. No coefficients available for target %d\n', ...
        s.de_number,target);
    end
    return;
  end
  %   Get correct sub-interval number for this set of coefficients and
  %   then get normalized Chebyshev time within that subinterval.
  dna = double(na);
  temp = dna * double(t);

  %   'tc' is the normalized Chebyshev time (-1 <= tc <= 1).

  tc = 2.0 * mod(temp,1.0) - 1.0;

  %   Check to see whether Chebyshev time has changed, and compute new
  %   polynomial values if it has.  (The element PC[1] is the value of
  %   t1[tc] and hence contains the value of 'tc' on the previous call.)

  if tc ~= PC(2)
    NP = 3;
    TWOT = tc + tc;
    PC(2) = tc; % T_1(x) = x
    PC(3) = TWOT * PC(2) - PC(1); % T_2(x) = 2x^2-1
    VC(3) = 2.0 * TWOT; % T'_2(x) = 4x
  end

  %   Be sure that at least 'ncf' polynomials have been evaluated and
  %   are stored in the array 'PC'.
  %   Chebyshev polynomial recurrence relationship PC[i+1] = T_i(tc)

  if NP < ncf
    for i = (NP+1):ncf
      % T_i-1(x) = 2x T_i-2(x) - T_i-3(x)
      PC(i) = TWOT * PC(i-1) - PC(i-2);
      % T'_i-1(x) = 2x T'_i-2(x) + 2 T_i-2(x) - T'_i-3(x)
      VC(i) = TWOT * VC(i-1) + PC(i-1) + PC(i-1) - VC(i-2);
      % T"_i-1(x) = 2x T"_i-2(x) + 4 T'_i-2(x) - T"_i-3(x)
      AC(i) = TWOT * AC(i-1) + 4*VC(i-1) - AC(i-2);
    end
    NP = ncf;
  end

  vfac = 2.0 * dna;
  %   Interpolate to get position for each component.

  k = buf + int32(floor(temp)) * (NDIM * ncf);
  if k + NDIM*ncf > length(table)
    if s.output > 0
      fprintf(s.output,'interpolate: past end of coefficients by %d/%d\n',...
        k + NDIM*ncf-length(table),length(table));
    end
    return;
  end
  % p = sum( a_i * T_i(x) )
  % v = sum( a_i * T'_i(x) )*2*n
  % a = sum( a_i * T"_i(x) )*4*n^2
  for i = 1:NDIM
    p = 0.0;
    v = 0.0;
    a = 0.0;
    for j = ncf:-1:1
      p = p + PC(j) * table(k + j);
      v = v + VC(j) * table(k + j);
      a = a + AC(j) * table(k + j);
    end
    position(i) = p;
    velocity(i) = v*vfac;
    acceleration(i) = a*vfac*vfac;
    k = k + ncf;
  end

end


function s = gsp_jtv_frame_synthesis(F,c)
%GSP_JTV_FRAME_SYNTHESIS Synthesis operator in time-vertex domain
%   Usage:  s = gsp_jtv_frame_synthesis(F,c,param);
%
%   Input parameters:
%         F          : Frame matrix (vertex_loc x time_loc x vertex x time)
%         c          : Coefficients matrix
%   Output parameters:
%         s          : Time-Vertex signal
%
%   This function compute the synthesis operator using frame matrix

% Author :  Francesco Grassi
% Date   : July 2016

[N,lag,~,T]=size(F);
s = reshape(reshape(F,N*lag,[])'*c(:),N,[]);


function [rc] = calc_ratechange(a,time_as,step,mintime, maxtime,timestep)
    % function [rc] = calc_ratechange(a,time_as,step,mintime,maxtime,timestep);
    % ----------------------------------------------------------------
    % Determines ratechanges within aftershock sequences
    %
    % Input parameters:
    %   a           earthquake catalog
    %   time_as     delay times (days)
    %   step        number of quakes to determine forecast period
    %   mintime     tart time
    %   maxtime     maximal time at which Omori parameters are calculated
    %   timestep    Timesteps between learning periods
    %
    % Output parameters:
    %   rc      Matrix containing: time, absdiff, sigma, numreal, nummod,
    %           pval, pvalstd, cval, cvalstd, kval, kvalstd, t_forecast, fMc
    %
    % Info: Version for ZMAP, original function by ratechange.m by S. Neukomm May 27, 2002
    % J. Woessner
    % last update: 09.07.03

    if min(time_as) >= 1
        rc = [];
        return
    end

    fMc0 = mcwithtime(a,time_as,7);

    rc = [];
    time = mintime;
    while time <= maxtime

        % determination of magnitude of completeness
        if time < 7
            fMc = mcwithtime(a,time_as,time);
        else
            fMc = fMc0;
        end

        % estimation of Omori parameters
        l = time_as <= time & a.Magnitude >= fMc;
        [pval, pvalstd, cval, cvalstd, kval, kvalstd, loopout] = bruteboot(time_as(l));

        if isnan(pval) == 0
            nummod = step; % forecasted number of aftershocks
            if pval == 1
                pv = 1-10^(-6);
            else
                pv = pval;
            end

            t_forecast = (-nummod*(pv-1)/kval+(time+cval)^(1-pv))^(1/(1-pv))-cval; % forecast interval
            if isreal(t_forecast) == 1  &&  t_forecast > time
                if t_forecast > max(time_as)
                    return
                end

                l = time_as <= t_forecast & a.Magnitude >= fMc & time_as >= time;
                numreal = sum(l); % observed number of aftershocks
                absdiff = numreal-nummod;

                % calculate uncertainty sigma in forecasted number of aftershocks
                time1 = t_forecast; mpm1 = 1-pv; t1c = time1+cval; t0c = time+cval;
                sigma = (((-t1c^mpm1+t0c^mpm1)/(pv-1)*kvalstd)^2+...
                    (kval/(pv-1)*(-t1c^mpm1*mpm1/t1c+t0c^mpm1*mpm1/t0c)*cvalstd)^2+...
                    (kval/(pv-1)*(t1c^mpm1*log(t1c)+t1c^mpm1/(pv-1)-t0c^mpm1*log(t0c)-t0c^mpm1/(pv-1))*pvalstd)^2)^0.5;

                rc = [rc; time absdiff sigma numreal nummod pval pvalstd cval cvalstd kval kvalstd t_forecast fMc];
            end
        end
        time = time+timestep;
    end


function [valMIC] = applyMIC(pathMINE,variable,matData,jobName)
% -------------------------------------------------------------------------
% function [valMIC] = applyMIC(pathMINE,variable,matData,jobName)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function computes the Maximal Information Coefficient (MIC) between
% the feature contained in the vector 'variable' and all the features
% contained in the matrix 'matData'. MIC is computed using the executable
% MINE.jar, which can be downloaded at: <http://www.exploredata.net/>.
% -------------------------------------------------------------------------
% INPUTS:
% - pathMINE: Full path to the MINE.jar executable.
% - variable: Column cector of size [nInst X 1], where 'nInst' refers to the
%             number of instances for the given feature tested.
% - matData: Matrix of size [nInst X nFeat], where 'nFeat' is the number of
%            features to be tested against 'variable'.
% - jobName: String specifying the name of the job to be sent to MINE.jar.
% -------------------------------------------------------------------------
% OUTPUTS:
% -  valMIC: Column vector of size [nFeat X 1], specifying the MIC between
%            each feature in 'matData' and the 'variable' features. Entry
%            number in 'valMIC' corresponds to the column number in
%            'matData'.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: May 2015
%--------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015  Martin Vallieres
%
%    This package 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 package 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 package.  If not, see <http://www.gnu.org/licenses/>.
%
%    _______________________________________________________________
%
% MINE version 1.0.1d
% Copyright 2011 by David Reshef and Yakir Reshef.
%
% This application is licensed under a Creative Commons
% Attribution-NonCommercial-NoDerivs 3.0 Unported License.
%
% See http://creativecommons.org/licenses/by-nc-nd/3.0/ for more information.
% -------------------------------------------------------------------------

startpath=pwd;
cd(pathMINE)

% Writing the MIC input file
if isempty(strfind(jobName,'.csv'))
    jobName = [jobName,'.csv'];
end
id = (0:size(matData,2))';
data = [variable,matData]';
csvwrite(jobName,[id,data],0,0)

% Executing MIC
commandSys = ['java -jar MINE.jar ',jobName,' -masterVariable 0'];
[~,~] = system(commandSys);

% Reading the output from MIC
commandRead = [jobName,',mv=0,cv=0.0,B=n^0.6,Results.csv'];
micOutput = csvread(commandRead,1,1,[1 1 size(matData,2) 2]);

% Sorting the results
[val,ind] = sort(micOutput(:,1));
valMIC = val;
valMIC(1:end) = micOutput(ind(1:end),2);

% Cleaning up
delete(jobName)
delete(commandRead)
commandLast=[jobName,',mv=0,cv=0.0,B=n^0.6,Status.txt'];
delete(commandLast)

cd(startpath)
end


classdef StiffnessMatrixComputer < handle

    properties (Access = private)
        LHS
        stiffnessMatrix
    end

    properties (Access = private)
        dim
        mesh
        youngModulus
        inertiaMoment
        freeNodes
    end

    methods (Access = public)

        function obj = StiffnessMatrixComputer(cParams)
            obj.init(cParams);
        end

        function compute(obj)
            obj.createStiffnessMatrix();
            obj.computeStiffnessMatrix()
        end

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

    end

    methods (Access = private)

        function obj = init(obj,cParams)
            obj.mesh          = cParams.mesh;
            obj.dim           = cParams.dim;
            obj.freeNodes     = cParams.freeNodes;
        end

        function createStiffnessMatrix(obj)
            s.type = 'StiffnessMatrixColumn';
            s.dim = obj.dim;
            s.mesh = obj.mesh;
            s.globalConnec = obj.mesh.connec;
            obj.LHS = LHSintegrator.create(s);
        end

        function computeStiffnessMatrix(obj)
            obj.stiffnessMatrix = obj.LHS.compute();
        end

    end

end


function parameters = initialise_parameters(ns_ps,Srate,method)
len_val = length(ns_ps);
switch lower(method)
    case 'martin'
        L_val=len_val;
        R_val=len_val/2;
        D_val=150; V_val=15; Um_val=10; Av_val=2.12;
        alpha_max_val=0.96;
        alpha_min_val=0.3;
        beta_max_val=0.8;
        x_val=[1 2 5 8 10 15 20 30 40 60 80 120 140 160];
        Y_M_val=[0 .26 .48 .58 .61 .668 .705 .762 .8 .841 .865 .89 .9 .91];
        Y_H_val=[0 .15 .48 .78 .98 1.55 2.0 2.3 2.52 2.9 3.25 4.0 4.1 4.1];
        xi_val=D_val;
        M_D_val=interp1(x_val,Y_M_val,xi_val);
        H_D_val=interp1(x_val,Y_H_val,xi_val);
        xi_val=V_val;
        M_V_val=interp1(x_val,Y_M_val,xi_val);
        H_V_val=interp1(x_val,Y_H_val,xi_val);
        minact_val(1:L_val,1:Um_val)=max(ns_ps);
        parameters = struct('n',2,'len',len_val,'alpha_corr',0.96,'alpha',0.96*ones(len_val,1),'P',ns_ps,'noise_ps',ns_ps,'Pbar',ns_ps,...
            'Psqbar',ns_ps,'actmin',ns_ps,'actmin_sub',ns_ps,'Pmin_u',ns_ps,'subwc',2,'u',1,'minact',minact_val,'lmin_flag',zeros(len_val,1),...
            'L',L_val,'R',R_val,'D',D_val,'V',V_val,'Um',Um_val,'Av',Av_val,'alpha_max',alpha_max_val,'alpha_min',alpha_min_val,...
            'beta_max',beta_max_val,'Y_M',Y_M_val,'Y_H',Y_H_val,'M_D',M_D_val,'H_D',H_D_val,'M_V',M_V_val,'H_V',H_V_val);
    case 'mcra'
        parameters = struct('n',2,'len',len_val,'P',ns_ps,'Pmin',ns_ps,'Ptmp',ns_ps,'pk',zeros(len_val,1),'noise_ps',ns_ps,...
            'ad',0.95,'as',0.8,'L',round(1000*2/20),'delta',5,'ap',0.2);
    case 'imcra'
        alpha_d_val=0.85;
        alpha_s_val=0.9;
        U_val=8;V_val=15;
        Bmin_val=1.66;gamma0_val=4.6;gamma1_val=3;
        psi0_val=1.67;alpha_val=0.92;beta_val=1.47;
        j_val=0;
        b_val=hanning(3);
        B_val=sum(b_val);
        b_val=b_val/B_val;
        Sf_val=zeros(len_val,1);Sf_tild_val=zeros(len_val,1);
        Sf_val(1) = ns_ps(1);
        for f=2:len_val-1
            Sf_val(f)=sum(b_val.*[ns_ps(f-1);ns_ps(f);ns_ps(f+1)]);
        end
        Sf_val(len_val)=ns_ps(len_val);
        Sf_tild_val = zeros(len_val,1);
        parameters = struct('n',2,'len',len_val,'noise_ps',ns_ps,'noise_tild',ns_ps,'gamma',ones(len_val,1),'Sf',Sf_val,...
            'Smin',Sf_val,'S',Sf_val,'S_tild',Sf_val,'GH1',ones(len_val,1),'Smin_tild',Sf_val,'Smin_sw',Sf_val,'Smin_sw_tild',Sf_val,...
            'stored_min',max(ns_ps)*ones(len_val,U_val),'stored_min_tild',max(ns_ps)*ones(len_val,U_val),'u1',1,'u2',1,'j',2,...
            'alpha_d',0.85,'alpha_s',0.9,'U',8,'V',15,'Bmin',1.66,'gamma0',4.6,'gamma1',3,'psi0',1.67,'alpha',0.92,'beta',1.47,...
            'b',b_val,'Sf_tild',Sf_tild_val);
    case 'doblinger'
        parameters = struct('n',2,'len',len_val,'alpha',0.7,'beta',0.96,'gamma',0.998,'noise_ps',ns_ps,'pxk_old',ns_ps,...
            'pxk',ns_ps,'pnk_old',ns_ps,'pnk',ns_ps);
    case 'hirsch'
        parameters = struct('n',2,'len',len_val,'as',0.85,'beta',1.5,'omin',1.5,'noise_ps',ns_ps,'P',ns_ps);
    case 'mcra2'
        freq_res=Srate/len_val;
        k_1khz=floor(1000/freq_res);
        k_3khz=floor(3000/freq_res);
        %delta_val=[2*ones(k_1khz,1);2*ones(k_3khz-k_1khz,1);5*ones(len_val/2-k_3khz,1);...
        %    5*ones(len_val/2-k_3khz,1);2*ones(k_3khz-k_1khz,1);2*ones(k_1khz,1)];
         delta_val=[2*ones(k_1khz,1);2*ones(k_3khz-k_1khz,1);5*ones(len_val/2-k_3khz,1)];
			delta_val=[delta_val;5;flipud(delta_val(2:end))];

        parameters = struct('n',2,'len',len_val,'ad',0.95,'as',0.8,'ap',0.2,'beta',0.8,'beta1',0.98,'gamma',0.998,'alpha',0.7,...
            'delta',delta_val,'pk',zeros(len_val,1),'noise_ps',ns_ps,'pxk_old',ns_ps,'pxk',ns_ps,'pnk_old',ns_ps,'pnk',ns_ps);

      case 'conn_freq'
        D = 7;
        b = triang(2*D+1)/sum(triang(2*D+1));
        b = b';
        beta_min = 0.7; % for R's recursion
        U = 5;
        V = 8;
        gamma1 = 6;
        gamma2 = 0.5;
        K_tild = 2*sum(b.^2)^2/sum(b.^4);
        alpha_max_val=0.96;
        alpha_min_val=0.3;
        stored_min = max(ns_ps)*ones(len_val,U);

        alpha_c = 0.7;
        noise_ps = ns_ps;
        Rmin_old = 1;
        Pmin_sw = ns_ps;
        Pmin = ns_ps;
        P = ns_ps;
        Decision = zeros(size(P));
        u1 = 1;
        j = 0;
        parameters = struct('len',len_val,'D',D,'b',b,'U',U,'V',V,'gamma1',gamma1,'gamma2',gamma2,'K_tild',K_tild,'alpha_c',alpha_c,...
            'noise_ps',noise_ps,'Rmin_old',Rmin_old,'Pmin_sw',Pmin_sw,'Pmin',Pmin,'SmthdP',P,'u1',u1,'j',j,'alpha',0,'alpha_max',alpha_max_val,...
            'stored_min',stored_min,'beta_min',beta_min,'Decision',Decision);

    otherwise
            error('Method not implemented. Check spelling.');
end


function [ f ] = DataDensityPlot( x, y, levels )
%DATADENSITYPLOT Plot the data density
%   Makes a contour map of data density
%   x, y - data x and y coordinates
%   levels - number of contours to show
%
% By Malcolm Mclean
%
    map = dataDensity(x, y, 256, 256);
    map = map - min(min(map));
    map = floor(map ./ max(max(map)) * (levels-1));
    f = figure();

    image(map);
    colormap(jet(levels));
    set(gca, 'XTick', [1 256]);
    set(gca, 'XTickLabel', [min(x) max(x)]);
    set(gca, 'YTick', [1 256]);
    set(gca, 'YTickLabel', [min(y) max(y)]);
    uiwait;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   q = solve_spherical_wrist_flexion_n2(robot, q, T, wrist)
%   Solves the inverse kinematic problem for a spherical wrist of the robot
%   Epson Flexion N2
%   robot: robot structure.
%   q: vector containing the values of the joints 1, 2 and 3.
%   T: orientation of the last reference system.
%   wrist: select -1 or 1 for two possible solutions (wrist up, wrist down)
%
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

function q = solve_spherical_wrist_flexion_n2(robot, q, T, wrist, method)

switch method

     %algebraic solution
    case 'algebraic'
        T01=dh(robot, q, 1);
        T12=dh(robot, q, 2);
        T23=dh(robot, q, 3);

        Q=inv(T23)*inv(T12)*inv(T01)*T;

        %detect the degenerate case when q(5)=0, this leads to zeros
        % in Q13, Q23, Q31 and Q32 and Q33=1
        thresh=1e-12;
        %detect if q(5)==0
        % this happens when cos(q5) in the matrix Q is close to 1
        if abs(Q(3,3)-1)>thresh
            %normal solution
            if wrist==1 %wrist up
                q(4)=atan2(-Q(2,3),-Q(1,3))+pi;
                q(6)=atan2(-Q(3,2),Q(3,1));
                %q(5)=atan2(-Q(3,2)/sin(q(6)),Q(3,3));
            else %wrist down
                q(4)=atan2(-Q(2,3),-Q(1,3))-pi+pi;
                q(6)=atan2(-Q(3,2),Q(3,1))+pi;
                %q(5)=atan2(-Q(3,2)/sin(q(6)),Q(3,3));
            end
            if abs(cos(q(6)+q(4)))>thresh
                cq5=(Q(1,1)+Q(2,2))/cos(q(4)+q(6))-1;
            end
            if abs(sin(q(6)+q(4)))>thresh
                cq5=(-Q(1,2)+Q(2,1))/sin(q(4)+q(6))-1;
            end
            if abs(sin(q(6)))>thresh
                sq5=-Q(3,2)/sin(q(6));
            end
            if abs(cos(q(6)))>thresh
                sq5=Q(3,1)/cos(q(6));
            end
            q(5)=atan2(sq5,cq5)+pi;

        else %degenerate solution, in this case, q4 cannot be determined,
             % so q(4)=0 is assigned
            if wrist==1 %wrist up
                q(4)=0;
                q(5)=0;
                q(6)=atan2(-Q(1,2)+Q(2,1),Q(1,1)+Q(2,2))-pi/2;
            else %wrist down
                q(4)=-pi;
                q(5)=0;
                q(6)=atan2(-Q(1,2)+Q(2,1),Q(1,1)+Q(2,2))+pi-pi/2;
            end

        end

       %geometric solution
    case 'geometric'
        % T is the noa matrix defining the position/orientation of the end
        % effector's reference system
        vx6=T(1:3,1);
        vz5=T(1:3,3); % The vector a z6=T(1:3,3) is coincident with z5

        % Obtain the position and orientation of the system 3
        % using the already computed joints q1, q2 and q3
        T01=dh(robot, q, 1);
        T12=dh(robot, q, 2);
        T23=dh(robot, q, 3);
        T03=T01*T12*T23;

        vx3=T03(1:3,1);
        vy3=T03(1:3,2);
        vz3=T03(1:3,3);

        % find z4 normal to the plane formed by z3 and a
        vz4=cross(vz3, vz5);	% end effector's vector a: T(1:3,3)

        % in case of degenerate solution,
        % when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution
        if norm(vz4) <= 0.000001
            if wrist == 1 %wrist up
                q(4)=0;
            else
                q(4)=-pi; %wrist down
            end
        else
            %this is the normal and most frequent solution
            cosq4=wrist*dot(-vy3,vz4);
            sinq4=wrist*dot(vx3,vz4);
            q(4)=atan2(sinq4, cosq4)+pi;
        end
        %propagate the value of q(4) to compute the system 4
        T34=dh(robot, q, 4);
        T04=T03*T34;
        vx4=T04(1:3,1);
        vy4=T04(1:3,2);

        % solve for q5
        cosq5=dot(vy4,vz5);
        sinq5=dot(-vx4,vz5);
        q(5)=atan2(sinq5, cosq5)+pi;

        %propagate now q(5) to compute T05
        T45=dh(robot, q, 5);
        T05=T04*T45;
        vx5=T05(1:3,1);
        vy5=T05(1:3,2);

        % solve for q6
        cosq6=dot(vx6,vx5);
        sinq6=dot(vx6,vy5);
        q(6)=atan2(sinq6, cosq6)-pi/2;

    otherwise
        disp('no method specified in solve_spherical_wrist');
end


function M = getmassmatvec3(elem2dof,volume,Dlambda,elemType,K)
%% GETMASSMATVEC3 get a mass matrix of finite element spaces in 3D
%
% M = GETMASSMATVEC3(elem2dof,volume,Dlambda,elemType,K) get mass matrix of
% the finite element space specified by elemType. The coefficient K is
% piecewise constant.
%
% The elemType can be:
%
% - "RT0": The lowest order Raviart-Thomas element
% - "ND0": The lowest order Nedelec element
% - "ND1": The full linear Nedelec element
% - "ND2m": The incomplete quadratic Nedelec element (P\Lambda_2^{-})
% - "ND2": The full quadratic Nedelec element (P\Lambda_2)
%
% Note that for H(div) element, the coefficient is Mij/K while for H(curl)
% element, it is Mij*K.
%
% See also Maxwell, Maxwell1, Maxwell2, Poisson3RT0
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('K','var'), K = []; end
NT = size(elem2dof,1);

if contains(elemType, 'ND')
    DiDj = zeros(NT,4,4);
    for i = 1:4
        for j = i:4
            DiDj(:,i,j) = dot(Dlambda(:,:,i),Dlambda(:,:,j),2);
            DiDj(:,j,i) = DiDj(:,i,j);
        end
    end
end

%% ND0: the lowest order edge element
if strcmp(elemType,'ND0')
    NE = double(max(elem2dof(:)));
    locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
    ii = zeros(21*NT,1); jj = zeros(21*NT,1); sM = zeros(21*NT,1);
    index = 0;
    for i = 1:6
        for j = i:6
            ii(index+1:index+NT) = double(elem2dof(:,i));
            jj(index+1:index+NT) = double(elem2dof(:,j));
            % mass matrix
            % locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
            i1 = locEdge(i,1); i2 = locEdge(i,2);
            j1 = locEdge(j,1); j2 = locEdge(j,2);
            Mij = 1/20*volume.*( (1+(i1==j1))*DiDj(:,i2,j2) ...
                               - (1+(i1==j2))*DiDj(:,i2,j1) ...
                               - (1+(i2==j1))*DiDj(:,i1,j2) ...
                               + (1+(i2==j2))*DiDj(:,i1,j1));
            if ~isempty(K)
                Mij = Mij.*K;
            end
            sM(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end
    diagIdx = (ii == jj);   upperIdx = ~diagIdx;
    M = sparse(ii(diagIdx),jj(diagIdx),sM(diagIdx),NE,NE);
    MU = sparse(ii(upperIdx),jj(upperIdx),sM(upperIdx),NE,NE);
    M = M + MU + MU';
end

%% ND1: The full linear Nedelec element
if strcmp(elemType,'ND1')
    NE = double(max(elem2dof(:)));
    Ndof = 2*NE;
    locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
    ii = zeros(21*NT,1); jj = zeros(21*NT,1);
    sMphi = zeros(21*NT,1); sMpsi = zeros(21*NT,1);
    index = 0;
    % two block diagonal matrix for phi and psi
    for i = 1:6
        for j = i:6
            % (phi_i,phi_j)
            ii(index+1:index+NT) = double(elem2dof(:,i));
            jj(index+1:index+NT) = double(elem2dof(:,j));
            % mass matrix
            % locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
            i1 = locEdge(i,1); i2 = locEdge(i,2);
            j1 = locEdge(j,1); j2 = locEdge(j,2);
            Mij = 1/20*volume.*( (1+(i1==j1))*DiDj(:,i2,j2) ...
                               - (1+(i1==j2))*DiDj(:,i2,j1) ...
                               - (1+(i2==j1))*DiDj(:,i1,j2) ...
                               + (1+(i2==j2))*DiDj(:,i1,j1));
            if ~isempty(K)
                Mij = Mij.*K;
            end
            sMphi(index+1:index+NT) = Mij;
            % (psi_i,psi_j)
            Mij = 1/20*volume.*( (1+(i1==j1))*DiDj(:,i2,j2) ...
                               + (1+(i1==j2))*DiDj(:,i2,j1) ...
                               + (1+(i2==j1))*DiDj(:,i1,j2) ...
                               + (1+(i2==j2))*DiDj(:,i1,j1));
            if ~isempty(K)
                Mij = Mij.*K;
            end
            sMpsi(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end
    diagIdx = (ii == jj);   upperIdx = ~diagIdx;
    M = sparse([ii(diagIdx); ii(diagIdx)+NE], [jj(diagIdx); jj(diagIdx)+NE],...
               [sMphi(diagIdx); sMpsi(diagIdx)],Ndof,Ndof);
    MU = sparse([ii(upperIdx); ii(upperIdx)+NE], [jj(upperIdx); jj(upperIdx)+NE],...
               [sMphi(upperIdx); sMpsi(upperIdx)],Ndof,Ndof);
    M = M + MU + MU';
    % off-diagonal matrix (psi, phi)
    ii = zeros(36*NT,1); jj = zeros(36*NT,1); ss = zeros(36*NT,1);
    index = 0;
    for i = 1:6
        for j = 1:6
            % local to global index map and its sign
            i1 = locEdge(i,1); i2 = locEdge(i,2);
            j1 = locEdge(j,1); j2 = locEdge(j,2);
            Mij = 1/20*volume.*( (1+(i1==j1))*DiDj(:,i2,j2) ...
                       - (1+(i1==j2))*DiDj(:,i2,j1) ...
                       + (1+(i2==j1))*DiDj(:,i1,j2) ...
                       - (1+(i2==j2))*DiDj(:,i1,j1));
            if ~isempty(K)
                Mij = Mij.*K;
            end
            ii(index+1:index+NT) = double(elem2dof(:,i))+NE;
            jj(index+1:index+NT) = double(elem2dof(:,j));
            ss(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end
    ML = sparse(ii,jj,ss,Ndof,Ndof);
    M  = M  + ML + ML';
end

%% ND2m: the incomplete quadratic Nedelec element
% see Maxwell2
% elem2dof = [elem2edge elem2edge+NE elem2face+2*NE elem2face+2*NE+NF];

if strcmp(elemType,'ND2m')
    NE = double(max(elem2dof(:, 1:6), [], "all"));
    NF = double(max(elem2dof(:, 12:16)-2*NE, [], "all"));
    Ndof = 2*(NE+NF);
    locBasesIdx = [1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % Nd0
               1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % grad(P2)
               3 2 4; 3 1 4; 2 1 4; 2 1 3; ...
               4 2 3; 4 1 3; 4 1 2; 3 1 2]; % face bubbles
    ii = zeros(210*NT,1); jj = zeros(210*NT,1);
    sM = zeros(210*NT,1);
    index = 0;
    for i = 1:20
        for j = i:20
            ii(index+1:index+NT) = double(elem2dof(:,i));
            jj(index+1:index+NT) = double(elem2dof(:,j));
            i1 = locBasesIdx(i,1); i2 = locBasesIdx(i,2); i3 = locBasesIdx(i,3);
            j1 = locBasesIdx(j,1); j2 = locBasesIdx(j,2); j3 = locBasesIdx(j,3);
            Mij = zeros(NT,1);
            if (i<=6) && (j<=6)
                % block 1: (phi_i,phi_j)
                Mij = 1/20*((1+(i1==j1))*DiDj(:,i2,j2) ...
                    - (1+(i1==j2))*DiDj(:,i2,j1) ...
                    - (1+(i2==j1))*DiDj(:,i1,j2) ...
                    + (1+(i2==j2))*DiDj(:,i1,j1));
            end
            if (i<=6) && (7<=j) && (j<=12)
                % block 2: (psi_j,phi_i)
                Mij = 1/20*( (1+(j1==i1))*DiDj(:,j2,i2) ...
                    - (1+(j1==i2))*DiDj(:,j2,i1) ...
                    + (1+(j2==i1))*DiDj(:,j1,i2) ...
                    - (1+(j2==i2))*DiDj(:,j1,i1));

            end
            if (7<=i) && (i<=12) && (7<=j) && (j<=12)
                % block 3: (psi_j,psi_i)
                Mij = 1/20*((1+(i1==j1))*DiDj(:,i2,j2) ...
                    + (1+(i1==j2))*DiDj(:,i2,j1) ...
                    + (1+(i2==j1))*DiDj(:,i1,j2) ...
                    + (1+(i2==j2))*DiDj(:,i1,j1));
            end
            if (i<=6) && (j>12)
                % block 4: (chi_j,phi_i)
                Mij = intlambda([j1,i1,j2],3)*DiDj(:,i2,j3) ...
                    -intlambda([j1,i1,j3],3)*DiDj(:,i2,j2) ...
                    -intlambda([j1,i2,j2],3)*DiDj(:,i1,j3) ...
                    +intlambda([j1,i2,j3],3)*DiDj(:,i1,j2);
            end
            if (7<=i) && (i<=12) && (j>12)
                % block 5: (chi_j,psi_i)
                Mij = intlambda([j1,i1,j2],3)*DiDj(:,i2,j3) ...
                    -intlambda([j1,i1,j3],3)*DiDj(:,i2,j2) ...
                    +intlambda([j1,i2,j2],3)*DiDj(:,i1,j3) ...
                    -intlambda([j1,i2,j3],3)*DiDj(:,i1,j2);
            end
            if (i>12) && (j>12)
                % block 6: (chi_j,chi_i)
                Mij = intlambda([i1,j1,i2,j2],3)*DiDj(:,i3,j3) ...
                    -intlambda([i1,j1,i2,j3],3)*DiDj(:,i3,j2) ...
                    -intlambda([i1,j1,i3,j2],3)*DiDj(:,i2,j3) ...
                    +intlambda([i1,j1,i3,j3],3)*DiDj(:,i2,j2);
            end
            if ~isempty(K)
                Mij = Mij.*K;
            end
            Mij = Mij.*volume;
            sM(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end

    diagIdx = (ii == jj);   upperIdx = ~diagIdx;
    M = sparse(ii(diagIdx),jj(diagIdx),sM(diagIdx),Ndof,Ndof);
    MU = sparse(ii(upperIdx),jj(upperIdx),sM(upperIdx),Ndof,Ndof);
    M = M + MU + MU';
end

%% ND2: the complete quadratic Nedelec element
% elem2dof = [elem2edge elem2edge+NE elem2edge+2*NE ...
%             elem2face+3*NE elem2face+3*NE+NF elem2face+3*NE+2*NF]
% index 1 to 18: edge basis
% index 19 to 30: face basis
% phi_1 = lambda_i^2 \nabla lambda_j
% phi_2 = lambda_j^2 \nabla lambda_i
% phi_3 = lambda_i lambda_j \nabla (lambda_i - lambda_j)
% psi_{F_{ijk}; 1,2,3} = ijk \in cyc {\lambda_i \lambda_j \nabla \lambda_k}

if strcmp(elemType,'ND2')
    NE = double(max(elem2dof(:, 1:6), [], "all"));
    NF = double(max(elem2dof(:, 19:22)-3*NE, [], "all"));
    Ndof = 3*(NE+NF);
    locBasesIdx = [1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % phi_1
               2 1 0; 3 1 0; 4 1 0; 3 2 0; 4 2 0; 4 3 0; ... % psi_2
               1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % psi_3
               2 3 4; 3 4 2; 4 2 3; ... % face 1
               1 3 4; 3 4 1; 4 1 3; ... % face 2
               1 2 4; 2 4 1; 4 1 2; ... % face 3
               1 2 3; 2 3 1; 3 1 2]; % face 4
    ii = zeros(465*NT,1); jj = zeros(465*NT,1);
    sM = zeros(465*NT,1);
    index = 0;
    for i = 1:30
        for j = i:30
            ii(index+1:index+NT) = double(elem2dof(:,i));
            jj(index+1:index+NT) = double(elem2dof(:,j));
            i1 = locBasesIdx(i,1); i2 = locBasesIdx(i,2); i3 = locBasesIdx(i,3);
            j1 = locBasesIdx(j,1); j2 = locBasesIdx(j,2); j3 = locBasesIdx(j,3);
            Mij = zeros(NT,1);
            if (i<=12) && (j<=12)
                % block 1: (phi_{1,2},phi_{1,2})
                Mij = intlambda([i1,i1,j1,j1],3)*DiDj(:,i2,j2);
            end
            if (i<=12) && (12<j) && (j<=18)
                % block 2: (phi_{1,2},phi_{3})
                Mij = intlambda([i1,i1,j1,j2],3)*...
                    (DiDj(:,i2,j1) - DiDj(:,i2,j2));
            end
            if (12<i) && (i<=18) && (12<j) && (j<=18)
                % block 3: (phi_{3},phi_{3})
                Mij = intlambda([i1,i2,j1,j2],3)* ...
                    (DiDj(:,i1,j1) + DiDj(:,i2,j2) ...
                    -DiDj(:,i1,j2) - DiDj(:,i2,j1));
            end
            if (i<=12) && (j>18)
                % block 4: (phi_{1,2}, psi_F)
                Mij = intlambda([i1,i1,j1,j2],3)*DiDj(:,i2,j3);
            end
            if (12<i) && (i<=18) && (j>18)
                % block 5: (phi_{3}, psi_F)
                Mij = intlambda([i1,i2,j1,j2],3)*...
                    (DiDj(:,i1,j3) - DiDj(:,i2,j3));
            end
            if (i>18) && (j>18)
                % block 6: (psi_F,psi_F)
                Mij = intlambda([i1,i2,j1,j2],3)*DiDj(:,i3,j3);
            end
            if ~isempty(K)
                Mij = Mij.*K;
            end
            Mij = Mij.*volume;
            sM(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end

    diagIdx = (ii == jj);   upperIdx = ~diagIdx;
    M = sparse(ii(diagIdx),jj(diagIdx),sM(diagIdx),Ndof,Ndof);
    MU = sparse(ii(upperIdx),jj(upperIdx),sM(upperIdx),Ndof,Ndof);
    M = M + MU + MU';
end

%% RT0: the lowest order face element
if strcmp(elemType,'RT0')
    NF = double(max(elem2dof(:)));
    localFace = [2 3 4; 1 3 4; 1 2 4; 1 2 3]; % ascend ordering
    M = sparse(NF,NF);
    for i = 1:4
        for j = i:4
            % local to global index map
            ii = double(elem2dof(:,i));
            jj = double(elem2dof(:,j));
            i1 = localFace(i,1); i2 = localFace(i,2); i3 = localFace(i,3);
            j1 = localFace(j,1); j2 = localFace(j,2); j3 = localFace(j,3);
            % computation of mass matrix --- (phi_i, phi_j)
            Mij = 1/5*volume.*( ...
                  (1+(i1==j1))*dot(mycross(Dlambda(:,:,i2),Dlambda(:,:,i3),2), ...
                                   mycross(Dlambda(:,:,j2),Dlambda(:,:,j3),2),2)...
                 +(1+(i1==j2))*dot(mycross(Dlambda(:,:,i2),Dlambda(:,:,i3),2), ...
                                   mycross(Dlambda(:,:,j3),Dlambda(:,:,j1),2),2)...
                 +(1+(i1==j3))*dot(mycross(Dlambda(:,:,i2),Dlambda(:,:,i3),2), ...
                                   mycross(Dlambda(:,:,j1),Dlambda(:,:,j2),2),2)...
                 +(1+(i2==j1))*dot(mycross(Dlambda(:,:,i3),Dlambda(:,:,i1),2), ...
                                   mycross(Dlambda(:,:,j2),Dlambda(:,:,j3),2),2)...
                 +(1+(i2==j2))*dot(mycross(Dlambda(:,:,i3),Dlambda(:,:,i1),2), ...
                                   mycross(Dlambda(:,:,j3),Dlambda(:,:,j1),2),2)...
                 +(1+(i2==j3))*dot(mycross(Dlambda(:,:,i3),Dlambda(:,:,i1),2), ...
                                   mycross(Dlambda(:,:,j1),Dlambda(:,:,j2),2),2)...
                 +(1+(i3==j1))*dot(mycross(Dlambda(:,:,i1),Dlambda(:,:,i2),2), ...
                                   mycross(Dlambda(:,:,j2),Dlambda(:,:,j3),2),2)...
                 +(1+(i3==j2))*dot(mycross(Dlambda(:,:,i1),Dlambda(:,:,i2),2), ...
                                   mycross(Dlambda(:,:,j3),Dlambda(:,:,j1),2),2)...
                 +(1+(i3==j3))*dot(mycross(Dlambda(:,:,i1),Dlambda(:,:,i2),2), ...
                                   mycross(Dlambda(:,:,j1),Dlambda(:,:,j2),2),2));
            if ~isempty(K)
                Mij = Mij./K;
            end
            if (j==i)
                M = M + sparse(ii,jj,Mij,NF,NF);
            else
                M = M + sparse([ii;jj],[jj;ii],[Mij; Mij],NF,NF);
            end
        end
    end
end


function Gamma = vorticity(L)
% vorticity Gamma

Gamma = L.rotationRate ./ L.strainRate;


% LFCalRefine - refine calibration by minimizing point/ray reprojection error, called by LFUtilCalLensletCam
%
% Usage:
%     CalOptions = LFCalRefine( InputPath, CalOptions )
%
% This function is called by LFUtilCalLensletCam to refine an initial camera model and pose
% estimates through optimization. This follows the calibration procedure described in:
%
% D. G. Dansereau, O. Pizarro, and S. B. Williams, "Decoding, calibration and rectification for
% lenslet-based plenoptic cameras," in Computer Vision and Pattern Recognition (CVPR), IEEE
% Conference on. IEEE, Jun 2013.
%
% Minor differences from the paper: camera parameters are automatically initialized, so no prior
% knowledge of the camera's parameters are required; the free intrinsics parameters have been
% reduced by two: H(3:4,5) were previously redundant with the camera's extrinsics, and are now
% automatically centered; and the light field indices [i,j,k,l] are 1-based in this implementation,
% and not 0-based as described in the paper.
%
% Inputs:
%
%     InputPath : Path to folder containing decoded checkerboard images. Checkerboard corners must
%                 be identified prior to calling this function, by running LFCalFindCheckerCorners
%                 for example. An initial estiamte must be provided in a CalInfo file, as generated
%                 by LFCalInit. LFUtilCalLensletCam demonstrates the complete procedure.
%
%     CalOptions struct controls calibration parameters :
%                        .Phase : 'NoDistort' excludes distortion parameters from the optimization
%                                 process; for any other value, distortion parameters are included
%             .CheckerInfoFname : Name of the file containing the summarized checkerboard
%                                 information, as generated by LFCalFindCheckerCorners. Note that
%                                 this parameter is automatically set in the CalOptions struct
%                                 returned by LFCalFindCheckerCorners.
%                 .CalInfoFname : Name of the file containing an initial estimate, to be refined.
%                                 Note that this parameter is automatically set in the CalOptions
%                                 struct returned by LFCalInit.
%          .ExpectedCheckerSize : Number of checkerboard corners, as recognized by the automatic
%                                 corner detector; edge corners are not recognized, so a standard
%                                 8x8-square chess board yields 7x7 corners
%            .LensletBorderSize : Number of pixels to skip around the edges of lenslets, a low
%                                 value of 1 or 0 is generally appropriate
%                   .SaveResult : Set to false to perform a "dry run"
%        [optional]    .OptTolX : Determines when the optimization process terminates. When the
%                                 estimted parameter values change by less than this amount, the
%                                 optimization terminates. See the Matlab documentation on lsqnonlin,
%                                 option `TolX' for more information. The default value of 5e-5 is set
%                                 within the LFCalRefine function; a value of 0 means the optimization
%                                 never terminates based on this criterion.
%      [optional]    .OptTolFun : Similar to OptTolX, except this tolerance deals with the error value.
%                                 This corresponds to Matlab's lsqnonlin option `TolFun'. The default
%                                 value of 0 is set within the LFCalRefine function, and means the
%                                 optimization never terminates based on this criterion.
%
% Outputs :
%
%     CalOptions struct maintains the fields of the input CalOptions, and adds the fields:
%
%                    .LFSize : Size of the light field, in samples
%            .IJVecToOptOver : Which samples in i and j were included in the optimization
%           .IntrinsicsToOpt : Which intrinsics were optimized, these are indices into the 5x5
%                              lenslet camera intrinsic matrix
%     .DistortionParamsToOpt : Which distortion params were optimized
%     .PreviousCamIntrinsics : Previous estimate of the camera's intrinsics
%     .PreviousCamDistortion : Previous estimate of the camera's distortion parameters
%                    .NPoses : Number of poses in the dataset
%
%
% User guide: LFToolbox.pdf
% See also:  LFUtilCalLensletCam, LFCalFindCheckerCorners, LFCalInit, LFUtilDecodeLytroFolder

% Copyright (c) 2013-2020 Donald G. Dansereau

function CalOptions = LFCalRefine( InputPath, CalOptions )

%---Defaults---
CalOptions = LFDefaultField( 'CalOptions', 'OptTolX', 5e-5 );
CalOptions = LFDefaultField( 'CalOptions', 'OptTolFun', 0 );

%---Load checkerboard corners and previous cal state---
CheckerInfoFname = fullfile(InputPath, CalOptions.CheckerInfoFname);
CalInfoFname = fullfile(InputPath, CalOptions.CalInfoFname);

load(CheckerInfoFname, 'CheckerObs', 'IdealChecker', 'LFSize');
[EstCamPosesV, EstCamIntrinsicsH, EstCamDistortionV, CamInfo, LensletGridModel, DecodeOptions] = ...
    LFStruct2Var( LFReadMetadata(CalInfoFname), 'EstCamPosesV', 'EstCamIntrinsicsH', 'EstCamDistortionV', 'CamInfo', 'LensletGridModel', 'DecodeOptions' );
CalOptions.LFSize = LFSize;

%---Set up optimization variables---
CalOptions.IJVecToOptOver = CalOptions.LensletBorderSize+1:LFSize(1)-CalOptions.LensletBorderSize;
CalOptions.IntrinsicsToOpt = sub2ind([5,5], [1,3, 2,4, 1,3, 2,4], [1,1, 2,2, 3,3, 4,4]);

switch( lower(CalOptions.Phase) )
    case 'nodistort'
        CalOptions.DistortionParamsToOpt = [];
    otherwise
        CalOptions.DistortionParamsToOpt = 1:5;
end
if( isempty(EstCamDistortionV) && ~isempty(CalOptions.DistortionParamsToOpt) )
    EstCamDistortionV(CalOptions.DistortionParamsToOpt) = 0;
end
CalOptions.PreviousCamIntrinsics = EstCamIntrinsicsH;
CalOptions.PreviousCamDistortion = EstCamDistortionV;

fprintf('\n===Calibration refinement step, optimizing:===\n');
fprintf('    Intrinsics: ');
disp(CalOptions.IntrinsicsToOpt);
if( ~isempty(CalOptions.DistortionParamsToOpt) )
    fprintf('    Distortion: ');
    disp(CalOptions.DistortionParamsToOpt);
end

%---Compute initial error between projected and measured corner positions---
IdealChecker = [IdealChecker; ones(1,size(IdealChecker,2))]; % homogeneous coord

%---Encode params and grab info required to build Jacobian sparsity matrix---
CalOptions.NPoses = size(EstCamPosesV,1);
[Params0, ParamsInfo, JacobSensitivity] = EncodeParams( EstCamPosesV, EstCamIntrinsicsH, EstCamDistortionV, CalOptions );

[PtPlaneDist0,JacobPattern] = FindError( Params0, CheckerObs, IdealChecker, CalOptions, ParamsInfo, JacobSensitivity );
if( numel(PtPlaneDist0) == 0 )
    error('No valid grid points found -- possible grid parameter mismatch');
end

fprintf('\n    Start SSE: %g m^2, RMSE: %g m\n', sum((PtPlaneDist0).^2), sqrt(mean((PtPlaneDist0).^2)));

%---Start the optimization---
ObjectiveFunc = @(Params) FindError(Params, CheckerObs, IdealChecker, CalOptions, ParamsInfo, JacobSensitivity );
OptimOptions = optimset('Display','iter', ...
    'TolX', CalOptions.OptTolX, ...
    'TolFun',CalOptions.OptTolFun, ...
    'JacobPattern', JacobPattern, ...
    'PlotFcns', @optimplotfirstorderopt, ...
    'UseParallel', 'Always' );
[OptParams, ~, FinalDist] = lsqnonlin(ObjectiveFunc, Params0, [],[], OptimOptions);

%---Decode the resulting parameters and check the final error---
[EstCamPosesV, EstCamIntrinsicsH, EstCamDistortionV] = DecodeParams(OptParams, CalOptions, ParamsInfo);
fprintf(' ---Finished calibration refinement---\n');

fprintf('Estimate of camera intrinsics: \n');
disp(EstCamIntrinsicsH);
if( ~isempty( EstCamDistortionV ) )
    fprintf('Estimate of camera distortion: \n');
    disp(EstCamDistortionV);
end

ReprojectionError = struct( 'SSE', sum(FinalDist.^2), 'RMSE', sqrt(mean(FinalDist.^2)) );
fprintf('\n    Start SSE: %g m^2, RMSE: %g m\n    Finish SSE: %g m^2, RMSE: %g m\n', ...
    sum((PtPlaneDist0).^2), sqrt(mean((PtPlaneDist0).^2)), ...
    ReprojectionError.SSE, ReprojectionError.RMSE );

if( CalOptions.SaveResult )
    TimeStamp = datestr(now,'ddmmmyyyy_HHMMSS');
    GeneratedByInfo = struct('mfilename', mfilename, 'time', TimeStamp, 'VersionStr', LFToolboxVersion);

    SaveFname = fullfile(InputPath, CalOptions.CalInfoFname);
    fprintf('\nSaving to %s\n', SaveFname);

    LFWriteMetadata(SaveFname, LFVar2Struct(GeneratedByInfo, LensletGridModel, EstCamIntrinsicsH, EstCamDistortionV, EstCamPosesV, CamInfo, CalOptions, DecodeOptions, ReprojectionError));
end

end

%---------------------------------------------------------------------------------------------------
function [Params0, ParamsInfo, JacobSensitivity] = EncodeParams( EstCamPosesV, EstCamIntrinsicsH, EstCamDistortionV, CalOptions )
% This makes use of FlattenStruct to reversibly flatten all params into a single array.
% It also applies the same process to a sensitivity list, to facilitate building a Jacobian
% Sparisty matrix.

% The 'P' structure contains all the parameters to encode, and the 'J' structure mirrors it exactly
% with a sensitivity list. Each entry in 'J' lists those poses that are senstitive to the
% corresponding parameter. e.g. The first estimated camera pose affects only observations made
% within the first pose, and so the sensitivity list for that parameter lists only the first pose. A
% `J' value of 0 means all poses are sensitive to that variable -- as in the case of the intrinsics,
% which affect all observations.
P.EstCamPosesV = EstCamPosesV;
J.EstCamPosesV = zeros(size(EstCamPosesV));
for( i=1:CalOptions.NPoses )
    J.EstCamPosesV(i,:) = i;
end

P.IntrinParams = EstCamIntrinsicsH(CalOptions.IntrinsicsToOpt);
J.IntrinParams = zeros(size(CalOptions.IntrinsicsToOpt));

P.DistortParams = EstCamDistortionV(CalOptions.DistortionParamsToOpt);
J.DistortParams = zeros(size(CalOptions.DistortionParamsToOpt));

[Params0, ParamsInfo] = FlattenStruct(P);
JacobSensitivity = FlattenStruct(J);
end
%---------------------------------------------------------------------------------------------------
function [EstCamPosesV, EstCamIntrinsicsH, EstCamDistortionV] = DecodeParams( Params, CalOptions, ParamsInfo )
P = UnflattenStruct(Params, ParamsInfo);
EstCamPosesV = P.EstCamPosesV;

EstCamIntrinsicsH = CalOptions.PreviousCamIntrinsics;
EstCamIntrinsicsH(CalOptions.IntrinsicsToOpt) = P.IntrinParams;

EstCamDistortionV = CalOptions.PreviousCamDistortion;
EstCamDistortionV(CalOptions.DistortionParamsToOpt) = P.DistortParams;

EstCamIntrinsicsH = LFRecenterIntrinsics(EstCamIntrinsicsH, CalOptions.LFSize);
end

%---------------------------------------------------------------------------------------------------
function [Params, ParamInfo] = FlattenStruct(P)
Params = [];
ParamInfo.FieldNames = fieldnames(P);
for( i=1:length( ParamInfo.FieldNames ) )
    CurFieldName = ParamInfo.FieldNames{i};
    CurField = P.(CurFieldName);
    ParamInfo.SizeInfo{i} = size(CurField);
    Params = [Params; CurField(:)];
end
end
%---------------------------------------------------------------------------------------------------
function [P] = UnflattenStruct(Params, ParamInfo)
CurIdx = 1;
for( i=1:length( ParamInfo.FieldNames ) )
    CurFieldName = ParamInfo.FieldNames{i};
    CurSize = ParamInfo.SizeInfo{i};
    CurField = Params(CurIdx + (0:prod(CurSize)-1));
    CurIdx = CurIdx + prod(CurSize);
    CurField = reshape(CurField, CurSize);
    P.(CurFieldName) = CurField;
end
end

%---------------------------------------------------------------------------------------------------
function [PtPlaneDists, JacobPattern] = FindError(Params, CheckerObs, IdealChecker, CalOptions, ParamsInfo, JacobSensitivity )
    %---Decode optim params---
    [EstCamPosesV, EstCamIntrinsicsH, EstCamDistortionV] = DecodeParams(Params, CalOptions, ParamsInfo);

    %---Tally up the total number of observations---
    TotCornerObs = size( [CheckerObs{:,CalOptions.IJVecToOptOver,CalOptions.IJVecToOptOver}], 2 );
    CheckCornerObs = 0;

    %---Preallocate JacobPattern if it's requested---
    if( nargout >= 2 )
        JacobPattern = zeros(TotCornerObs, length(Params));
    end

    %---Preallocate point-plane distances---
    PtPlaneDists = zeros(1, TotCornerObs);

    %---Compute point-plane distances---
    OutputIdx = 0;
    for( PoseIdx = 1:CalOptions.NPoses )
        %---Convert the pertinent camera pose to a homogeneous transform---
        CurEstCamPoseV = squeeze(EstCamPosesV(PoseIdx, :));
        CurEstCamPoseH = eye(4);
        CurEstCamPoseH(1:3,1:3) = rodrigues(CurEstCamPoseV(4:6));
        CurEstCamPoseH(1:3,4) = CurEstCamPoseV(1:3);

        %---Iterate through the corners---
        for( TIdx = CalOptions.IJVecToOptOver )
            for( SIdx = CalOptions.IJVecToOptOver )
                CurCheckerObs = CheckerObs{PoseIdx, TIdx,SIdx};
                NCornerObs = size(CurCheckerObs,2);
                if( NCornerObs ~= prod(CalOptions.ExpectedCheckerSize) )
                    continue; % this implementation skips incomplete observations
                end
                CheckCornerObs = CheckCornerObs + NCornerObs;

                %---Assemble observed corner positions into complete 4D [i,j,k,l] indices---
                CurCheckerObs_Idx = [repmat([SIdx;TIdx], 1, NCornerObs); CurCheckerObs; ones(1, NCornerObs)];

                %---Transform ideal 3D corner coords into camera's reference frame---
                IdealChecker_CamFrame = CurEstCamPoseH * IdealChecker;
                IdealChecker_CamFrame = IdealChecker_CamFrame(1:3,:); % won't be needing homogeneous points

                %---Project observed corner indices to [s,t,u,v] rays---
                CurCheckerObs_Ray = EstCamIntrinsicsH * CurCheckerObs_Idx;

                %---Apply direction-dependent distortion model---
                if( ~isempty(EstCamDistortionV) && any(EstCamDistortionV(:)~=0))
                    k1 = EstCamDistortionV(1);
                    k2 = EstCamDistortionV(2);
                    k3 = EstCamDistortionV(3);
                    b1dir = EstCamDistortionV(4);
                    b2dir = EstCamDistortionV(5);
                    Direction = CurCheckerObs_Ray(3:4,:);
                    Direction = bsxfun(@minus, Direction, [b1dir;b2dir]);
                    DirectionR2 = sum(Direction.^2);
                    Direction = Direction .* repmat((1 + k1.*DirectionR2 + k2.*DirectionR2.^2 + k3.*DirectionR2.^3),2,1);
                    Direction = bsxfun(@plus, Direction, [b1dir;b2dir]);
                    CurCheckerObs_Ray(3:4,:) = Direction;
                end

                %---Find 3D point-ray distance---
                STPlaneIntersect = [CurCheckerObs_Ray(1:2,:); zeros(1,NCornerObs)];
				% Here interpret u,v as relative, at a distance of 1 m
				% Thus we use a relative 2pp, with D = 1m.
                RayDir = [CurCheckerObs_Ray(3:4,:); ones(1,NCornerObs)];
                CurDist3D = LFFind3DPtRayDist( STPlaneIntersect, RayDir, IdealChecker_CamFrame );

                PtPlaneDists(OutputIdx + (1:NCornerObs)) = CurDist3D;

                if( nargout >=2 )
                    % Build the Jacobian pattern. First we enumerate those observations related to
                    % the current pose, then find all parameters to which those observations are
                    % sensitive. This relies on the JacobSensitivity list constructed by the
                    % FlattenStruct function.
                    CurObservationList = OutputIdx + (1:NCornerObs);
                    CurSensitivityList = (JacobSensitivity==PoseIdx | JacobSensitivity==0);
                    JacobPattern(CurObservationList, CurSensitivityList) = 1;
                end
                OutputIdx = OutputIdx + NCornerObs;
            end
        end
    end

    %---Check that the expected number of observations have gone by---
    if( CheckCornerObs ~= TotCornerObs )
        error(['Mismatch between expected (%d) and observed (%d) number of corners' ...
            ' -- possibly caused by a grid parameter mismatch'], TotCornerObs, CheckCornerObs);
    end
end

%---Compute distances from 3D rays to a 3D points---
function [Dist] = LFFind3DPtRayDist( PtOnRay, RayDir, Pt3D )

RayDir = RayDir ./ repmat(sqrt(sum(RayDir.^2)), 3,1); % normalize ray
Pt3D = Pt3D - PtOnRay;    % Vector to point

PD1 = dot(Pt3D, RayDir);
PD1 = repmat(PD1,3,1).*RayDir; % Project point vec onto ray vec

Pt3D = Pt3D - PD1;
Dist = sqrt(sum(Pt3D.^2, 1)); % Distance from point to projected point

end


function treepack_test11 ( )

%*****************************************************************************80
%
%% TREEPACK_TEST11 tests TREE_RB_ENUM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 June 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TREEPACK_TEST11\n' );
  fprintf ( 1, '  TREE_RB_ENUM enumerates the rooted binary trees on a \n' );
  fprintf ( 1, '  given number of nodes.\n' );
  fprintf ( 1, '\n' );

  for nnode = 0 : 11

    num = tree_rb_enum ( nnode );

    fprintf ( 1, '  %8d  %8d\n', nnode, num );

  end

  return
end


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

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

if ~isempty(handles.V.Segs)
    choice = questdlg('Segmentation mask is detected, transform operation will reset them, preceed?','Mask Reset', ...
                      'No, go save mask','Yes','No, go save mask');
    if isempty(choice)
        warndlg('Transform is cancelled.');
        return;
    end
    % Handle response
    switch choice
        case 'No, go save mask'
            warndlg('Save your mask before transform.');
            return;
    end

    handles.Mask=handles.Mask*0;
    handles.V.Segs=[];
end
% close 3D slicer
global Figure_handles
if isfield(Figure_handles,'MU_display2')
    slicer_display_handles=guidata(Figure_handles.MU_display2);
    if Figure_handles.MU_display == slicer_display_handles.Parent
        close(Figure_handles.MU_display2);
    end
end

if numel(handles.V.DimSize) == 3
    defaultOrigin = [ceil(handles.V.DimSize(2)/2) ceil(handles.V.DimSize(1)/2) ceil(handles.V.DimSize(3)/2)];
else
    defaultOrigin = [ceil(handles.V.DimSize(2)/2) ceil(handles.V.DimSize(1)/2) 0];
end

Input = inputdlg({'Please input a rotation origin [x,y,z].'; 'Please input a rotation axis [x,y,z].'; 'Please input a rotation angle (rad).'},'Input Value',1, ...
                 {['[' num2str(defaultOrigin(1)) ',' num2str(defaultOrigin(2)) ',' num2str(defaultOrigin(3)) ']'],'[0,0,1]','0'});
if isempty(Input)
    warndlg('Matrix rotation was cancelled.');
    return;
end

[Type,ok] = listdlg('ListString',{'Nearest','Linear','Cubic'}, ...
                    'SelectionMode','single',...
                    'PromptString','Interpolation Method',...
                    'Name','Interpolation');
if ok==0
    warndlg('Matrix rotation was cancelled.');
    return;
end

switch Type
    case 1
        interp = 'nearest';
    case 2
        interp = 'linear';
    case 3
        interp = 'cubic';
end

try
    MU_update_waitbar(handles.Progress_axes,1,3);
    pause(0.1);
    eval(['Ori=' Input{1} ';']);
    eval(['Axis=' Input{2} ';']);
    eval(['Angle=' Input{3} ';']);
    if Angle==0
        MU_update_waitbar(handles.Progress_axes,3,3);
        return;
    end
    [T, R]=rotate3DT_MU(Ori, Axis, Angle,interp);
    handles.TMatrix =rotate3D(handles.TMatrix, T, R);
    MU_update_waitbar(handles.Progress_axes,2,3);
    handles.Mask =round(rotate3D(handles.Mask, T, R));
    MergeM=get(handles.Matrix_name_edit,'String');
    set(handles.Matrix_name_edit,'String',[MergeM '_rot']);
    handles=MU_update_image(handles.Matrix_display_axes,{handles.TMatrix,handles.Mask},handles,0);
    MU_update_waitbar(handles.Progress_axes,3,3);
catch me
    errordlg('The input rotation value is invalid.');
    return;
end

guidata(handles.MU_matrix_display, handles);

end


function value = sweet2_condition ( )

%*****************************************************************************80
%
%% SWEET2_CONDITION returns the L1 condition of the SWEET2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 February 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VALUE, the L1 condition.
%
  a_norm = 30.733333333333334;
  b_norm = 1.601605164968818;
  value = a_norm * b_norm;

  return
end


% clear; close all; clc;

fs = 200;
t = 0:1/fs:5-1/fs;
x = sin(2*pi*1*t);

figure;
set(gcf,'position',[500 500 640 480]);
figsize = get(gcf,'position');
h = plot(t,x);

SliderH = uicontrol('style','slider','position',[70 6 512 20],'min',1,'max',5,'value',1);

while(1)
    plot(t, sin(2*pi*SliderH.Value*t));
    pause(0.01);
    cla
end


classdef FDV < ALGORITHM
% <multi/many> <real/integer> <large/none>
% Fuzzy decision variable framework with various internal optimizers
% Rate      --- 0.8 --- Fuzzy evolution rate. Default = 0.8
% Acc       --- 0.4 --- Step acceleration. Default = 0.4
% optimizer ---   5 --- Internal optimisation algorithm. 1 = NSGA-II, 2 = NSGA-III, 3 = MOEA/D, 4 = CMOPSO, 5 = LMOCSO
% type      ---   1 --- The type of aggregation function for MOEA/D

%------------------------------- Reference --------------------------------
% X. Yang, J. Zou, S. Yang, J. Zheng, and Y. Liu, A fuzzy decision
% variables framework for large-scale multiobjective optimization, IEEE
% Transactions on Evolutionary Computation, 2021.
%--------------------------------------------------------------------------

%  Copyright (C) 2021 Xu Yang
%  Xu Yang <xuyang.busyxu@qq.com> or <xuyang369369@gmail.com>

    methods
        function main(Algorithm,Problem)
            %% Set the default parameters
            [Rate,Acc,optimizer,type] = Algorithm.ParameterSet(0.8,0.4,5,1);

            %% NSGAII
            if optimizer==1
                % Generate random population
                Population = Problem.Initialization();
                [~,FrontNo,CrowdDis] = EnvironmentalSelection_NSGAII(Population,Problem.N);

                % Optimization
                while Algorithm.NotTerminated(Population)
                    MatingPool = TournamentSelection(2,Problem.N,FrontNo,-CrowdDis);
                    OffDec     = OperatorGA(Problem,Population(MatingPool).decs);
                    %% FDV
                    if Problem.FE/Problem.maxFE <= Rate
                        Offspring = FDVOperator(Problem,Rate,Acc,OffDec);
                    else
                        Offspring = Problem.Evaluation(OffDec);
                    end
                    %%
                    [Population,FrontNo,CrowdDis] = EnvironmentalSelection_NSGAII([Population,Offspring],Problem.N);
                end
            end

            %% NSGAIII
            if optimizer==2
                % Generate the reference points and random population
                [Z,Problem.N] = UniformPoint(Problem.N,Problem.M);
                Population    = Problem.Initialization();
                Zmin          = min(Population(all(Population.cons<=0,2)).objs,[],1);

                % Optimization
                while Algorithm.NotTerminated(Population)
                    MatingPool = TournamentSelection(2,Problem.N,sum(max(0,Population.cons),2));
                    OffDec     = OperatorGA(Problem,Population(MatingPool).decs);
                    %% FDV
                    if Problem.FE/Problem.maxFE <= Rate
                        Offspring = FDVOperator(Problem,Rate,Acc,OffDec);
                    else
                        Offspring = Problem.Evaluation(OffDec);
                    end
                    Zmin       = min([Zmin;Offspring(all(Offspring.cons<=0,2)).objs],[],1);
                    Population = EnvironmentalSelection_NSGAIII([Population,Offspring],Problem.N,Z,Zmin);
                end
            end

            %% MOEA/D
            if optimizer==3
                % Generate the weight vectors
                [W,Problem.N] = UniformPoint(Problem.N,Problem.M);
                T = ceil(Problem.N/10);

                % Detect the neighbours of each solution
                B = pdist2(W,W);
                [~,B] = sort(B,2);
                B = B(:,1:T);

                % Generate random population
                Population = Problem.Initialization();
                Z = min(Population.objs,[],1);

                % Optimization
                while Algorithm.NotTerminated(Population)
                    % For each solution
                    for i = 1 : Problem.N
                        % Choose the parents
                        P = B(i,randperm(size(B,2)));

                        % Generate an offspring
                        OffDec = OperatorGAhalf(Problem,Population(P(1:2)).decs);
                        %% FDV
                        if Problem.FE/Problem.maxFE <= Rate
                            Offspring = FDVOperator(Problem,Rate,Acc,OffDec);
                        else
                            Offspring = Problem.Evaluation(OffDec);
                        end

                        % Update the ideal point
                        Z = min(Z,Offspring.obj);

                        % Update the neighbours
                        switch type
                            case 1
                                % PBI approach
                                normW   = sqrt(sum(W(P,:).^2,2));
                                normP   = sqrt(sum((Population(P).objs-repmat(Z,T,1)).^2,2));
                                normO   = sqrt(sum((Offspring.obj-Z).^2,2));
                                CosineP = sum((Population(P).objs-repmat(Z,T,1)).*W(P,:),2)./normW./normP;
                                CosineO = sum(repmat(Offspring.obj-Z,T,1).*W(P,:),2)./normW./normO;
                                g_old   = normP.*CosineP + 5*normP.*sqrt(1-CosineP.^2);
                                g_new   = normO.*CosineO + 5*normO.*sqrt(1-CosineO.^2);
                            case 2
                                % Tchebycheff approach
                                g_old = max(abs(Population(P).objs-repmat(Z,T,1)).*W(P,:),[],2);
                                g_new = max(repmat(abs(Offspring.obj-Z),T,1).*W(P,:),[],2);
                            case 3
                                % Tchebycheff approach with normalization
                                Zmax  = max(Population.objs,[],1);
                                g_old = max(abs(Population(P).objs-repmat(Z,T,1))./repmat(Zmax-Z,T,1).*W(P,:),[],2);
                                g_new = max(repmat(abs(Offspring.obj-Z)./(Zmax-Z),T,1).*W(P,:),[],2);
                            case 4
                                % Modified Tchebycheff approach
                                g_old = max(abs(Population(P).objs-repmat(Z,T,1))./W(P,:),[],2);
                                g_new = max(repmat(abs(Offspring.obj-Z),T,1)./W(P,:),[],2);
                        end
                        Population(P(g_old>=g_new)) = Offspring;
                    end
                end
            end

            %% CMOPSO
            if optimizer == 4
                % Generate random population
                Population = Problem.Initialization();

                % Optimization
                while Algorithm.NotTerminated(Population)
                    [OffDec,OffVel] = Operator_CMOPSO(Problem,Population);
                    %% FDV
                    if Problem.FE/Problem.maxFE <= Rate
                        Offspring = FDVOperator(Rate,Acc,OffDec,OffVel);
                    else
                        Offspring = Problem.Evaluation(OffDec,OffVel);
                    end
                    Population = EnvironmentalSelection_CMOPSO([Population,Offspring],Problem.N);
                end
            end

            %% LMOCSO
            if optimizer == 5
                 % Generate random population
                [V,Problem.N] = UniformPoint(Problem.N,Problem.M);
                Population    = Problem.Initialization();
                Population    = EnvironmentalSelection_LMOCSO(Population,V,(Problem.FE/Problem.maxFE)^2);

                % Optimization
                while Algorithm.NotTerminated(Population)
                    % Calculate the fitness by shift-based density   SDE (the shift-based density estimation strategy)
                    PopObj = Population.objs;
                    N      = size(PopObj,1);
                    fmax   = max(PopObj,[],1);
                    fmin   = min(PopObj,[],1);
                    PopObj = (PopObj-repmat(fmin,N,1))./repmat(fmax-fmin,N,1);
                    Dis    = inf(N);
                    for i = 1 : N
                        SPopObj = max(PopObj,repmat(PopObj(i,:),N,1));
                        for j = [1:i-1,i+1:N]
                            Dis(i,j) = norm(PopObj(i,:)-SPopObj(j,:));
                        end
                    end
                    Fitness = min(Dis,[],2);

                    if length(Population) >= 2
                        Rank = randperm(length(Population),floor(length(Population)/2)*2);
                    else
                        Rank = [1,1];
                    end
                    Loser  = Rank(1:end/2);
                    Winner = Rank(end/2+1:end);
                    Change = Fitness(Loser) >= Fitness(Winner);
                    Temp   = Winner(Change);
                    Winner(Change) = Loser(Change);
                    Loser(Change)  = Temp;

                    [OffDec,OffVel] = Operator_LMOCSO(Problem,Population(Loser),Population(Winner),Rate);
                    %% FDV
                    iter = Problem.FE/Problem.maxFE;
                    if iter <= Rate
                        Offspring = FDVOperator(Problem,Rate,Acc,OffDec,OffVel);
                    else
                        Offspring = Problem.Evaluation(OffDec,OffVel);
                    end
                    Population = EnvironmentalSelection_LMOCSO([Population,Offspring],V,(Problem.FE/Problem.maxFE)^2);
                end
            end
        end
    end
end


function varargout = log2(varargin)

varargout{1} = log(varargin{1})/log(2);


function zz=v_lpccw2zz(cw)
%V_LPCPZ2ZZ LPC: Power spectrum roots to LPC poles ZZ=(CW)
% pz are the roots of the power spectrum polynomial pp(cos(w))

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_lpccw2zz.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

zs=sqrt(cw.^2-1);
zz=cw-sign(real(conj(cw).*zs)).*zs;


function [dc, dh, d_emb, grad_W_rnn, d_feed_input] = rnnStepLayerBackprop(W_rnn, prev_state, cur_state, cur_top_grad, dc, dh, ...
  maskInfo, params, isFeedInput)
% Backprop multi-layer RNN for one time step.
% Input:
%   W_rnn: recurrent connections of multiple layers, e.g., W_rnn{ll}.
%   prevState: previous hidden state, e.g., for LSTM, prevState.c{ll}, prevState.h{ll}.
%   curState: current hidden state, e.g., for LSTM, curState.c{ll}, curState.h{ll}.
%
% Output:
%
% Thang Luong @ 2015, <lmthang@stanford.edu>

numLayers = length(W_rnn);

unmaskedIds = maskInfo.unmaskedIds;
maskedIds = maskInfo.maskedIds;
grad_W_rnn = cell(numLayers, 1);
for ll=numLayers:-1:1 % layer
  % add grad from the top
  if ~isempty(cur_top_grad)
    dh{ll}(:, unmaskedIds) = dh{ll}(:, unmaskedIds) + cur_top_grad(1:params.lstmSize, unmaskedIds);
  end

  % cell backprop
  [dc{ll}, dh{ll}, d_input, grad_W_rnn{ll}] = lstmUnitBackprop(W_rnn{ll}, cur_state{ll}, prev_state{ll}.c_t, dc{ll}, dh{ll}, maskedIds, ...
    params, ll==1 && isFeedInput);

  % pass down hidden state grad to the below layer
  cur_top_grad = d_input;
end

% emb
d_emb = d_input(1:params.lstmSize, :);

% feed softmax vector
if isFeedInput
  d_feed_input = d_input(params.lstmSize+1:2*params.lstmSize, :);
else
  d_feed_input = [];
end


function result=lvq_predict(P,Tc,Num_Compet,w1,w2)
n=size(P,2);
result=zeros(2,n);
result(1,:)=Tc;
for i=1:n
    d=zeros(Num_Compet,1);
    for j=1:Num_Compet
        d(j)=sqrt(sse(w1(j,:)'-P(:,i)));
    end
    n1=compet(-1*d);
    n2=purelin(w2*n1);
    result(2,i)=vec2ind(n2);
end
Num_Correct=length(find(result(2,:)==Tc));
accuracy=Num_Correct/n;
disp(['accuracy=' num2str(accuracy*100) '%(' num2str(Num_Correct) '/' num2str(n) ')']);


% given an ND tensor of size DxN1xN2xN3.., compute the outer product of the
% first dimension independently to return DxDxN1xN2xN3...
%
function output = outProdND(data)

[D,N1,N2,N3] = size(data);
A = reshape(data,D, N1*N2*N3);

% B = permute(bsxfun(@times, A, conj(permute(A,[3 2 1]))), [1 3 2]); % slower
B = bsxfun(@times, permute(A, [1 3 2]), permute(conj(A), [3 1 2]));

output = reshape(B, D,D,N1,N2,N3);

end


function [] = showChannelTrend(EEG, EEGChannels, displayChannels, seconds)
%% Displays the trend line for EEG over an interval
%
% Parameters
%    EEG              EEGLAB EEG structure
%    EEGChannels      vector numbers of channels corresponding to EEG
%    displayChannels  vector containing list of channels to visualize
%    seconds          interval (in seconds) to analyze (empty = all)
%
% This function shows a mumber of visualizations both in time and
% frequency space
%

%% Set up the data to be visualized
EEGOrig = EEG;
t = 0:(size(EEGOrig.data, 2) - 1);
t = t/EEGOrig.srate;
if ~isempty(seconds)
    tIndex = (t >= seconds(1) & t <= seconds(2));
    t = t(tIndex);
    EEGOrig.data = EEGOrig.data(:, tIndex);
    EEGOrig.pnts = sum(tIndex);
    EEGOrig.times = EEGOrig.times(tIndex);
end

%% Display channels must be EEG channels
displayChannels = intersect(EEGChannels, displayChannels);

%% Calculate the correlations and the models

myData = EEGOrig.data;
myPval = zeros(size(myData, 1), 2);
myCorr = zeros(size(myData, 1), 1);
for k = 1:size(myData, 1)
  myPval(k, :) = polyfit(t, myData(k, :), 1);
  myCorr(k) = corr(t', (myData(k, :))');
end

%% Display the correlation values and the polynomial coefficients
cTitle = 'Summary of trends for EEG channels';
figure('Color', [1, 1, 1], 'Name', cTitle)
hold on
line([-1, 1], [0, 0], 'Color', [.85, .85, .85], 'LineWidth', 3);
plot(myCorr(EEGChannels), myPval(EEGChannels, 1), 'xk', 'MarkerSize', 12, 'LineWidth', 2)
xlabel('Channel-trend correlation')
ylabel('Slope of trend')
title(cTitle)
box on
hold off

%%
for k = 1:length(displayChannels)
    chan = displayChannels(k);
    myTitle = ['Channel ' num2str(chan) ' (corr=' num2str(myCorr(chan)) ')'];
    figure('Color', [1, 1, 1], 'Name', myTitle)
    plot(t, myData(chan, :), '-k')
    t1 = get(gca, 'XLim');
    hold on
    line(t1, myPval(chan, 1)*t1 + myPval(chan, 2), 'Color', [1, 0, 0], 'LineWidth', 2);
    xlabel('Seconds')
    ylabel('Voltage (uV)')
    hold off
    box on
end

%% Display the correlation values and the polynomial coefficients
%% Parameters that must be preset
chans = displayChannels;
EEGSmall = EEGOrig;
EEGSmall.nbchan = length(chans);
EEGSmall.data = EEGOrig.data(chans, :);
params.detrendType = 'high pass';
params.detrendCutoff = 1;
[EEGLine, lineNoiseOut] = cleanLineNoise(EEGSmall, params);
EEGLineFilt = removeTrend(EEGLine, params);
EEGFilt = removeTrend(EEGSmall, params);
EEGFiltLine = cleanLineNoise(EEGFilt, params);
%% Remove from model
t = 0:(size(EEGSmall.data, 2) - 1);
t = t/EEGSmall.srate;
EEGModel = EEGSmall;
for k = 1:length(chans)
  ch = chans(k);
  myModel = polyval(myPval(ch, :), t);
  EEGModel.data(k, :) = EEGModel.data(k, :) - myModel;
end
%%
EEGModelLine = cleanLineNoise(EEGModel, params);
EEGModelLineFilt = removeTrend(EEGModelLine, params);

%% Compute the filtering for models and EEG
SOrig = cell(length(chans), 1);
fOrig = cell(length(chans), 1);
SFilt = cell(length(chans), 1);
fFilt = cell(length(chans), 1);
SFiltLine = cell(length(chans), 1);
fFiltLine = cell(length(chans), 1);
SLine = cell(length(chans), 1);
fLine = cell(length(chans), 1);
SLineFilt = cell(length(chans), 1);
fLineFilt = cell(length(chans), 1);
SModel = cell(length(chans), 1);
fModel = cell(length(chans), 1);
SModelLine = cell(length(chans), 1);
fModelLine = cell(length(chans), 1);
SModelLineFilt = cell(length(chans), 1);
fModelLineFilt = cell(length(chans), 1);
winSize = lineNoiseOut.taperWindowSize;
sParams = lineNoiseOut;
for k = 1:length(chans)
   [SOrig{k},fOrig{k}] = mtspectrumsegc(EEGSmall.data(k, :), winSize, sParams);
   [SLine{k},fLine{k}] = mtspectrumsegc(EEGLine.data(k, :), winSize, sParams);
   [SLineFilt{k},fLineFilt{k}] = mtspectrumsegc(EEGLineFilt.data(k, :), winSize, sParams);
   [SFilt{k},fFilt{k}] = mtspectrumsegc(EEGFilt.data(k, :), winSize, sParams);
   [SFiltLine{k},fFiltLine{k}] = mtspectrumsegc(EEGFiltLine.data(k, :), winSize, sParams);
   [SModel{k},fModel{k}] = mtspectrumsegc(EEGModel.data(k, :), winSize, sParams);
   [SModelLine{k},fModelLine{k}] = mtspectrumsegc(EEGModelLine.data(k, :), winSize, sParams);
   [SModelLineFilt{k},fModelLineFilt{k}] = mtspectrumsegc(EEGModelLineFilt.data(k, :), winSize, sParams);
end
%% Figures with the entire spectral range and all filtering variations
thisName = EEG.setname;
colors = jet(10);
for k = 1:length(chans)
    tString = [thisName ': Channel ' num2str(chans(k)) ' (all versions)'];
    figure('Name', tString, 'Color', [1, 1, 1])
    hold on
    plot(fOrig{k}, 10*log10(SOrig{k}), 'Color', [0.85, 0.85, 0.85], 'LineWidth', 3', 'LineStyle', '-')
    plot(fLine{k}, 10*log10(SLine{k}), 'Color', [0, 0, 0])
    plot(fLineFilt{k}, 10*log10(SLineFilt{k}), 'Color', colors(1, :))
    plot(fFilt{k}, 10*log10(SFilt{k}), 'Color', colors(2, :))
    plot(fFiltLine{k}, 10*log10(SFiltLine{k}), 'Color', colors(4, :))
    plot(fModel{k}, 10*log10(SModel{k}), 'Color', colors(6, :));
    plot(fModelLine{k}, 10*log10(SModelLine{k}), 'Color', colors(7, :))
    plot(fModelLineFilt{k}, 10*log10(SModelLineFilt{k}), 'Color', colors(10, :)')
    legend('Orig', 'Line', 'L-Filt', 'Filt', 'Filt-L', 'Model', 'Model-L', 'Model-LF');
    hold off
    xlabel('Hz')
    ylabel('Power')
    title(tString, 'Interpreter', 'none')
    box on
end
%% Figures with just interaction of filtering and line noise -- limited range
for k = 1:length(chans)
    tString = [thisName ': Channel ' num2str(chans(k)) ' (limited spectrum)'];
    figure('Name', tString, 'Color', [1, 1, 1])
    hold on
    plot(fOrig{k}, 10*log10(SOrig{k}), 'Color', [0.7, 0.7, 0.7], 'LineWidth', 3, 'LineStyle', '-')
    plot(fFiltLine{k}, 10*log10(SFiltLine{k}), 'Color', [0.85, 0.85, 0.85], 'LineWidth', 6,  'LineStyle', '-')
    plot(fLineFilt{k}, 10*log10(SLineFilt{k}), 'Color', [0, 0, .8])
    plot(fModelLineFilt{k}, 10*log10(SModelLineFilt{k}), 'Color', [1, 0.2, 0.2])
    legend('Orig', 'Filt-L', 'L-Filt',  'Model-LF');
    hold off
    xlabel('Hz')
    ylabel('Power')
    set(gca, 'XLim', [0, 110], 'XLimMode', 'manual')
    title(tString, 'Interpreter', 'none')
    box on
end