function [Population,succ1,succ2,Fitness] = First_Stage_EnvironmentalSelection(Population,N,isOrigin)
% The environmental selection of SPEA2

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

% This function is written by Kangjia Qiao

    %% Calculate the fitness of each solution
    if isOrigin==1
        Fitness = CalFitness(Population.objs,Population.cons);
    else
        Fitness = CalFitness(Population.objs);
    end

    %% Environmental selection
    Next = Fitness < 1;
    if sum(Next) < N
        [~,Rank] = sort(Fitness);
        Next(Rank(1:N)) = true;
    elseif sum(Next) > N
        Del  = Truncation(Population(Next).objs,sum(Next)-N);
        Temp = find(Next);
        Next(Temp(Del)) = false;
    end

    % Population for next generation
    Population = Population(Next);
    Fitness    = Fitness(Next);
    % Sort the population
    [Fitness,rank] = sort(Fitness);
    Population = Population(rank);

    %% calculate success rate1
    off_index = Next(1+N:2*N);
    succ1 = zeros(1,length(off_index));
    for j = 1:length(off_index)
        if off_index(j) == 1
            succ1(j)=1;
        end
    end
    %% calculate success rate2
    off_index = Next(1+2*N:end);
    succ2 = zeros(1,length(off_index));
    for j = 1:length(off_index)
        if off_index(j) == 1
            succ2(j)=1;
        end
    end
end

function Del = Truncation(PopObj,K)
% Select part of the solutions by truncation

    %% Truncation
    Distance = pdist2(PopObj,PopObj);
    Distance(logical(eye(length(Distance)))) = inf;
    Del = false(1,size(PopObj,1));
    while sum(Del) < K
        Remain   = find(~Del);
        Temp     = sort(Distance(Remain,Remain),2);
        [~,Rank] = sortrows(Temp);
        Del(Remain(Rank(1))) = true;
    end
end


%NUMROWS Number of rows in matrix
%
% NR = NUMROWS(M) is the number of rows in the matrix M.
%
% Notes::
% - Readable shorthand for SIZE(M,1);
%
% See also NUMCOLS, SIZE.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

function r = numrows(m)

	r = size(m, 1);


%% Copyright (C) 2016 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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 software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym besselj (@var{alpha}, @var{x})
%% Symbolic Bessel function of the first kind.
%%
%% Example:
%% @example
%% @group
%% syms n x
%% A = 2*besselj(n,x)
%%   @result{} A = (sym) 2⋅besselj(n, x)
%% diff(A)
%%   @result{} (sym) besselj(n - 1, x) - besselj(n + 1, x)
%% @end group
%% @end example
%%
%% @seealso{@@sym/bessely, @@sym/besseli, @@sym/besselk}
%% @end defmethod

function J = besselj(n, x)

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

  J = elementwise_op ('besselj', sym(n), sym(x));

end

%!test
%! X = [1 2 3; 4 5 6];
%! ns = [sym(0) 1 -2; sym(1)/2 -sym(3)/2 pi];
%! n = double(ns);
%! A = double(besselj(ns, X));
%! B = besselj(n, X);
%! assert (all (all (abs (A - B) < 50*eps*abs(A))))

%!test
%! % roundtrip
%! syms x
%! A = besselj(2, 10);
%! q = besselj(2, x);
%! h = function_handle(q);
%! B = h(10);
%! assert (abs (A - B) <= eps*abs(A))

%!error besselj(sym('x'))


% SP_EVALUATE_ELEMENT_LIST: compute the basis functions in a given list of elements.
%
%     sp = sp_evaluate_element_list (space, msh_elems, 'option1', value1, ...)
%
% INPUTS:
%
%    space:     object defining the space of discrete functions (see sp_vector)
%    msh_elems: msh structure containing the information of quadrature or
%               visualization points, for a given list of elements
%               (see msh_cartesian/msh_evaluate_element_list)
%   'option', value: additional optional parameters, currently available options are:
%
%              Name     |   Default value |  Meaning
%           ------------+-----------------+----------------------------------
%            value      |      true       |  compute shape_functions
%            gradient   |      false      |  compute shape_function_gradients
%            divergence |      false      |  compute shape_function_divs
%            curl       |      false      |  compute shape_function_curls
%
% OUTPUT:
%
%    sp: struct representing the discrete function space, with the following fields:
%              (see the article for a detailed description)
%
%    FIELD_NAME      (SIZE)                                     DESCRIPTION
%    ncomp           (scalar)                                   number of components of the functions of the space
%    ndof            (scalar)                                   total number of degrees of freedom
%    ndof_dir        (ncomp x ndim matrix)                      for each component, number of degrees of freedom along each direction
%    nsh_max         (scalar)                                   maximum number of shape functions per element
%    nsh             (1 x msh_col.nel vector)                   actual number of shape functions per each element
%    connectivity    (nsh_max x msh_col.nel vector)             indices of basis functions that do not vanish in each element
%    shape_functions (ncomp x msh_col.nqn x nsh_max x msh_col.nel)  basis functions evaluated at each quadrature node in each element
%    shape_function_gradients
%       (ncomp x rdim x msh_col.nqn x nsh_max x msh_col.nel)    basis function gradients evaluated at each quadrature node in each element
%    shape_function_divs (msh_col.nqn x nsh_max x msh_col.nel)  basis function divergence evaluated at each quadrature node in each element
%    shape_function_curls
%         2D:  (msh_col.nqn x nsh_max x msh_col.nel)            basis function curl evaluated at each quadrature node in each element
%         3D:  (3 x msh_col.nqn x nsh_max x msh_col.nel)
%
% Copyright (C) 2009, 2010, 2011 Carlo de Falco
% Copyright (C) 2011, 2015, 2019 Rafael Vazquez
%
%    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/>.

function sp = sp_evaluate_element_list (space, msh, varargin)

value = true;
gradient = false;
divergence = false;
curl = false;
hessian = false;
if (~isempty (varargin))
  if (~rem (length (varargin), 2) == 0)
    error ('sp_evaluate_element_list: options must be passed in the [option, value] format');
  end
  for ii=1:2:length(varargin)-1
    if (strcmpi (varargin {ii}, 'value'))
      value = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'gradient'))
      gradient = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'curl'))
      curl = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'divergence'))
      divergence = varargin {ii+1};
    elseif (strcmpi (varargin {ii}, 'hessian'))
      hessian = varargin {ii+1};
    else
      warning ('Ignoring unknown option %s', varargin {ii});
    end
  end
end

grad_param = gradient || divergence || curl || hessian;
value_param = value || grad_param;
div_param = false; curl_param = false;
switch (lower (space.transform))
  case {'curl-preserving'}
    curl_param = curl;
  case {'div-preserving'}
    div_param = divergence;
end

sp = sp_evaluate_element_list_param (space, msh, 'value', value_param, 'gradient', grad_param, 'divergence', div_param, 'curl', curl_param, 'hessian', hessian);

if (isempty (msh.elem_list))
  return
end

switch (lower (space.transform))
  case {'grad-preserving'}
    sp = sp_vector_grad_preserving_transform (sp, msh, value, gradient, curl, divergence, hessian);
  case {'curl-preserving'}
    sp = sp_vector_curl_preserving_transform (sp, msh, value, curl);
    if (gradient || divergence || hessian)
      warning ('Gradient, divergence and hessian not implemented for curl-preserving transformation')
    end
  case {'div-preserving'}
    sp = sp_vector_div_preserving_transform (sp, msh, value, gradient, curl, divergence);
    if (hessian)
      warning ('Hessian not implemented for div-preserving transformation')
    end
end

end


function fnew=removeisolatedsurf(v,f,maxdiameter)
%
% fnew=removeisolatedsurf(v,f,maxdiameter)
%
% remove disjointed surface fragment filtered by using mesh diameter
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%    v: list of nodes of the input surface
%    f: list of triangles of the input surface
%    maxdiameter: maximum bounding box size for surface removal
%
% ouput:
%    fnew: new face list after removing the components smaller than
%          maxdiameter
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

fc=finddisconnsurf(f);
for i=1:length(fc)
    xdia=v(fc{i},1);
    xdia=max(xdia(:))-min(xdia(:));
    if(xdia<=maxdiameter) fc{i}=[]; continue; end
    ydia=v(fc{i},2);
    ydia=max(ydia(:))-min(ydia(:));
    if(ydia<=maxdiameter) fc{i}=[]; continue; end
    zdia=v(fc{i},3);
    zdia=max(zdia(:))-min(zdia(:));
    if(zdia<=maxdiameter) fc{i}=[]; continue; end
end
fnew=[];
for i=1:length(fc)
	if(length(fc{i})) fnew=[fnew;fc{i}]; end
end
if(size(fnew,1)~=size(f,1))
   fprintf(1,'removed %d elements of small isolated surfaces',size(f,1)-size(fnew,1));
end


% Short test script for running algorithms related to the glottal source
%
% Please find the path management in the startup.m script in the root directory
% of this repository. Note that by starting matlab in the root directory, this
% script should automatically run. If it is not the case, you can also move to the
% root directory and run this script manually.
%
% 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
%

clear all;

% Settings
F0min = 80; % Minimum F0 set to 80 Hz
F0max = 500; % Maximum F0 set to 80 Hz
frame_shift = 10; % Frame shift in ms

% Load soundfile
[x,fs] = audioread(['howtos' filesep 'arctic_a0007.wav']);

% Check the speech signal polarity
polarity = polarity_reskew(x,fs);
x=polarity*x;

% Extract the pitch and voicing information
[srh_f0,srh_vuv,srh_vuvc,srh_time] = pitch_srh(x,fs,F0min,F0max,frame_shift);

% Extract the maximum voiced frequency
[max_voiced_freq] = maximum_voiced_frequency(x,fs,srh_f0.*srh_vuv,srh_time);

% Creaky probability estimation
warning off
try
    [creak_pp,creak_bin] = detect_creaky_voice(x,fs); % Detect creaky voice
    creak=interp1(creak_bin(:,2),creak_bin(:,1),1:length(x));
    creak(creak<0.5)=0; creak(creak>=0.5)=1;
    do_creak=1;
catch
    disp('Version or toolboxes do not support neural network object used in creaky voice detection. Creaky detection skipped.')
    creak=zeros(length(x),1);
    creak_pp=zeros(length(x),2);
    creak_pp(:,2)=1:length(x);
    do_creak=0;
end
warning on

% GCI estimation
sd_gci = gci_sedreams(x,fs,median(srh_f0),1);        % SEDREAMS
se_gci = se_vq(x,fs,median(srh_f0),creak);           % SE-VQ

%res = lpcresidual(x,25/1000*fs,5/1000*fs,fs/1000+2); % LP residual
res = maxlpresidual(x,fs,round(fs/1000)+2);

m = mdq(res,fs,se_gci); % Maxima dispersion quotient measurement

ps = peakslope(x,fs);   % peakSlope extraction

[gf_iaif,gfd_iaif] = iaif_ola(x,fs);    % Glottal flow (and derivative) by the IAIF method

dgf_cc = complex_cepstrum(x,fs,sd_gci,srh_f0,srh_vuv); % Glottal flow derivative by the complex cesptrum-based method

[NAQ,QOQ,H1H2,HRF,PSP] = get_vq_params(gf_iaif,gfd_iaif,fs,se_gci); % Estimate conventional glottal parameters

% Estimate the Rd parameter of the Liljencrants-Fant (LF) model
srh_f0(find(srh_f0==0)) = 100;
opt = sin_analysis();
opt.fharmonic  = true;
opt.use_ls     = false;
opt.debug = 0;
frames = sin_analysis(x, fs, [srh_time(:),srh_f0(:)], opt);
rds = rd_msp(frames, fs);

% Extract Cepstral Peak Prominence
CPPv = cpp( x, fs, 1, 'mean' );

% Plots
t=(0:length(x)-1)/fs;

fig(1) = subplot(311);
    plot(t,x, 'b');
    hold on
    plot(srh_time, srh_vuv, 'g');
    stem(sd_gci,ones(1,length(sd_gci))*-.1,'m');
    stem(se_gci,ones(1,length(se_gci))*-.1,'r');
    legend('Speech signal','Voicing (SRH)', 'GCI (SEDREAMS)','GCI (SE-VQ)');
    xlabel('Time [s]');
    ylabel('Amplitude');

fig(2) = subplot(312);
    plot(srh_time, srh_f0, 'r');
    legend('f0 (SRH)');
    xlabel('Time [s]');
    ylabel('Hz');

fig(3) = subplot(313);
    plot(srh_time, max_voiced_freq, 'g');
    legend('Maximum voiced frequency');
    xlabel('Time [s]');
    ylabel('Hz');

linkaxes(fig, 'x');
xlim([0 srh_time(end)]);
figure

fig(4) = subplot(311);
    plot(t,x, 'b');
    hold on
    plot(ps(:,1), ps(:,2),'--b');
    plot(m(:,1),m(:,2),'m');
    plot(rds(:,1), rds(:,2), 'r');
    legend('Speech signal','PeakSlope','MDQ','Rd');
    xlabel('Time [s]');

fig(5) = subplot(312);
    plot(t,x, 'b');
    hold on
    plot(t,norm(x)*gf_iaif./norm(gf_iaif), 'g');
    plot(creak_pp(:,2)/fs,creak_pp(:,1),'r')
    xlabel('Time [s]');
    ylabel('Amplitude');
    legend('Speech signal','Glottal flow (IAIF)','Creaky voice probability','Location','NorthWest');

fig(6) = subplot(313);
    plot(t,x, 'b');
    hold on
    plot(t,norm(x)*dgf_cc./norm(dgf_cc), 'r');
    xlabel('Time [s]');
    ylabel('Amplitude');
    legend('Speech signal','Glottal flow derivative (CC)');

linkaxes(fig, 'x');
xlim([0 srh_time(end)]);


function cS = antigorite
% serpentine

cs_Atg = crystalSymmetry('m',[4.7013 1 0.7844], [90, 91.633, 90]*degree);
N = Miller({0,0,1},{0,0,-1},{1,0,0},{-1,0,0},{2,1,0},{-2,1,0},cs_Atg);
dist = [0.15, 0.15, 0.4, 0.4, 2, 2];
cS = crystalShape(N./dist);


function x = rbmup(rbm, x)
    x = sigm(repmat(rbm.c', size(x, 1), 1) + x * rbm.W');
end


function HD = VARhd(VAR)
% =======================================================================
% Compute the historical decomposition of the time series in a VAR
% estimated with VARmodel and identified with VARir/VARfevd
% =======================================================================
% HD = VARhd(VAR)
% -----------------------------------------------------------------------
% INPUTS
%   - VAR: structure, result of VARmodel -> VARir/VARfevd function
% -----------------------------------------------------------------------
% OUTPUT
%   - HD: structure including the historical decomposition
% =======================================================================
% Ambrogio Cesa Bianchi, March 2015
% ambrogio.cesabianchi@gmail.com

% I thank Andrey Zubarev for finding a bug in the contribution of the
% exogenous variables when nvar_ex~=0 and nlag_ex>0.

%% Check inputs
%===============================================
if ~exist('VAR','var')
    error('You need to provide VAR structure, result of VARmodel');
end
% If there is VAR check that the inverse of the A matrix is not empty
invA = VAR.invA;
if isempty(invA)
    error('You need to identify the VAR before running VARhd. Run VARir/VARfevd first.');
end

%% Retrieve and initialize variables
%===============================================
Fcomp   = VAR.Fcomp;                      % Companion matrix
const   = VAR.const;                      % constant and/or trends
F       = VAR.Ft';                        % make comparable to notes
eps     = invA\transpose(VAR.residuals);  % structural errors
nvar    = VAR.nvar;                       % number of endogenous variables
nvar_ex = VAR.nvar_ex;                    % number of exogenous (excluding constant and trend)
nvarXeq = VAR.nvar * VAR.nlag;            % number of lagged endogenous per equation
nlag    = VAR.nlag;                       % number of lags
nlag_ex = VAR.nlag_ex;                    % number of lags of the exogenous
Y       = VAR.Y;                          % left-hand side
X       = VAR.X(:,1+const:nvarXeq+const); % right-hand side (no exogenous)
nobs    = size(Y,1);                      % number of observations

%% Compute historical decompositions
%===============================================

% Contribution of each shock
    invA_big = zeros(nvarXeq,nvar);
    invA_big(1:nvar,:) = invA;
    Icomp = [eye(nvar) zeros(nvar,(nlag-1)*nvar)];
    HDshock_big = zeros(nlag*nvar,nobs+1,nvar);
    HDshock = zeros(nvar,nobs+1,nvar);
    for j=1:nvar; % for each variable
        eps_big = zeros(nvar,nobs+1); % matrix of shocks conformable with companion
        eps_big(j,2:end) = eps(j,:);
        for i = 2:nobs+1
            HDshock_big(:,i,j) = invA_big*eps_big(:,i) + Fcomp*HDshock_big(:,i-1,j);
            HDshock(:,i,j) =  Icomp*HDshock_big(:,i,j);
        end
    end

% Initial value
    HDinit_big = zeros(nlag*nvar,nobs+1);
    HDinit = zeros(nvar, nobs+1);
    HDinit_big(:,1) = X(1,:)';
    HDinit(:,1) = Icomp*HDinit_big(:,1);
    for i = 2:nobs+1
        HDinit_big(:,i) = Fcomp*HDinit_big(:,i-1);
        HDinit(:,i) = Icomp *HDinit_big(:,i);
    end

% Constant
    HDconst_big = zeros(nlag*nvar,nobs+1);
    HDconst = zeros(nvar, nobs+1);
    CC = zeros(nlag*nvar,1);
    if const>0
        CC(1:nvar,:) = F(:,1);
        for i = 2:nobs+1
            HDconst_big(:,i) = CC + Fcomp*HDconst_big(:,i-1);
            HDconst(:,i) = Icomp * HDconst_big(:,i);
        end
    end

% Linear trend
    HDtrend_big = zeros(nlag*nvar,nobs+1);
    HDtrend = zeros(nvar, nobs+1);
    TT = zeros(nlag*nvar,1);
    if const>1;
        TT(1:nvar,:) = F(:,2);
        for i = 2:nobs+1
            HDtrend_big(:,i) = TT*(i-1) + Fcomp*HDtrend_big(:,i-1);
            HDtrend(:,i) = Icomp * HDtrend_big(:,i);
        end
    end

% Quadratic trend
    HDtrend2_big = zeros(nlag*nvar, nobs+1);
    HDtrend2 = zeros(nvar, nobs+1);
    TT2 = zeros(nlag*nvar,1);
    if const>2;
        TT2(1:nvar,:) = F(:,3);
        for i = 2:nobs+1
            HDtrend2_big(:,i) = TT2*((i-1)^2) + Fcomp*HDtrend2_big(:,i-1);
            HDtrend2(:,i) = Icomp * HDtrend2_big(:,i);
        end
    end

% Exogenous
    HDexo_big = zeros(nlag*nvar,nobs+1);
    HDexo = zeros(nvar,nobs+1);
    EXO = zeros(nlag*nvar,nvar_ex*(nlag_ex+1));
    if nvar_ex>0;
        VARexo = VAR.X_EX;
        EXO(1:nvar,:) = F(:,nvar*nlag+const+1:end); % this is c in my notes
        for i = 2:nobs+1
            HDexo_big(:,i) = EXO*VARexo(i-1,:)' + Fcomp*HDexo_big(:,i-1);
            HDexo(:,i) = Icomp * HDexo_big(:,i);
        end
    end

% All decompositions must add up to the original data
HDendo = HDinit + HDconst + HDtrend + HDtrend2 + HDexo + sum(HDshock,3);

%% Save and reshape all HDs
%===============================================
HD.shock = zeros(nobs+nlag,nvar,nvar);  % [nobs x shock x var]
    for i=1:nvar
        for j=1:nvar
            HD.shock(:,j,i) = [nan(nlag,1); HDshock(i,2:end,j)'];
        end
    end
HD.init   = [nan(nlag-1,nvar); HDinit(:,1:end)'];    % [nobs x var]
HD.const  = [nan(nlag,nvar);   HDconst(:,2:end)'];   % [nobs x var]
HD.trend  = [nan(nlag,nvar);   HDtrend(:,2:end)'];   % [nobs x var]
HD.trend2 = [nan(nlag,nvar);   HDtrend2(:,2:end)'];  % [nobs x var]
HD.exo    = [nan(nlag,nvar);   HDexo(:,2:end)'];     % [nobs x var]
HD.endo   = [nan(nlag,nvar);   HDendo(:,2:end)'];    % [nobs x var]


 function [PhaseLockingData] = bz_PhaseModulation(varargin)
% USAGE
%[PhaseLockingData] = bz_PhaseModulation(varargin)
%
% INPUTS
% spikes        -spike time cellinfo struct
%
% lfp           -lfp struct with a single channel from bz_GetLFP()
%
% passband      -frequency range for phase modulation [lowHz highHz] form
%
% intervals     -(optional) may specify timespans over which to calculate
%               phase modulation.  Formats accepted: tstoolbox intervalSet
%               or a 2column matrix of [starts stops] in seconds
%
% samplingRate  -specifies lfp sampling frequency default=1250
%
% method        -method selection for how to generate phase,
%               possibilties are: 'hilbert' (default) or 'wavelet'
%
% powerThresh   -integer power threshold to use as cut off,
%               measured in standard deviations (default = 2)
%
% plotting      -logical if you want to plot, false if not, default=true
%
% saveMat       -logical to save cellinfo .mat file with results, default=false
%
%
% OUTPUTS
%
% phasedistros  - Spike distribution perecentages for each cell in each bin
%               specified by phasebins
%
% phasebins     - 180 bins spanning from 0 to 2pi
%
% phasestats    - ncellsx1 structure array with following (via
%                 CircularDistribution.m from FMAToolbox)
%                    phasestats.m        mean angle
%                    phasestats.mode     distribution mode
%                    phasestats.k        concentration
%                    phasestats.p        p-value for Rayleigh test
%                    phasestats.r        mean resultant length
%
%
% Calculates distribution of spikes over various phases from a specified
% cycle of an lfp vector.   Phase 0 means peak of lfp wave.
%
% Brendon Watson 2015
% edited by david tingley, 2017

%% defaults
p = inputParser;
addRequired(p,'spikes',@bz_isCellInfo);
addRequired(p,'lfp',@bz_isLFP);
addRequired(p,'passband',@isnumeric)
addParameter(p,'intervals',[0 inf],@isnumeric)
addParameter(p,'samplingRate',1250,@isnumeric)
addParameter(p,'method','hilbert',@isstr)
addParameter(p,'plotting',true,@islogical)
addParameter(p,'numBins',180,@isnumeric)
addParameter(p,'powerThresh',2,@isnumeric)
addParameter(p,'saveMat',false,@islogical)

parse(p,varargin{:})

spikes = p.Results.spikes;
lfp = p.Results.lfp;
passband = p.Results.passband;

intervals = p.Results.intervals; % interval(s) over which to calculate
samplingRate = p.Results.samplingRate; % sampling rate of continuous signal (LFP)
method = p.Results.method;
plotting = p.Results.plotting;
numBins = p.Results.numBins;
powerThresh = p.Results.powerThresh;
saveMat = p.Results.saveMat;

%% Get phase for every time point in LFP
switch lower(method)
    case ('hilbert')
        [b a] = butter(3,[passband(1)/(samplingRate/2) passband(2)/(samplingRate/2)],'bandpass'); % order 3
%         [b a] = cheby2(4,20,passband/(samplingRate/2));
        filt = FiltFiltM(b,a,double(lfp.data(:,1)));
        power = fastrms(filt,ceil(samplingRate./passband(1)));  % approximate power is frequency band
        hilb = hilbert(filt);
        lfpphase = mod(angle(hilb),2*pi);
        clear fil
    case ('wavelet')% Use Wavelet transform to calulate the signal phases
%         nvoice = 12;
%         freqlist= 2.^(log2(passband(1)):1/nvoice:log2(passband(2)));
%         error('awt_freqlist, where did this come from?')
%         wt = awt_freqlist(double(lfp.data(:,1)), samplingRate, freqlist);
%         amp = (real(wt).^2 + imag(wt).^2).^.5;
%         phase = atan2(imag(wt),real(wt));
%         [~,mIdx] = max(amp'); %get index with max power for each timepiont
%         for i = 1:size(wt,1)
%             lfpphase(i) = phase(i,mIdx(i));
%         end
%         lfpphase = mod(lfpphase,2*pi);
        [wave,f,t,coh,wphases,raw,coi,scale,priod,scalef]=getWavelet(double(lfp.data(:,1)),samplingRate,passband(1),passband(2),8,0);
        [~,mIdx]=max(wave);%get index max power for each timepiont
        pIdx=mIdx'+[0;size(f,2).*cumsum(ones(size(t,1)-1,1))];%converting to indices that will pick off single maxamp index from each of the freq-based phases at eacht timepoint
        lfpphases=wphases(pIdx);%get phase of max amplitude wave at each timepoint
        lfpphases = mod(lfpphases,2*pi);%covert to 0-2pi rather than -pi:pi
% %     case ('peaks')
        % not yet coded
        % filter, smooth, diff = 0, diffdiff = negative
end

%% update intervals to remove sub-threshold power periods
disp('finding intervals below power threshold...')
thresh = mean(power) + std(power)*powerThresh;
minWidth = (samplingRate./passband(2)) * 2; % set the minimum width to two cycles

below=find(power<thresh);
if max(diff(diff(below))) == 0
    below_thresh = [below(1) below(end)];
elseif length(below)>0;
    ends=find(diff(below)~=1);
    ends(end+1)=length(below);
    ends=sort(ends);
    lengths=diff(ends);
    stops=below(ends)./samplingRate;
    starts=lengths./samplingRate;
    starts = [1; starts];
    below_thresh(:,2)=stops;
    below_thresh(:,1)=stops-starts;
else
    below_thresh=[];
end

% now merge interval sets from input and power threshold
intervals = SubtractIntervals(intervals,below_thresh);  % subtract out low power intervals

intervals = intervals(diff(intervals')>minWidth./samplingRate,:); % only keep min width epochs

%% Get phases for each spike for each cell
h = [];
% cum_spkphases = [];
phasebins=[];
spkphases = cell(1,length(spikes.times));
for a = 1:length(spikes.times)

    bools = InIntervals(spikes.times{a},intervals);
    s =spikes.times{a}(bools);
%     s = spikes{a};
    if isempty(s)
        phasedistros(:,a) = zeros(numBins,1);
        phasestats.m(a) = nan;
        phasestats.r(a) = nan;
        phasestats.k(a) = nan;
        phasestats.p(a) = nan;
        phasestats.mode(a) = nan;
        spkphases{a} = nan;
    else
        spkphases{a} = lfpphase(ceil(s*samplingRate));

%         cum_spkphases = vertcat(cum_spkphases, spkphases{a});

    %% Gather binned counts and stats (incl Rayleigh Test)
        [phasedistros(:,a),phasebins,ps]=CircularDistribution(spkphases{a},'nBins',numBins);
        phasestats.m(a) = mod(ps.m,2*pi);
        phasestats.r(a) = ps.r;
        phasestats.k(a) = ps.k;
        phasestats.p(a) = ps.p;
        phasestats.mode(a) = ps.mode;

    %% plotting
        if plotting
            if ~exist('PhaseModulationFig','dir')
                mkdir('PhaseModulationFig');
            end
            h(end+1) = figure;
            hax = subplot(1,2,1);
            rose(spkphases{a})
            title(hax,['Cell #' num2str(a) '. Rayleigh p = ' num2str(phasestats.p(a)) '.'])

            hax = subplot(1,2,2);
            bar(phasebins*180/pi,phasedistros(:,a))
            xlim([0 360])
            set(hax,'XTick',[0 90 180 270 360])
            hold on;
            plot([0:360],cos(pi/180*[0:360])*0.05*max(phasedistros(:,a))+0.95*max(phasedistros(:,a)),'color',[.7 .7 .7])
            set(h(end),'name',['PhaseModPlotsForCell' num2str(a)]);
            print(fullfile('PhaseModulationFig',['PhaseModPlotsForCell' num2str(a)]),'-dpng','-r0');
        end
    end
end
%% Cumulative effect across all spikes from all cells... not saving these stats for now
% phasebins=[];
% if length(cum_spkphases) > 10
%     [cpd,phasebins,cps]=CircularDistribution(cum_spkphases,'nBins',180);
%     cRp = cps.p;
%
%     if plotting
%         h(end+1) = figure;
%         hax = subplot(1,2,1);
%         rose(cum_spkphases)
%         title(hax,['All Spikes/Cells Accumulated. Rayleigh p = ' num2str(cps.p) '.'])
%
%         hax = subplot(1,2,2);
%         bar(phasebins*180/pi,cpd)
%         xlim([0 360])
%         set(hax,'XTick',[0 90 180 270 360])
%         hold on;
%         plot([0:360],cos(pi/180*[0:360])*0.05*max(cpd)+0.95*max(cpd),'color',[.7 .7 .7])
%         set(h(end),'name',['PhaseModPlotsForAllCells']);
%     end
% end

detectorName = 'bz_PhaseModulation';
channels = lfp.channels;
detectorParams = v2struct(intervals,samplingRate,method,plotting,numBins,...
    passband,powerThresh,channels);

PhaseLockingData = v2struct(phasedistros,phasebins,...
                            phasestats,spkphases,...
                            detectorName, detectorParams);
PhaseLockingData.region = spikes.region;
PhaseLockingData.UID = spikes.UID;
PhaseLockingData.sessionName = spikes.sessionName;

if saveMat
 save([lfp.Filename(1:end-4) '.PhaseLockingData.cellinfo.mat'],'PhaseLockingData');
end

end


function [f] = spm_fx_Lagrangian(P,M,U)
% FORMAT [f] = spm_fx_Lagrangian(P,M,U)
%
% flow subfunction for Langrangian demo

% Lagrangian and Q
%==========================================================================
[n m] = size(U);

for i = 1:m
    x = U(:,i);
    L = P.P{1};
    for j = 2:length(P.P)
        L = L + P.P{j}(:)'*x;
        x = kron(x,U(:,i));
    end

end

% estimate parameters of Lagrangian and Q
%==========================================================================
f    = P.Q*dLdx;


% Batch script for sparse prediction
clear all

% --- training method ID
method_id = 8;
% 1: linear_sparse_space
% 2: linear_sparse_space_vec
% 3: linear_sparse_stepwise
% 4: linear_sparse_stepwise_vec
% 5: linear_sparse_stepwise_aa
% 6: linear_sparse_seq
% 7: linear_stepwise_reg_bayes

% --- Basic Learning Parameters
parm.Ntrain = 1000; % # of total training iteration
parm.Nskip  = 100;	% skip steps for display info

% --- Time delay embedding parameter
parm.Tau   = 1 ;   % Lag time steps
parm.Dtau  = 3 ;   % Number of embedding dimension
parm.Tpred = 0 ;   % Prediction time step : y(t+Tpred) = W * x(t)

% --- Normalization parameter
parm.data_norm  = 1;	% Normalize input and output

% File name of old training result for retraining
old_file = [];

% File name for test data
datafile  = ['./test/test.mat'];
modelfile = ['./test/model'];
newdata   = 0; % = 1: make new data

% --- Make or Load training & test data
if newdata == 0 && exist(datafile,'file')
	load(datafile, ...
		'xdata','ydata','xtest','ytest')
else
	% Training & test data setting
	Tdata  = 1000 ; % # of training data
	Ttest  = 1000 ; % # of test data

	parm.Ydim = 4;
	parm.Xdim = 200;   % Input dim
	parm.Meff = 30;    % Effective Input
	parm.Sdim = 10; % number of sinusoidal input
	parm.Tmin = 50; % minimum period
	parm.Tmax = 200;% maximum period
	parm.SY   = [0.3; 0.1; 0.5; 1.0];% output moise variance
	parm.Ntrial = 1;

	parm.sy = parm.SY;
	[xdata,ydata,Wout,Weff,Wrdn,Win,Wirr] = ...
		make_train_data(parm, Tdata);

	parm.sy = 0.0;
	[xtest,ytest] = ...
		make_train_data(parm, Ttest, Wout,Wrdn,Win,Wirr);

	if ~isempty(datafile)
		save(datafile, ...
			'xdata','ydata','xtest','ytest','Wout','Weff','parm')
	end
end

% Normalize input data
[X,nparm] = normalize_data(xdata, parm.data_norm);
parm.xmean = nparm.xmean;
parm.xnorm = nparm.xnorm;

% Normalize output data
[Y,nparm] = normalize_data(ydata, parm.data_norm);
parm.ymean = nparm.xmean;
parm.ynorm = nparm.xnorm;

% Time alignment for prediction using embedding input
[tx,ty] = pred_time_index(X,parm);
X = X(:,tx,:);
Y = Y(:,ty,:);

% --- Initialization of Model Parameters
if ~isempty(old_file)
	% Start from old result
	load([old_file], 'Model')
else
	Model = [];
end

%profile_on = 1;
%profile_start(profile_on);

%
% --- Sparse estimation
%
switch	method_id
case	1
	[Model, Info] = linear_sparse_space(X, Y, Model, parm);
case	2
	[Model, Info] = linear_sparse_space_vec(X, Y, Model, parm);
case	3
	[Model, Info] = linear_sparse_stepwise(X, Y, Model, parm);
case	4
	[Model, Info] = linear_sparse_stepwise_vec(X, Y, Model, parm);
case	5
	[Model, Info] = linear_sparse_stepwise_aa(X, Y, Model, parm);
case	6
	[Model, Info] = linear_sparse_seq(X, Y, Model, parm);
case	7
	[Model, Info] = linear_stepwise_reg_bayes(X, Y, Model, parm);
case	8
	[Model, Info] = linear_fit(X, Y, Model, parm);
case	10
	% --- Delay embedding is done before training
	Xd = delay_embed(X, parm.Tau, parm.Dtau);
	[Model, Info] = linear_map_sparse_cov(Xd, Y, Model, parm);
end

%
% --- Estimate prediction error for test data
%

% Time alignment for prediction using embedding input
[tx,ty] = pred_time_index(xtest,parm);
xtest = xtest(:,tx,:);
ytest = ytest(:,ty,:);

% Use normalization constant calculated by training data
xtest = normalize_data(xtest, parm.data_norm, parm);

% --- Prediction for test data
ypred = predict_output(xtest, Model, parm);
err   = sum((ytest(:)-ypred(:)).^2)/sum(ytest(:).^2)

if ~isempty(modelfile)
	fsave = [modelfile sprintf('_id%d.mat',method_id)];
	save(fsave, 'Model', 'Info', 'parm');
end

plot_predict

%profile_end(profile_on)
return


function headmodel = ft_headmodel_singlesphere(mesh, varargin)

% FT_HEADMODEL_SINGLESPHERE creates a volume conduction model of the
% head by fitting a spherical model to a set of points that describe
% the head surface.
%
% For MEG this implements Cuffin BN, Cohen D.  "Magnetic fields of
% a dipole in special volume conductor shapes" IEEE Trans Biomed Eng.
% 1977 Jul;24(4):372-81.
%
% Use as
%   headmodel = ft_headmodel_singlesphere(mesh, ...)
%
% Optional arguments should be specified in key-value pairs and can include
%   conductivity     = number, conductivity of the sphere
%
% See also FT_PREPARE_VOL_SENS, FT_COMPUTE_LEADFIELD

% FIXME document both EEG and MEG case

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

% get the optional arguments
conductivity = ft_getopt(varargin, 'conductivity', 1);

if any(strcmp(varargin(1:2:end), 'unit')) || any(strcmp(varargin(1:2:end), 'units'))
  % the geometrical units should be specified in the input mesh
  ft_error('the ''unit'' option is not supported any more');
end

if isnumeric(mesh) && size(mesh,2)==3
  % assume that it is a Nx3 array with vertices
  % convert it to a structure, this is needed to determine the units further down
  mesh = struct('pos', mesh);
elseif isstruct(mesh) && isfield(mesh,'bnd')
  % take the triangulated surfaces from the input structure
  mesh = mesh.bnd;
end

% replace pnt with pos
mesh = fixpos(mesh);

if ~isstruct(mesh) || numel(mesh)>1 || ~isfield(mesh, 'pos')
  ft_error('the input mesh should be a set of points or a single triangulated surface')
end

if numel(conductivity)~=1
  ft_error('the conductivity should be a single number')
end

if numel(mesh)~=1
  ft_error('fitting a single sphere requires a single mesh')
end

% start with an empty volume conductor
headmodel = [];

% ensure that the mesh has units, estimate them if needed
mesh = ft_determine_units(mesh);

% copy the geometrical units into the volume conductor
headmodel.unit = mesh.unit;

% fit a single sphere to all headshape points
[single_o, single_r] = fitsphere(mesh.pos);

headmodel.r    = single_r;
headmodel.o    = single_o;
headmodel.cond = conductivity;
headmodel.type = 'singlesphere';

fprintf('single sphere: radius = %.1f, conductivity = %f\n', headmodel.r, headmodel.cond);


%% STUDY_04_wrist_scaffold_multiple_fiber_evaluation
% Below is a demonstration for:
%
% * Finite element analysis of the performance of additively manufactured scaffolds for scapholunate ligament reconstruction
%___________________________________________________________________________________________________________________________
%  This study presents a patient-specific computational biomechanical evaluation of the effect of scaffold length,
%  and positioning of the bone attachment sites. Through segmentation and image processing of medical image data
%  for natural wrist motion, detailed 3D geometries as well as patient-specific physiological wrist motion could
%  be derived. This data formed the input for detailed finite element analysis, enabling computational of scaffold
%  stress and strain distributions, which are key predictors of scaffold structural integrity.
%___________________________________________________________________________________________________________________________
% * febio_spec version 3.0
% * febio, FEBio
% * hexahedral elements, hex8
% * static, solid
% * hyperelastic, Ogden
% * displacement logfile
% * stress logfile

clear; close all; clc;

% Plot settings
fontSize=40;
numSmoothStepsMain=1;
cParSmoothMain.n=numSmoothStepsMain;
cParSmoothMain.Method='HC';

%%
% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'2021_Nataliya_Perevoshchikova_wrist_project','data','temp');
loadPathSurface=fullfile(defaultFolder,'2021_Nataliya_Perevoshchikova_wrist_project','data','STL');
loadPathMotion=fullfile(defaultFolder,'2021_Nataliya_Perevoshchikova_wrist_project','data','motion');

%% Building a quadrilateral circular mesh
d=0.35;
h=7;
xSpacing=0.93;
ySpacing=0.525;
nCopies_x=4;
nCopies_y=7;
r=d/2;
ne=2; %Elements in radius
f=0.6; %Fraction (with respect to outer radius) where central square appears

pointSpacingHeight=0.1;
numStepsSweep=ceil(h/pointSpacingHeight);

xc=zeros(numStepsSweep,1);
yc=zeros(numStepsSweep,1);
zc=linspace(-h/2,h/2,numStepsSweep)';
Vc=[xc yc zc];

%SweepLoft
%Create the mesh
[Fq,Vq]=discQuadMesh(ne,r,f);

Vq(:,3)=0;
Vq1=Vq;
Vq1(:,3)=Vq1(:,3)-h/2;
Vq2=Vq;
Vq2(:,3)=Vq2(:,3)+h/2;

% Visualizing mesh
cFigure; hold on;
title('Single fiber','FontSize',fontSize);
plotV(Vq1,'k.','lineWidth',5,'MarkerSize',25);
plotV(Vq2,'r.','lineWidth',5,'MarkerSize',25);
plotV(Vc,'g.-','lineWidth',5,'MarkerSize',25);

axisGeom;
camlight headlight;
drawnow;

[~,~,~,S]=sweepLoft(Vq1,Vq2,[0 0 1],[0 0 1],Vc,numStepsSweep,0,0);

X=S.X'; Y=S.Y'; Z=S.Z'; %Coordinate matrices
V=[X(:) Y(:) Z(:)]; %Create node list

I=size(Vq,1)*((1:1:numStepsSweep)-1);
I=I(ones(size(Fq,1),1),:);
I=I(:);

FQ=repmat(Fq,numStepsSweep,1)+I(:,ones(size(Fq,2),1));
Efib=[FQ(1:end-size(Fq,1),:) FQ(size(Fq,1)+1:end,:)]; %The hexahedral elements

[Efib,V,ind1,ind2]=mergeVertices(Efib,V); %Merge nodes (start and end are not shared yet)
%%
nCopies=nCopies_x*nCopies_y;
VC=repmat({V},nCopies,1);
EC=repmat({Efib},nCopies,1);
numNodesBar=size(V,1);
[Efib,V]=joinElementSets(EC,VC);

c=1;

for qx=1:1:nCopies_x
    for qy=1:1:nCopies_y
        if c>1
            ind1=((c-1)*numNodesBar)+1;
            ind2=(c*numNodesBar);
            V(ind1:ind2,1)=V(ind1:ind2,1)+(qx-1)*xSpacing;
            V(ind1:ind2,2)=V(ind1:ind2,2)+(qy-1)*ySpacing;
        end
        c=c+1;
    end
end

[F]=element2patch(Efib);

%% Get boundary conditions faces
%Get boundary faces
ind=tesBoundary(F,V);
Fb=F(ind,:);
%Get tops/bottoms
Nb=patchNormal(Fb,V);

zVec=[0 0 1];
d=dot(Nb,zVec(ones(size(Nb,1),1),:),2);
Z=V(:,3);
ZF=mean(Z(Fb),2);
logicTop_Fb=(d>0.9) & ZF>=(max(V(:,3))-eps(1));
logicBottom_Fb=(d<-0.9) & ZF<=(min(V(:,3))+eps(1));
F_start=Fb(logicTop_Fb,:);
F_end=Fb(logicBottom_Fb,:);

logic_Lig=(ZF>-1.5) & ZF<=1.5;
F_Lig=Fb(logic_Lig,:);

%%
% Visualizing mesh
cFigure; hold on;
title('Multiple fibers','FontSize',fontSize);
patch('faces',Fb,'vertices',V,'FaceColor','g','FaceAlpha',0.3,'EdgeColor','None');
plotV(V(F_end',:),'k.','lineWidth',5,'MarkerSize',20);
plotV(V(F_start',:),'r.','lineWidth',5,'MarkerSize',20);

camlight headlight;
axisGeom(gca,fontSize);
drawnow;

F_L=F_end;
F_S=F_start;

%% Scaphoid and lunate at first frame from TriPlane, Brown University
loadName=fullfile(loadPathSurface,['modelGeometry.mat']);
dataStruct=load(loadName);

FS_triP=dataStruct.FS;
FL_triP=dataStruct.FL;
FC_triP=dataStruct.FC;
FR_triP=dataStruct.FR;

VL_triP=dataStruct.VL;
VS_triP=dataStruct.VS;
VC_triP=dataStruct.VC;
VR_triP=dataStruct.VR;

%%%
hFig=cFigure;
hold on;
title('Radiography','FontSize',fontSize);

gpatch(FS_triP,VS_triP,'w','none',0.5);
gpatch(FL_triP,VL_triP,'w','none',0.5);
gpatch(FC_triP,VC_triP,'w','none',0.5);
gpatch(FR_triP,VR_triP,'w','none',0.5);

grid on; axis equal;
axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Cutting Radius
pointSpacingR=mean(patchEdgeLengths(FR_triP,VR_triP));
%% Cut Capitate bone
% The capitate is cut in the x direction.

%Create a logic for cutting away faces
max_X1=max(VR_triP(:,1))-70*pointSpacingR; %Max x-level used for cutting
logicVertices=VR_triP(:,1)<max_X1; %Logic for the points below this level
logicFaces=all(logicVertices(FR_triP),2); %Logic for the faces
logicFaces=triSurfLogicSharpFix(FR_triP,logicFaces,3); %Altered logic so it is smoother

%%
% Visualize
cFigure; hold on;
gpatch(FS_triP,VS_triP,'w','none',0.5);
gpatch(FL_triP,VL_triP,'w','none',0.5);
gpatch(FR_triP,VR_triP,logicFaces,'none',0.5);
gpatch(FC_triP,VC_triP,'w','none',0.5);

camlight('headlight');
axisGeom(gca,fontSize);
drawnow;

%%
% Cut away faces using logic
FR_triP=FR_triP(logicFaces,:); %The faces to keep
[FR_triP,VR_triP]=patchCleanUnused(FR_triP,VR_triP); %Remove unused points

%Attempt to self triangulate potentially jagged edge
Eb=patchBoundary(FR_triP,VR_triP); %Get boundary edges
indBoundary=edgeListToCurve(Eb); %Convert boundary edges to a curve list
indBoundary=indBoundary(1:end-1); %Trim off last point since it is equal to first on a closed loop
angleThreshold=pi*(120/180); %threshold for self triangulation
[FR_triP,VR_triP,indBoundaryTop]=triSurfSelfTriangulateBoundary(FR_triP,VR_triP,indBoundary,angleThreshold,1);

%Force boundary to have the max X level chosen
VR_triP(indBoundaryTop,1)=max_X1;

%%
% Visualize
cFigure; hold on;
gpatch(FS_triP,VS_triP,'w','none',0.5);
gpatch(FL_triP,VL_triP,'w','none',0.5);
gpatch(FC_triP,VC_triP,'w','none',0.5);
gpatch(FR_triP,VR_triP,'w','k',0.5);
plotV(VR_triP(indBoundaryTop,:),'r-','LineWidth',3);

camlight('headlight');
axisGeom(gca,fontSize);
drawnow;

%% Close over top of radius
% The top boundary curve of the cut surface is filled with triangles. This
% is a 2D method. The x-coordinate is added after.
[F2t,V2t]=regionTriMesh2D({VR_triP(indBoundaryTop,[2 3])},pointSpacingR,0,0);
V2t(:,3)=V2t(:,2);
V2t(:,2)=V2t(:,1);
V2t(:,1)=max_X1; %Add/set X-level

%%
% Visualize
cFigure; hold on;
gpatch(FS_triP,VS_triP,'w','none',0.5);
gpatch(FL_triP,VL_triP,'w','none',0.5);
gpatch(FC_triP,VC_triP,'w','none',0.5);
gpatch(FR_triP,VR_triP,'rw','k',0.5);
gpatch(F2t,V2t,'gw','k',1);
plotV(VR_triP(indBoundaryTop,:),'r-','LineWidth',3);
camlight('headlight');
axisGeom(gca,fontSize);
drawnow;

%% Joining surface features
% Add all surface sets together in joint list of faces, vertices
[FR_triP,VR_triP]=joinElementSets({FR_triP,F2t},{VR_triP,V2t});
%% Merge shared nodes
% The join operation only adds the sets together. Nodes with the same
% coordinates are not seen as the same yet and need to be merged.
[FR_triP,VR_triP]=mergeVertices(FR_triP,VR_triP); %Merge nodes

%Remove tri connected
[FS,VS]=triSurfRemoveThreeConnect(FS_triP,VS_triP);
[FL,VL]=triSurfRemoveThreeConnect(FL_triP,VL_triP);
[FC,VC]=triSurfRemoveThreeConnect(FC_triP,VC_triP);
[FR,VR]=triSurfRemoveThreeConnect(FR_triP,VR_triP);
%Smoothen
[VL]=patchSmooth(FL,VL,[],cParSmoothMain);
[VS]=patchSmooth(FS,VS,[],cParSmoothMain);
[VR]=patchSmooth(FR,VR,[],cParSmoothMain);
[VC]=patchSmooth(FC,VC,[],cParSmoothMain);
%Refine
[FS,VS]=subtri(FS,VS,1);
[FL,VL]=subtri(FL,VL,1);
[FR,VR]=subtri(FR,VR,1);
[FC,VC]=subtri(FC,VC,1);
%Smoothen
[VL]=patchSmooth(FL,VL,[],cParSmoothMain);
[VS]=patchSmooth(FS,VS,[],cParSmoothMain);
[VR]=patchSmooth(FR,VR,[],cParSmoothMain);
[VC]=patchSmooth(FC,VC,[],cParSmoothMain);
%%%%

%Construct placement according to natural SLIL positioning
alphaRot=pi/2;
R=euler2DCM([alphaRot 0 0]); %The rotation tensor for each step
V=V*R; %Rotated 1 part for visualization of stepwise amount

alphaRot=pi/2;
R=euler2DCM([0 alphaRot 0]); %The rotation tensor for each step
V=V*R; %Rotated 1 part for visualization of stepwise amount

alphaRot=-pi/15;
R=euler2DCM([0 0 alphaRot]); %The rotation tensor for each step
V=V*R; %Rotated 1 part for visualization of stepwise amount

V(:,3)=V(:,3)-1.5;
V(:,1)=V(:,1)-15;
V(:,2)=V(:,2)-3.2;

hFig=cFigure;
hold on;

gpatch(FS,VS,'w','none',0.7);
gpatch(FL,VL,'w','none',0.7);

gpatch(FC,VC,'w','none',0.5);
gpatch(FR,VR,'w','none',0.5);

gpatch(Fb,V,'gw','k',1);

grid on; axis equal;
axisGeom(gca,fontSize);
camlight headlight;
drawnow;

%% Joining node sets
[E_F,V,C]=joinElementSets({Efib,FS,FL,FC,FR},{V,VS,VL,VC,VR});
E=E_F{1};
FS1=E_F{2};
FL1=E_F{3};
FC1=E_F{4};
FR1=E_F{5};

% hFig=cFigure;
% hold on;
%
% gpatch(FS1,V,'w','none',0.7);
% gpatch(FL1,V,'w','none',0.7);
%
% gpatch(FC1,V,'w','none',0.5);
% gpatch(FR1,V,'w','none',0.5);
%
% gpatch(E,V,'gw','k',1);
%
% grid on; axis equal;
% axisGeom(gca,fontSize);
% camlight headlight;
% drawnow;

%% FEA control settings
Step=5;
numTimeSteps=round((1989-1000)/Step);%Number of time steps desired
max_refs=300; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=150; %Optimum number of iterations
max_retries=50; %Maximum number of retires
dtmin=(1/numTimeSteps)/10; %Minimum time step size
dtmax=1/numTimeSteps; %Maximum time step size
tStep=0.005*Step; %Duration of step
min_residual=1e-20;
Mag_val=1;

%% Control parameters
% Defining file names
febioFebFileNamePart='tempModel';
febioFebFileName=fullfile(savePath,[febioFebFileNamePart,'.feb']); %FEB file name
febioLogFileName=[febioFebFileNamePart,'.txt']; %FEBio log file name
febioLogFileName_disp=[febioFebFileNamePart,'_disp_out.txt']; %Log file name for exporting displacement
febioLogFileName_force=[febioFebFileNamePart,'_force_out.txt']; %Log file name for exporting force
febioLogFileName_stress=[febioFebFileNamePart,'_stress_out.txt']; %Log file name for exporting stress

%% Defining the FEBio input structure
% See also |febioStructTemplate| and |febioStruct2xml| and the FEBio user
% manual.

%Get a template with default settings
[febio_spec]=febioStructTemplate;

%febio_spec version
febio_spec.ATTR.version='3.0';

%Module section
febio_spec.Module.ATTR.type='solid';

%Control section
febio_spec.Control.analysis='STATIC';
febio_spec.Control.time_steps=numTimeSteps;
febio_spec.Control.step_size=tStep;
febio_spec.Control.time_stepper.dtmin=dtmin;
febio_spec.Control.time_stepper.dtmax=dtmax;
febio_spec.Control.time_stepper.max_retries=max_retries;
febio_spec.Control.time_stepper.opt_iter=opt_iter;
febio_spec.Control.solver.max_refs=max_refs;
febio_spec.Control.solver.max_ups=max_ups;
febio_spec.Control.solver.min_residual=min_residual;

%Material section
materialName1='Material1';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.ATTR.name=materialName1;

%Material section
C1=57.21894783089972;
C2=57.23071414361289;
K=11444.966197451262;
M1=2.00012552045284;
M2=-1.9997730268725347;
t1=25.365;
g1=1.23339;

materialOption='elastic';%'elastic_viscoelastic'
switch materialOption
    case 'elastic'
        febio_spec.Material.material{1}.ATTR.type='Ogden';
        febio_spec.Material.material{1}.c1=C1;
        febio_spec.Material.material{1}.m1=M1;
        febio_spec.Material.material{1}.c2=C2;
        febio_spec.Material.material{1}.m2=M2;
        febio_spec.Material.material{1}.k=K;
    case 'elastic_viscoelastic'
        %Viscoelastic part
        febio_spec.Material.material{1}.ATTR.type='uncoupled viscoelastic';
        febio_spec.Material.material{1}.g1=g1;
        febio_spec.Material.material{1}.t1=t1;

        %Elastic part
        febio_spec.Material.material{1}.elastic{1}.ATTR.type='Ogden';
        febio_spec.Material.material{1}.elastic{1}.c1=C1;
        febio_spec.Material.material{1}.elastic{1}.m1=M1;
        febio_spec.Material.material{1}.elastic{1}.c2=C2;
        febio_spec.Material.material{1}.elastic{1}.m2=M2;
        febio_spec.Material.material{1}.elastic{1}.k=K;

end

materialName2='Lunate_side';
febio_spec.Material.material{2}.ATTR.name=materialName2;
febio_spec.Material.material{2}.ATTR.type='rigid body';
febio_spec.Material.material{2}.ATTR.id=2;
febio_spec.Material.material{2}.density=1;
febio_spec.Material.material{2}.center_of_mass=[0 0 0];

materialName3='Scaphoid_side';
febio_spec.Material.material{3}.ATTR.name=materialName3;
febio_spec.Material.material{3}.ATTR.type='rigid body';
febio_spec.Material.material{3}.ATTR.id=3;
febio_spec.Material.material{3}.density=1;
febio_spec.Material.material{3}.center_of_mass=[0 0 0];

materialName4='Lunate';
febio_spec.Material.material{4}.ATTR.name=materialName4;
febio_spec.Material.material{4}.ATTR.type='rigid body';
febio_spec.Material.material{4}.ATTR.id=4;
febio_spec.Material.material{4}.density=1;
febio_spec.Material.material{4}.center_of_mass=[0 0 0];

materialName5='Scaphoid';
febio_spec.Material.material{5}.ATTR.name=materialName5;
febio_spec.Material.material{5}.ATTR.type='rigid body';
febio_spec.Material.material{5}.ATTR.id=5;
febio_spec.Material.material{5}.density=1;
febio_spec.Material.material{5}.center_of_mass=[0 0 0];

materialName6='Capitate';
febio_spec.Material.material{6}.ATTR.name=materialName6;
febio_spec.Material.material{6}.ATTR.type='rigid body';
febio_spec.Material.material{6}.ATTR.id=6;
febio_spec.Material.material{6}.density=1;
febio_spec.Material.material{6}.center_of_mass=[0 0 0];

materialName7='Radius';
febio_spec.Material.material{7}.ATTR.name=materialName7;
febio_spec.Material.material{7}.ATTR.type='rigid body';
febio_spec.Material.material{7}.ATTR.id=7;
febio_spec.Material.material{7}.density=1;
febio_spec.Material.material{7}.center_of_mass=[0 0 0];

%Geometry section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='nodeSet_all'; %The node set name
febio_spec.Mesh.Nodes{1}.node.ATTR.id=(1:size(V,1))'; %The node id's
febio_spec.Mesh.Nodes{1}.node.VAL=V; %The nodel coordinates

% % -> Elements
partName1='Scaffold';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of the element set
febio_spec.Mesh.Elements{1}.ATTR.mat=1; %material index for this set
febio_spec.Mesh.Elements{1}.ATTR.type='hex8'; %Element type of this set
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E;

partName2='Lunate side';
febio_spec.Mesh.Elements{2}.ATTR.name=partName2; %Name of the element set
febio_spec.Mesh.Elements{2}.ATTR.mat=2; %material index for this set
febio_spec.Mesh.Elements{2}.ATTR.type='quad4'; %Element type of this set
febio_spec.Mesh.Elements{2}.elem.ATTR.id=size(E,1)+(1:1:size(F_L,1))'; %Element id's
febio_spec.Mesh.Elements{2}.elem.VAL=F_L;

partName3='Scaphoid side';
febio_spec.Mesh.Elements{3}.ATTR.name=partName3; %Name of the element set
febio_spec.Mesh.Elements{3}.ATTR.mat=3; %material index for this set
febio_spec.Mesh.Elements{3}.ATTR.type='quad4'; %Element type of this set
febio_spec.Mesh.Elements{3}.elem.ATTR.id=size(E,1)+(1:1:size(F_S,1))'; %Element id's
febio_spec.Mesh.Elements{3}.elem.VAL=F_S;

partName4='Lunate';
febio_spec.Mesh.Elements{4}.ATTR.name=partName4; %Name of the element set
febio_spec.Mesh.Elements{4}.ATTR.mat=4; %material index for this set
febio_spec.Mesh.Elements{4}.ATTR.type='tri3'; %Element type of this set
febio_spec.Mesh.Elements{4}.elem.ATTR.id=size(E,1)+(1:1:size(FL1,1))'; %Element id's
febio_spec.Mesh.Elements{4}.elem.VAL=FL1;

partName5='Scaphoid';
febio_spec.Mesh.Elements{5}.ATTR.name=partName5; %Name of the element set
febio_spec.Mesh.Elements{5}.ATTR.mat=5; %material index for this set
febio_spec.Mesh.Elements{5}.ATTR.type='tri3'; %Element type of this set
febio_spec.Mesh.Elements{5}.elem.ATTR.id=size(E,1)+(1:1:size(FS1,1))'; %Element id's
febio_spec.Mesh.Elements{5}.elem.VAL=FS1;

%Capitate
partName6='Capitate';
febio_spec.Mesh.Elements{6}.ATTR.name=partName6; %Name of the element set
febio_spec.Mesh.Elements{6}.ATTR.mat=6; %material index for this set
febio_spec.Mesh.Elements{6}.ATTR.type='tri3'; %Element type of this set
febio_spec.Mesh.Elements{6}.elem.ATTR.id=size(E,1)+(1:1:size(FC1,1))'; %Element id's
febio_spec.Mesh.Elements{6}.elem.VAL=FC1;

%Radius
partName7='Radius';
febio_spec.Mesh.Elements{7}.ATTR.name=partName7; %Name of the element set
febio_spec.Mesh.Elements{7}.ATTR.mat=7; %material index for this set
febio_spec.Mesh.Elements{7}.ATTR.type='tri3'; %Element type of this set
febio_spec.Mesh.Elements{7}.elem.ATTR.id=size(E,1)+(1:1:size(FR1,1))'; %Element id's
febio_spec.Mesh.Elements{7}.elem.VAL=FR1;

%MeshDomains section
febio_spec.MeshDomains.SolidDomain.ATTR.name=partName1;
febio_spec.MeshDomains.SolidDomain.ATTR.mat=materialName1;

febio_spec.MeshDomains.ShellDomain{1}.ATTR.name=partName2;
febio_spec.MeshDomains.ShellDomain{1}.ATTR.mat=materialName2;

febio_spec.MeshDomains.ShellDomain{2}.ATTR.name=partName3;
febio_spec.MeshDomains.ShellDomain{2}.ATTR.mat=materialName3;

febio_spec.MeshDomains.ShellDomain{3}.ATTR.name=partName4;
febio_spec.MeshDomains.ShellDomain{3}.ATTR.mat=materialName4;

febio_spec.MeshDomains.ShellDomain{4}.ATTR.name=partName5;
febio_spec.MeshDomains.ShellDomain{4}.ATTR.mat=materialName5;

febio_spec.MeshDomains.ShellDomain{5}.ATTR.name=partName6;
febio_spec.MeshDomains.ShellDomain{5}.ATTR.mat=materialName6;

febio_spec.MeshDomains.ShellDomain{6}.ATTR.name=partName7;
febio_spec.MeshDomains.ShellDomain{6}.ATTR.mat=materialName7;

% Boundary conditions
i=1;
for count=1:numTimeSteps

    FileData = load(fullfile(loadPathMotion, sprintf('Flex_Ext_%d_TriPlane_Lunate.mat',count)));
    MatT_Lunate = FileData.Trans_L;

    FileData = load(fullfile(loadPathMotion, sprintf('Flex_Ext_%d_TriPlane_Scaphoid.mat',count)));
    MatT_Scaphoid = FileData.Trans_S;

    FileData = load(fullfile(loadPathMotion, sprintf('Flex_Ext_%d_TriPlane_Capitate.mat',count)));
    MatT_Capitate = FileData.Trans_C;

    RLunate                       = MatT_Lunate(1:3,1:3);%radians
    RScaphoid                     = MatT_Scaphoid(1:3,1:3); %radians
    RCapitate                     = MatT_Capitate(1:3,1:3); %radians

    Prescribe_Lunate{i}       = MatT_Lunate(1:3,end);
    Prescribe_Scaphoid{i}     = MatT_Scaphoid(1:3,end);
    Prescribe_Capitate{i}     = MatT_Capitate(1:3,end);

    axangL                    = rotm2axang(RLunate);%Axis-Angle {[x, y, z], angle (radians)}
    AngleLunate{i}            = axangL(1:3)*axangL(4);%angle *rot_axis; Axis with angle magnitude (radians) [x, y, z]
    axangS                    = rotm2axang(RScaphoid);
    AngleScaphoid{i}          = axangS(1:3)*axangS(4);%angle*rot_axis;
    axangC                    = rotm2axang(RCapitate);
    AngleCapitate{i}          = axangC(1:3)*axangC(4);%angle*rot_axis;
    tStartNow(i)=count*tStep;
    i=i+1;
end

%Boundary conditions
%Rigid section
% %Start and End
febio_spec.Rigid.rigid_constraint{1}.ATTR.name='Rigid_Lunate_X';
febio_spec.Rigid.rigid_constraint{1}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{1}.rb=2;
febio_spec.Rigid.rigid_constraint{1}.dof='Rx';
febio_spec.Rigid.rigid_constraint{1}.value.ATTR.lc=1;
febio_spec.Rigid.rigid_constraint{1}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{1}.relative=0;

febio_spec.Rigid.rigid_constraint{2}.ATTR.name='Rigid_Lunate_Y';
febio_spec.Rigid.rigid_constraint{2}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{2}.rb=2;
febio_spec.Rigid.rigid_constraint{2}.dof='Ry';
febio_spec.Rigid.rigid_constraint{2}.value.ATTR.lc=2;
febio_spec.Rigid.rigid_constraint{2}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{2}.relative=0;

febio_spec.Rigid.rigid_constraint{3}.ATTR.name='Rigid_Lunate_Z';
febio_spec.Rigid.rigid_constraint{3}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{3}.rb=2;
febio_spec.Rigid.rigid_constraint{3}.dof='Rz';
febio_spec.Rigid.rigid_constraint{3}.value.ATTR.lc=3;
febio_spec.Rigid.rigid_constraint{3}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{3}.relative=0;

febio_spec.Rigid.rigid_constraint{4}.ATTR.name='Rigid_Lunate_Ru';
febio_spec.Rigid.rigid_constraint{4}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{4}.rb=2;
febio_spec.Rigid.rigid_constraint{4}.dof='Ru';
febio_spec.Rigid.rigid_constraint{4}.value.ATTR.lc=4;
febio_spec.Rigid.rigid_constraint{4}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{4}.relative=0;

febio_spec.Rigid.rigid_constraint{5}.ATTR.name='Rigid_Lunate_Rv';
febio_spec.Rigid.rigid_constraint{5}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{5}.rb=2;
febio_spec.Rigid.rigid_constraint{5}.dof='Rv';
febio_spec.Rigid.rigid_constraint{5}.value.ATTR.lc=5;
febio_spec.Rigid.rigid_constraint{5}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{5}.relative=0;

febio_spec.Rigid.rigid_constraint{6}.ATTR.name='Rigid_Lunate_Rw';
febio_spec.Rigid.rigid_constraint{6}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{6}.rb=2;
febio_spec.Rigid.rigid_constraint{6}.dof='Rw';
febio_spec.Rigid.rigid_constraint{6}.value.ATTR.lc=6;
febio_spec.Rigid.rigid_constraint{6}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{6}.relative=0;

febio_spec.Rigid.rigid_constraint{7}.ATTR.name='Rigid_Scaphoid_X';
febio_spec.Rigid.rigid_constraint{7}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{7}.rb=3;
febio_spec.Rigid.rigid_constraint{7}.dof='Rx';
febio_spec.Rigid.rigid_constraint{7}.value.ATTR.lc=7;
febio_spec.Rigid.rigid_constraint{7}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{7}.relative=0;

febio_spec.Rigid.rigid_constraint{8}.ATTR.name='Rigid_Scaphoid_Y';
febio_spec.Rigid.rigid_constraint{8}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{8}.rb=3;
febio_spec.Rigid.rigid_constraint{8}.dof='Ry';
febio_spec.Rigid.rigid_constraint{8}.value.ATTR.lc=8;
febio_spec.Rigid.rigid_constraint{8}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{8}.relative=0;

febio_spec.Rigid.rigid_constraint{9}.ATTR.name='Rigid_Scaphoid_Z';
febio_spec.Rigid.rigid_constraint{9}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{9}.rb=3;
febio_spec.Rigid.rigid_constraint{9}.dof='Rz';
febio_spec.Rigid.rigid_constraint{9}.value.ATTR.lc=9;
febio_spec.Rigid.rigid_constraint{9}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{9}.relative=0;

febio_spec.Rigid.rigid_constraint{10}.ATTR.name='Rigid_Scaphoid_RX';
febio_spec.Rigid.rigid_constraint{10}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{10}.rb=3;
febio_spec.Rigid.rigid_constraint{10}.dof='Ru';
febio_spec.Rigid.rigid_constraint{10}.value.ATTR.lc=10;
febio_spec.Rigid.rigid_constraint{10}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{10}.relative=0;

febio_spec.Rigid.rigid_constraint{11}.ATTR.name='Rigid_Scaphoid_RY';
febio_spec.Rigid.rigid_constraint{11}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{11}.rb=3;
febio_spec.Rigid.rigid_constraint{11}.dof='Rv';
febio_spec.Rigid.rigid_constraint{11}.value.ATTR.lc=11;
febio_spec.Rigid.rigid_constraint{11}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{11}.relative=0;

febio_spec.Rigid.rigid_constraint{12}.ATTR.name='Rigid_Scaphoid_RZ';
febio_spec.Rigid.rigid_constraint{12}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{12}.rb=3;
febio_spec.Rigid.rigid_constraint{12}.dof='Rw';
febio_spec.Rigid.rigid_constraint{12}.value.ATTR.lc=12;
febio_spec.Rigid.rigid_constraint{12}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{12}.relative=0;

% % -> Prescribed boundary conditions on the rigid body bones
febio_spec.Rigid.rigid_constraint{13}.ATTR.name='Rigid_Lunate_side_X';
febio_spec.Rigid.rigid_constraint{13}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{13}.rb=4;
febio_spec.Rigid.rigid_constraint{13}.dof='Rx';
febio_spec.Rigid.rigid_constraint{13}.value.ATTR.lc=1;
febio_spec.Rigid.rigid_constraint{13}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{13}.relative=0;

febio_spec.Rigid.rigid_constraint{14}.ATTR.name='Rigid_Lunate_side_Y';
febio_spec.Rigid.rigid_constraint{14}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{14}.rb=4;
febio_spec.Rigid.rigid_constraint{14}.dof='Ry';
febio_spec.Rigid.rigid_constraint{14}.value.ATTR.lc=2;
febio_spec.Rigid.rigid_constraint{14}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{14}.relative=0;

febio_spec.Rigid.rigid_constraint{15}.ATTR.name='Rigid_Lunate_side_Z';
febio_spec.Rigid.rigid_constraint{15}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{15}.rb=4;
febio_spec.Rigid.rigid_constraint{15}.dof='Rz';
febio_spec.Rigid.rigid_constraint{15}.value.ATTR.lc=3;
febio_spec.Rigid.rigid_constraint{15}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{15}.relative=0;

febio_spec.Rigid.rigid_constraint{16}.ATTR.name='Rigid_Lunate_side_RX';
febio_spec.Rigid.rigid_constraint{16}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{16}.rb=4;
febio_spec.Rigid.rigid_constraint{16}.dof='Ru';
febio_spec.Rigid.rigid_constraint{16}.value.ATTR.lc=4;
febio_spec.Rigid.rigid_constraint{16}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{16}.relative=0;

febio_spec.Rigid.rigid_constraint{17}.ATTR.name='Rigid_Lunate_side_RY';
febio_spec.Rigid.rigid_constraint{17}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{17}.rb=4;
febio_spec.Rigid.rigid_constraint{17}.dof='Rv';
febio_spec.Rigid.rigid_constraint{17}.value.ATTR.lc=5;
febio_spec.Rigid.rigid_constraint{17}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{17}.relative=0;

febio_spec.Rigid.rigid_constraint{18}.ATTR.name='Rigid_Lunate_side_RZ';
febio_spec.Rigid.rigid_constraint{18}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{18}.rb=4;
febio_spec.Rigid.rigid_constraint{18}.dof='Rw';
febio_spec.Rigid.rigid_constraint{18}.value.ATTR.lc=6;
febio_spec.Rigid.rigid_constraint{18}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{18}.relative=0;

% %Scaphoid
febio_spec.Rigid.rigid_constraint{19}.ATTR.name='Rigid_Scaphoid_side_X';
febio_spec.Rigid.rigid_constraint{19}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{19}.rb=5;
febio_spec.Rigid.rigid_constraint{19}.dof='Rx';
febio_spec.Rigid.rigid_constraint{19}.value.ATTR.lc=7;
febio_spec.Rigid.rigid_constraint{19}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{19}.relative=0;

febio_spec.Rigid.rigid_constraint{20}.ATTR.name='Rigid_Scaphoid_side_Y';
febio_spec.Rigid.rigid_constraint{20}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{20}.rb=5;
febio_spec.Rigid.rigid_constraint{20}.dof='Ry';
febio_spec.Rigid.rigid_constraint{20}.value.ATTR.lc=8;
febio_spec.Rigid.rigid_constraint{20}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{20}.relative=0;

febio_spec.Rigid.rigid_constraint{21}.ATTR.name='Rigid_Scaphoid_side_Z';
febio_spec.Rigid.rigid_constraint{21}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{21}.rb=5;
febio_spec.Rigid.rigid_constraint{21}.dof='Rz';
febio_spec.Rigid.rigid_constraint{21}.value.ATTR.lc=9;
febio_spec.Rigid.rigid_constraint{21}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{21}.relative=0;

febio_spec.Rigid.rigid_constraint{22}.ATTR.name='Rigid_Scaphoid_side_RX';
febio_spec.Rigid.rigid_constraint{22}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{22}.rb=5;
febio_spec.Rigid.rigid_constraint{22}.dof='Ru';
febio_spec.Rigid.rigid_constraint{22}.value.ATTR.lc=10;
febio_spec.Rigid.rigid_constraint{22}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{22}.relative=0;

febio_spec.Rigid.rigid_constraint{23}.ATTR.name='Rigid_Scaphoid_side_RY';
febio_spec.Rigid.rigid_constraint{23}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{23}.rb=5;
febio_spec.Rigid.rigid_constraint{23}.dof='Rv';
febio_spec.Rigid.rigid_constraint{23}.value.ATTR.lc=11;
febio_spec.Rigid.rigid_constraint{23}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{23}.relative=0;

febio_spec.Rigid.rigid_constraint{24}.ATTR.name='Rigid_Scaphoid_side_RZ';
febio_spec.Rigid.rigid_constraint{24}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{24}.rb=5;
febio_spec.Rigid.rigid_constraint{24}.dof='Rw';
febio_spec.Rigid.rigid_constraint{24}.value.ATTR.lc=12;
febio_spec.Rigid.rigid_constraint{24}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{24}.relative=0;

%%Capitate
febio_spec.Rigid.rigid_constraint{25}.ATTR.name='Rigid_Capitate_X';
febio_spec.Rigid.rigid_constraint{25}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{25}.rb=6;
febio_spec.Rigid.rigid_constraint{25}.dof='Rx';
febio_spec.Rigid.rigid_constraint{25}.value.ATTR.lc=13;
febio_spec.Rigid.rigid_constraint{25}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{25}.relative=0;

febio_spec.Rigid.rigid_constraint{26}.ATTR.name='Rigid_Capitate_Y';
febio_spec.Rigid.rigid_constraint{26}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{26}.rb=6;
febio_spec.Rigid.rigid_constraint{26}.dof='Ry';
febio_spec.Rigid.rigid_constraint{26}.value.ATTR.lc=14;
febio_spec.Rigid.rigid_constraint{26}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{26}.relative=0;

febio_spec.Rigid.rigid_constraint{27}.ATTR.name='Rigid_Capitate_Z';
febio_spec.Rigid.rigid_constraint{27}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{27}.rb=6;
febio_spec.Rigid.rigid_constraint{27}.dof='Rz';
febio_spec.Rigid.rigid_constraint{27}.value.ATTR.lc=15;
febio_spec.Rigid.rigid_constraint{27}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{27}.relative=0;

febio_spec.Rigid.rigid_constraint{28}.ATTR.name='Rigid_Capitate_RX';
febio_spec.Rigid.rigid_constraint{28}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{28}.rb=6;
febio_spec.Rigid.rigid_constraint{28}.dof='Ru';
febio_spec.Rigid.rigid_constraint{28}.value.ATTR.lc=16;
febio_spec.Rigid.rigid_constraint{28}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{28}.relative=0;

febio_spec.Rigid.rigid_constraint{29}.ATTR.name='Rigid_Capitate_RY';
febio_spec.Rigid.rigid_constraint{29}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{29}.rb=6;
febio_spec.Rigid.rigid_constraint{29}.dof='Rv';
febio_spec.Rigid.rigid_constraint{29}.value.ATTR.lc=17;
febio_spec.Rigid.rigid_constraint{29}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{29}.relative=0;

febio_spec.Rigid.rigid_constraint{30}.ATTR.name='Rigid_Capitate_RZ';
febio_spec.Rigid.rigid_constraint{30}.ATTR.type='prescribe';
febio_spec.Rigid.rigid_constraint{30}.rb=6;
febio_spec.Rigid.rigid_constraint{30}.dof='Rw';
febio_spec.Rigid.rigid_constraint{30}.value.ATTR.lc=18;
febio_spec.Rigid.rigid_constraint{30}.value.VAL=Mag_val;
febio_spec.Rigid.rigid_constraint{30}.relative=0;

% %Radius
febio_spec.Rigid.rigid_constraint{31}.ATTR.name='Rigid_Radiuas';
febio_spec.Rigid.rigid_constraint{31}.ATTR.type='fix';
febio_spec.Rigid.rigid_constraint{31}.rb=7;
febio_spec.Rigid.rigid_constraint{31}.dofs='Rx,Ry,Rz,Ru,Rv,Rw';

k=1;
for i=1:numTimeSteps+1

    if k==1
        Mat_X_Lunate(k,1)=0;
        Mat_X_Lunate(k,2)=0;

        Mat_Y_Lunate(k,1)=0;
        Mat_Y_Lunate(k,2)=0;

        Mat_Z_Lunate(k,1)=0;
        Mat_Z_Lunate(k,2)=0;

        Mat_RX_Lunate(k,1)=0;
        Mat_RX_Lunate(k,2)=0;

        Mat_RY_Lunate(k,1)=0;
        Mat_RY_Lunate(k,2)=0;

        Mat_RZ_Lunate(k,1)=0;
        Mat_RZ_Lunate(k,2)=0;

        Mat_X_Scaphoid(k,1)=0;
        Mat_X_Scaphoid(k,2)=0;

        Mat_Y_Scaphoid(k,1)=0;
        Mat_Y_Scaphoid(k,2)=0;

        Mat_Z_Scaphoid(k,1)=0;
        Mat_Z_Scaphoid(k,2)=0;

        Mat_RX_Scaphoid(k,1)=0;
        Mat_RX_Scaphoid(k,2)=0;

        Mat_RY_Scaphoid(k,1)=0;
        Mat_RY_Scaphoid(k,2)=0;

        Mat_RZ_Scaphoid(k,1)=0;
        Mat_RZ_Scaphoid(k,2)=0;

        Mat_X_Capitate(k,1)=0;
        Mat_X_Capitate(k,2)=0;

        Mat_Y_Capitate(k,1)=0;
        Mat_Y_Capitate(k,2)=0;

        Mat_Z_Capitate(k,1)=0;
        Mat_Z_Capitate(k,2)=0;

        Mat_RX_Capitate(k,1)=0;
        Mat_RX_Capitate(k,2)=0;

        Mat_RY_Capitate(k,1)=0;
        Mat_RY_Capitate(k,2)=0;

        Mat_RZ_Capitate(k,1)=0;
        Mat_RZ_Capitate(k,2)=0;
    else
        Mat_X_Lunate(k,1)=tStartNow(k-1);
        Mat_X_Lunate(k,2)=Prescribe_Lunate{k-1}(1);

        Mat_Y_Lunate(k,1)=tStartNow(k-1);
        Mat_Y_Lunate(k,2)=Prescribe_Lunate{k-1}(2);

        Mat_Z_Lunate(k,1)=tStartNow(k-1);
        Mat_Z_Lunate(k,2)=Prescribe_Lunate{k-1}(3);

        Mat_RX_Lunate(k,1)=tStartNow(k-1);
        Mat_RX_Lunate(k,2)=AngleLunate{k-1}(1);

        Mat_RY_Lunate(k,1)=tStartNow(k-1);
        Mat_RY_Lunate(k,2)=AngleLunate{k-1}(2);

        Mat_RZ_Lunate(k,1)=tStartNow(k-1);
        Mat_RZ_Lunate(k,2)=AngleLunate{k-1}(3);

        Mat_X_Scaphoid(k,1)=tStartNow(k-1);
        Mat_X_Scaphoid(k,2)=Prescribe_Scaphoid{k-1}(1);

        Mat_Y_Scaphoid(k,1)=tStartNow(k-1);
        Mat_Y_Scaphoid(k,2)=Prescribe_Scaphoid{k-1}(2);

        Mat_Z_Scaphoid(k,1)=tStartNow(k-1);
        Mat_Z_Scaphoid(k,2)=Prescribe_Scaphoid{k-1}(3);

        Mat_RX_Scaphoid(k,1)=tStartNow(k-1);
        Mat_RX_Scaphoid(k,2)=AngleScaphoid{k-1}(1);

        Mat_RY_Scaphoid(k,1)=tStartNow(k-1);
        Mat_RY_Scaphoid(k,2)=AngleScaphoid{k-1}(2);

        Mat_RZ_Scaphoid(k,1)=tStartNow(k-1);
        Mat_RZ_Scaphoid(k,2)=AngleScaphoid{k-1}(3);

        Mat_X_Capitate(k,1)=tStartNow(k-1);
        Mat_X_Capitate(k,2)=Prescribe_Capitate{k-1}(1);

        Mat_Y_Capitate(k,1)=tStartNow(k-1);
        Mat_Y_Capitate(k,2)=Prescribe_Capitate{k-1}(2);

        Mat_Z_Capitate(k,1)=tStartNow(k-1);
        Mat_Z_Capitate(k,2)=Prescribe_Capitate{k-1}(3);

        Mat_RX_Capitate(k,1)=tStartNow(k-1);
        Mat_RX_Capitate(k,2)=AngleCapitate{k-1}(1);

        Mat_RY_Capitate(k,1)=tStartNow(k-1);
        Mat_RY_Capitate(k,2)=AngleCapitate{k-1}(2);

        Mat_RZ_Capitate(k,1)=tStartNow(k-1);
        Mat_RZ_Capitate(k,2)=AngleCapitate{k-1}(3);

    end
   k=k+1;
end

%Lunate
febio_spec.LoadData.load_controller{1}.ATTR.id=1;
febio_spec.LoadData.load_controller{1}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{1}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{1}.points.point.VAL=Mat_X_Lunate;

febio_spec.LoadData.load_controller{2}.ATTR.id=2;
febio_spec.LoadData.load_controller{2}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{2}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{2}.points.point.VAL=Mat_Y_Lunate;

febio_spec.LoadData.load_controller{3}.ATTR.id=3;
febio_spec.LoadData.load_controller{3}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{3}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{3}.points.point.VAL=Mat_Z_Lunate;

febio_spec.LoadData.load_controller{4}.ATTR.id=4;
febio_spec.LoadData.load_controller{4}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{4}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{4}.points.point.VAL=Mat_RX_Lunate;

febio_spec.LoadData.load_controller{5}.ATTR.id=5;
febio_spec.LoadData.load_controller{5}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{5}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{5}.points.point.VAL=Mat_RY_Lunate;

febio_spec.LoadData.load_controller{6}.ATTR.id=6;
febio_spec.LoadData.load_controller{6}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{6}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{6}.points.point.VAL=Mat_RZ_Lunate;

% %Scaphoid
febio_spec.LoadData.load_controller{7}.ATTR.id=7;
febio_spec.LoadData.load_controller{7}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{7}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{7}.points.point.VAL=Mat_X_Scaphoid;

febio_spec.LoadData.load_controller{8}.ATTR.id=8;
febio_spec.LoadData.load_controller{8}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{8}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{8}.points.point.VAL=Mat_Y_Scaphoid;

febio_spec.LoadData.load_controller{9}.ATTR.id=9;
febio_spec.LoadData.load_controller{9}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{9}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{9}.points.point.VAL=Mat_Z_Scaphoid;

febio_spec.LoadData.load_controller{10}.ATTR.id=10;
febio_spec.LoadData.load_controller{10}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{10}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{10}.points.point.VAL=Mat_RX_Scaphoid;

febio_spec.LoadData.load_controller{11}.ATTR.id=11;
febio_spec.LoadData.load_controller{11}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{11}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{11}.points.point.VAL=Mat_RY_Scaphoid;

febio_spec.LoadData.load_controller{12}.ATTR.id=12;
febio_spec.LoadData.load_controller{12}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{12}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{12}.points.point.VAL=Mat_RZ_Scaphoid;

% %Capitate
febio_spec.LoadData.load_controller{13}.ATTR.id=13;
febio_spec.LoadData.load_controller{13}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{13}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{13}.points.point.VAL=Mat_X_Capitate;

febio_spec.LoadData.load_controller{14}.ATTR.id=14;
febio_spec.LoadData.load_controller{14}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{14}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{14}.points.point.VAL=Mat_Y_Capitate;

febio_spec.LoadData.load_controller{15}.ATTR.id=15;
febio_spec.LoadData.load_controller{15}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{15}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{15}.points.point.VAL=Mat_Z_Capitate;

febio_spec.LoadData.load_controller{16}.ATTR.id=16;
febio_spec.LoadData.load_controller{16}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{16}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{16}.points.point.VAL=Mat_RX_Capitate;

febio_spec.LoadData.load_controller{17}.ATTR.id=17;
febio_spec.LoadData.load_controller{17}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{17}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{17}.points.point.VAL=Mat_RY_Capitate;

febio_spec.LoadData.load_controller{18}.ATTR.id=18;
febio_spec.LoadData.load_controller{18}.ATTR.type='loadcurve';
febio_spec.LoadData.load_controller{18}.interpolate='LINEAR';
febio_spec.LoadData.load_controller{18}.points.point.VAL=Mat_RZ_Capitate;

febio_spec.Output.logfile.ATTR.file=febioLogFileName;
febio_spec.Output.logfile.node_data{1}.ATTR.file=febioLogFileName_disp;
febio_spec.Output.logfile.node_data{1}.ATTR.data='ux;uy;uz';
febio_spec.Output.logfile.node_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.node_data{1}.VAL=1:size(V,1);

febio_spec.Output.logfile.node_data{2}.ATTR.file=febioLogFileName_force;
febio_spec.Output.logfile.node_data{2}.ATTR.data='Rx;Ry;Rz';
febio_spec.Output.logfile.node_data{2}.ATTR.delim=',';
febio_spec.Output.logfile.node_data{2}.VAL=1:size(V,1);

febio_spec.Output.logfile.element_data{1}.ATTR.file=febioLogFileName_stress;
febio_spec.Output.logfile.element_data{1}.ATTR.data='sx;sy;sz';
febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';
%% Quick viewing of the FEBio input file structure
%The febView function can be used to view the xml structure in a MATLAB figure window.
%
%disp('Viewing the febio file');
%febView(febio_spec); %Viewing the febio file

%% Exporting the FEBio input file
% Exporting the febio_spec structure to an FEBio input file is done using
% the |febioStruct2xml| function.
febioStruct2xml(febio_spec,febioFebFileName); %Exporting to file and domNode

% %
% %
% % %% Running the FEBio analysis
% % % To run the analysis defined by the created FEBio input file the
% % % |runMonitorFEBio| function is used. The input for this function is a
% % % structure defining job settings e.g. the FEBio input file name. The
% % % optional output runFlag informs the user if the analysis was run
% % % succesfully.

febioAnalysis.run_filename=febioFebFileName; %The input file name
febioAnalysis.run_logname=febioLogFileName; %The name for the log file
febioAnalysis.disp_on=1; %Display information on the command window
febioAnalysis.disp_log_on=1; %Display convergence information in the command window
febioAnalysis.runMode='internal';%'internal';
febioAnalysis.t_check=0.25; %Time for checking log file (dont set too small)
febioAnalysis.maxtpi=1e99; %Max analysis time
febioAnalysis.maxLogCheckTime=3; %Max log file checking time

[runFlag]=runMonitorFEBio(febioAnalysis);%START FEBio NOW!!!!!!!!


function [mu, con, B] = clvm_Mstep(w, Y, YY, YTY, X, XX, XY, varargin)
% MSTEP_CLG Compute ML/MAP estimates for a conditional linear Von Mises
% [mu, Con, B] = clvm_Mstep(w, Y, YY, YTY, X, XX, XY, varargin)

% This currently only accounts for conditions where there are no cts parents.
% Dsc-->Cts is the structure that this can handle.

% INPUTS:
% w(i) = sum_t w(i,t) = responsibilities for each mixture component
%  If there is only one mixture component (i.e., Q does not exist),
%  then w(i) = N = nsamples,  and
%  all references to i can be replaced by 1.
% Y(:,i) = sum_t w(i,t) cos(y(:,t)) = weighted observations
% YY(:,:,i) = sum_t w(i,t) sin(y(:,t)) = weighted outer product

[cov_type, tied_cov, ...
 clamped_cov, clamped_mean, clamped_weights,  cov_prior, ...
 xs, ys, post] = ...
    process_options(varargin, ...
		    'cov_type', 'full', 'tied_cov', 0,  'clamped_cov', [], 'clamped_mean', [], ...
		    'clamped_weights', [], 'cov_prior', [], ...
		    'xs', [], 'ys', [], 'post', []);

[Ysz Q] = size(Y);

if isempty(X) % no regression
    B2 = zeros(Ysz, 1, Q);
  for i=1:Q
    B(:,:,i) = B2(:,1:0,i); % make an empty array of size Ysz x 0 x Q
  end
  [mu, con] = mixvonMises_Mstep(w, Y, YY, YTY, varargin{:});
  return;
end


function [yarray0, dyarray0, tarray] = calctraj(via,dt,tacc)
ptlen = size(via,2);
y0 = via(1,:);
yarray0 = mstraj(via(2:end,:),ones(1,ptlen),[],y0,dt,tacc); % row vector for each xr
yarray0 = [y0;yarray0];
dyarray0 = [diff(yarray0)./dt;zeros(1,ptlen)];
tarray = 0:dt:(size(yarray0,1)-1)*dt;
end


%%********************************************************
%% Arrow:
%%
%% Fx = Arrow(pblk,f,x,options);
%%
%% if options == 0;
%%    Fx = Arr(F)*x
%% if options == 1;
%%    Fx = Arr(F)^{-1}*x
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%********************************************************

   function   Fx = Arrow(pblk,f,x,options);

   if nargin == 3; options = 0; end;

   s = 1 + [0, cumsum(pblk{2})];
   idx1 = s(1:length(pblk{2}));
   if options == 0
      inprod = mexqops(pblk{2},f,x,1);
      Fx  = mexqops(pblk{2},f(idx1),x,3) + mexqops(pblk{2},x(idx1),f,3);
      Fx(idx1) = inprod;
   else
      gamf2 = mexqops(pblk{2},f,f,2);
      gamprod = mexqops(pblk{2},f,x,2);
      alpha = gamprod./gamf2;
      Fx = mexqops(pblk{2},1./f(idx1),x,3) - mexqops(pblk{2},alpha./f(idx1),f,3);
      Fx(idx1) = alpha;
   end
%%
%%********************************************************


function [p, v, q] = ch_nav_equ_local_tan(p, v, q ,acc, gyr, dt, gN)
%  惯导解算更新，当地直角坐标系，不考虑地球自转
% p          位置 XYZ 单位 m
% v          速度 XYZ 单位 m/s
% q         Qb2n姿态,四元数表示
% acc      比力， 加速度计测量值 单位  (m/s^2),
% gyr      角速度 (rad/s)]
% dt        dt (s) 积分间隔如 0.01s
% gn       当地重力向量

old_v = v;

sf = acc;

%  姿态结算
q = ch_att_upt(q, gyr, dt);

% 速度解算
sf = ch_qmulv(q, sf);
sf = sf + gN;
v = old_v + dt *sf;

% 位置解算
p = p + (old_v + v) *dt/2;

end

%
%
% function x = ch_nav_equ_local_tan(x ,u, dt, gN)
%
% persistent a_old;
% if isempty(a_old)
%    a_old= u(1:3);
% end
%
% old_v = x(4:6);
%
% a_new =u(1:3);
% %sf = sf + 0.5*cross(u(4:6)*dt, sf);
%
% %  姿态结算
% gyr = u(4:6);
% q_old = x(7:10);
% x(7:10) = ch_att_upt(x(7:10), gyr, dt);
% q_new = x(7:10);
%
% % 速度解算
%
% x(4:6) = old_v + ((ch_qmulv(q_new, a_new) + ch_qmulv(q_old, a_old) )/2 + gN) *dt;
%
% % 位置解算
% x(1:3) = x(1:3) + (old_v + x(4:6)) *dt/2;
% a_old = a_new;
% end
%
%


function y = psdscale(ud,x,K,transp)
% y = psdscale(ud,x,K [,transp])
%
% PSDSCALE  Computes length lenud (=sum(K.s.^2)) vector y.
%   !transp (default) then y[k] = vec(Ldk' * Xk * Ldk)
%   transp == 1 then y[k] = vec(Udk' * Xk * Udk)
%   Uses pivot ordering ud.perm if available and nonempty.
%
% **********  INTERNAL FUNCTION OF SEDUMI **********
%
% See also scaleK, factorK.

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

%The function is quite dirty as the type and dimension of the inputs may
%change, that is why we have so many subcases.

Ks = K.s;
if isempty(Ks)
    y = [];
    return
end
if nargin < 4
    transp = false;
end
Kq = Ks .* Ks;
nr = K.rsdpN;
nc = length(Ks);
N  = sum(Kq) + sum(Kq(nr+1:end));
y  = zeros(N,1);
xi = length(x) - N;
yi = 0;
ui = 0;
if isstruct(ud)
    perm = ud.perm;
    if isempty(perm)
        prep = false;
        postp = false;
    else
        prep = ~transp;
        postp = transp;
        pi = 0;
    end
    ud = ud.u;
else
    prep  = false;
    postp = false;
end
for i = 1 : nc
    ki = Ks(i);
    qi = Kq(i);
    TT = ud(ui+1:ui+qi); ui=ui+qi;
    if i > nr
        TT = TT + 1i*ud(ui+1:ui+qi); ui=ui+qi;
    end
    TT = reshape(TT,ki,ki);
    if transp
        TT = triu(TT);
    else
        TT = tril(TT);
    end
    XX = x(xi+1:xi+qi); xi=xi+qi;
    if i > nr
        XX = XX + 1i*x(xi+1:xi+qi); xi=xi+qi;
    end
    XX = reshape(XX,ki,ki);
    if prep
        PP = perm(pi+1:pi+ki); pi=pi+ki;
        if any(diff(PP)~=1)
            XX = XX(PP,PP);
        end
    end
    if nnz(XX) < 0.1 * qi
        XX = sparse(XX);
    end
    XX = TT' * XX * TT;
    if postp
        PP = perm(pi+1:pi+ki); pi=pi+ki;
        if any(diff(PP)~=1)
            XX(PP,PP) = XX;
        end
    end
    y(yi+1:yi+qi) = real(XX);
    yi = yi+qi;
    if i > nr
        XX = imag(XX);
        % Needed, otherwise psdfactor() will sometimes fail.
        XX(1:ki+1:end) = 0;
        y(yi+1:yi+qi) = XX;
        yi = yi+qi;
    end
end


%  The soacm code is modified on the basis of an implementation of C-V model, which is:
%  created on 04/26/2004
%  author: Chunming Li
%  email: li_chunming@hotmail.com
%  Copyright (c) 2004-2006 by Chunming Li
function phi = evol_soacm(ort,img,phi_init,mu,nu,lambda_1,lambda_2,delta_t,epsilon)

    img = mirror_expand(img);

    phi = mirror_expand(phi_init);
    phi = mirror_ensure(phi);
    delta_h = Delta(phi,epsilon);
    cur = curvature(phi); % compute curvature of phi function
    [gray_c1,gray_c2] = binaryfit(phi,img,epsilon);

    if ort == -1
        t1 = lambda_1 * (img-gray_c1).^2;
        t2 = lambda_2 * (img-gray_c2).^2;
        phi = phi + delta_t * delta_h .* ( mu*cur - nu - t1 + t2);
    else
        t3 = (img-ort).^2;
        phi = phi + delta_t * delta_h .* ( mu*cur - nu - t3);
    end

    phi = mirror_shrink(phi);
end


function h = labelpoints (xpos, ypos, labels, varargin)
%  h = labelpoints (xpos, ypos, labels, position, buffer, adjust_axes, {parameters})
%
%   Given x and y position vectors (xpos, ypos) and given
%     a vector of labels this script will label all data points
%     and output the label handles in vector, h.
%
%   'xpos' and 'ypos' are required to be the same length. However, if all labels
%     fall along the same horizontal or vertical line, the function will accept
%     a single number for that xpos or ypos (see examples).
%
%   'labels' should be a cell or numerical array of the
%     same length as xpos or ypos.  Alternatively it can be a singleton
%     that will be replicated for all labels (see examples).
%
%   'position' (optional) describes the position of the labels
%     relative to their locations by entering one of the following
%     abbreviated compass directions in single quotes (default is 'NW').
%     N, S, E, W, NE, NW, SE, SW, center
%
%   'buffer' (optional) is a number between 0:1 that adds distance between
%     the label and the plotted point where 0 (default) is none and 1 is
%     1/10th of the axis.  Ignored for 'center' position.
%
%   'adjust_axes' (optional, default=0): depending on the positioning of labels,
%     some may fall beyond the axis limits. adjust_axes = 1 will readjust xlim
%     & ylim slightly so labels are not beyond axis limits.
%
%   This function includes three optional parameters to label only the outliers.
%     This can come in handy when there are many points but only the outliers
%     should be labeled. (may require stats toolbox)  (see examples)
%       'outliers_SD', N   -  will only label points that are greater than N
%                             standard deviations from the median of xpos or ypos.
%       'outliers_Q', N    -  will only label points that are greater than N
%                             times the interquartile range of xpos or ypos.
%       'outliers_N', N    -  will calculate the distance of each point from the median
%                             point(xpos,ypos) and will label the N-furthest points.
%                             Alternatively, N can be a decimal to label N% of the points.
%
%   The following parameters may also be entered in any order
%     'FontSize', N -   font size of all labels
%     'Color', S    -   font color of all labels ('y' 'm' 'c' 'r' 'g' 'b' 'w' 'k')
%     'rotation', N -   will rotate the labels N degrees about label-center
%                       (positive is counterclockwise).
%
%
%   Examples:
%      Fake Data:
%       x = 1:10;  y=rand(1,10);
%       scatter(x,y)
%       labs = {'a' 'b' 'c' 'd' 'middle' 'f' 'g' 'h' 'i' 'last'};
%
%      Label Examples
%       txt_h = labelpoints(x, y, labs);
%       txt_h = labelpoints(x, y, labs, 'E', 0.15);
%       txt_h = labelpoints(x, y, labs, 'E', 0.15, 1);
%       txt_h = labelpoints(x, y, labs, 'W', 0.15, 1, 'FontSize', 14, 'Color', 'r');
%       txt_h = labelpoints(x, y, labs, 'W', 0.15, 1, 'FontSize', 12, 'Color', 'm', 'rotation', 45);
%
%      Also works for
%       labs = [1:1:10];
%       labs = {'Sofia' '' '' '' 'Bucharest' '' '' 'Belgrade' '' 'Ankara'}
%       labs = '*';
%       labs = 'string';
%
%      When all labels share same xpos or ypos
%         boxplot(1:10)
%         labelpoints(0.8, [3, 5.5, 8], {'25%' '50%' '75%'}, 'center');
%
%      Outlier Examples
%       Fake Data:
%         x = [rand(1,30), rand(1,8)*2];
%         y = [rand(1,30), rand(1,8)*2];
%         scatter(x, y)
%         labs = 1:38;
%        labelpoints(x, y, labs, 'N', 0.1, 1, 'outliers_N', 5);                  %will label 5 furthest points from median
%        labelpoints(x, y, labs, 'N', 0.1, 1, 'outliers_N', 0.1, 'Color', 'r');  %will label 10% of furthest points from median
%        labelpoints(x, y, labs, 'N', 0.1, 1, 'outliers_SD', 2);                 %will label all points > 2 SD from median
%        labelpoints(x, y, labs, 'N', 0.1, 1, 'outliers_Q', 1.5);                %will label points greater that 1.5 x IQR
%
%   Alternative use:
%     Density Distributions:
%       x = randn(1,100); y = 1:100;
%       scatter(x,y)
%       labs = '|';
%       txt_h = labelpoints(x, 8, labs, 'center');
%
%     Single Labeling
%       x = 2004:2013;  y=rand(1,10);
%       plot(x,y,'-o')
%       labs = 'acquisition';
%       labelpoints(x(3), y(3), labs, 'N', 0.2, 1);
%       labelpoints(2008.5, min(ylim), {['labelpoints.m   ', datestr(now, 'mm/dd/yy')]}, 'N', 0.3, 0, 'fontsize', 12, 'color', 'm');
%
%     Use labels instead of markers
%       x = randn(1,15); y = randn(1,15);
%       labs = char('a'+(1:15)-1)';
%       labelpoints(x, y, labs, 'center', 0, 1, 'color', 'b');
%
%  140331 v.1
%  141115 v.2
% Copyright (c) 2014, Adam Danz
%All rights reserved

% source: http://www.mathworks.com/matlabcentral/fileexchange/46891-labelpoints

% Changes history
%   11/02/14    if labels are entered as char, they are convered to cell
%   11/03/14    varargin added to accomodate auto_outlier feature
%   11/03/14    convert inputs to row vector if they are column vectors
%   11/04/14    now position in put is not case sensitive
%   11/04/14    now 1 label can be entered to label multiple points
%   11/04/14    now fontsize and color can be specified by params
%   11/05/14    changed 'outlier' to 'outlier_SD' and added 'outlier_Q'
%   11/07/14    added option to rotate test
%   11/15/14    added 'outliers_N' input option and cleaned up text rotation section.
%   11/19/14    curr_extent is not always a cell.  fixed.
%   11/20/14    when outliers_N is selected N is min(N, lenght(xpos));
%   11/21/14    removes entire point and label when there is an 'inf' value

%%
% Check Class of 'labels'
    %If 'labels' are numberical, convert to cell
    if isnumeric(labels) == 1
        labels = num2cell(labels);
    end

    % if 'labels' are char, convert to cell
    if ischar(labels)
        labels = cellstr(labels);
    end

% if all labels share the same xpos or ypos (only 1 value entered in 1 of the position vectors)
    if length(xpos)==1 && length(ypos)>1
        xpos = repmat(xpos, size(ypos));
    elseif length(ypos)==1 && length(xpos)>1
        ypos = repmat(ypos, size(xpos));
    end

% if only one lable is entered for all points, replicate it
    if length(labels)==1 && length(xpos) > 1
        labels = repmat(labels, [1, length(xpos)]);
    end

% ensures xpos, ypos, and labels are all row vectors
    if iscolumn(xpos);      xpos = xpos';       end
    if iscolumn(ypos);      ypos = ypos';       end
    if iscolumn(labels);    labels = labels';   end

%check that x, y, and labels are same length
    if isequal(length(xpos), length(ypos), length(labels)) == 0
        error('xpos, ypos, and labels must all be the same length unless using one input for labels.')
    end

%if an 'inf' value is entered, this will remove that entire point and label
    xinf = find(xpos==inf);
    yinf = find(ypos==inf);
    findinf = [xinf yinf];
    if ~isempty(findinf)
        xpos(findinf)=[];
        ypos(findinf)=[];
        labels(findinf) = [];
    end

%Validate inputs and optional parameters
    validPositions = {'N' 'NE' 'E' 'SE' 'S' 'SW' 'W' 'NW' 'center'};
    checkPosition = @(x) any(validatestring(x, validPositions));

    validColors = {'y' 'm' 'c' 'r' 'g' 'b' 'w' 'k'};
    checkColors = @(x) any(validatestring(x, validColors));

    p = inputParser;
    p.FunctionName = mfilename;
    addRequired(p, 'xpos', @isnumeric);
    addRequired(p, 'ypos', @isnumeric);
    addRequired(p, 'labels');
    addOptional(p, 'position', 'NW', checkPosition);
    addOptional(p, 'buffer', 0, @isnumeric);
    addOptional(p, 'adjust_axes', 0, @isnumeric);

    addParamValue(p, 'outliers_SD', 3, @isnumeric);
    addParamValue(p, 'outliers_Q', 1.5, @isnumeric);
    addParamValue(p, 'outliers_N', 1, @isnumeric);

    addParamValue(p, 'FontSize', 10, @isnumeric);
    addParamValue(p, 'Color', 'k', checkColors);
    addParamValue(p, 'rotation', 0, @isnumeric);
    parse(p, xpos, ypos, labels, varargin{:})

mfile = [mfilename,'.m'];

%calculate buffer
    a = axis/10;% I've somewhat arbitrarily divided by 10 to make 'buffer' more sensitive
    u1 = 0;     %x offset
    u2 = 0;     %y offset

%assign position
    switch upper(p.Results.position)
        case 'E',       va = 'middle'; ha = 'left';         u1 = a(2)-a(1);
        case 'W',       va = 'middle'; ha = 'right';        u1 = (a(2)-a(1))*-1;
        case 'N',       va = 'bottom'; ha = 'center';       u2 = a(4)-a(3);
        case 'S',       va = 'top'; ha = 'center';          u2 = (a(4)-a(3))*-1;
        case 'NE',      va = 'bottom'; ha = 'left';         u1 = (a(2)-a(1))/2;     u2 = (a(4)-a(3))/2;
        case 'NW',      va = 'bottom'; ha = 'right';        u1 = (a(2)-a(1))*-0.5;  u2 = (a(4)-a(3))/2;
        case 'SE',      va = 'top'; ha = 'left';            u1 = (a(2)-a(1))/2;     u2 = (a(4)-a(3))*-0.5;
        case 'SW',      va = 'top'; ha = 'right';           u1 = (a(2)-a(1))*-0.5;  u2 = (a(4)-a(3))*-0.5;
        case 'CENTER',  va = 'middle'; ha = 'center';
    end

%Factor in buffer
    u1 = u1*p.Results.buffer;
    u2 = u2*p.Results.buffer;

%If outliers parameters are selected
    if sum(strcmp(varargin, 'outliers_SD')) == 1 || sum(strcmp(varargin, 'outliers_Q')) == 1 || sum(strcmp(varargin, 'outliers_N')) == 1
        if sum(strcmp(varargin, 'outliers_SD')) == 1
            outlier_idx = logical(abs(xpos - median(xpos)) > p.Results.outliers_SD*std(xpos)  |  abs(ypos - median(ypos)) > p.Results.outliers_SD*std(ypos)); %index of outliers

        elseif sum(strcmp(varargin, 'outliers_Q')) == 1
            xbounds = [prctile(xpos,25) - p.Results.outliers_Q * iqr(xpos) , prctile(xpos, 75) + p.Results.outliers_Q * iqr(xpos)];   %[lower upper] bounds of outliers
            ybounds = [prctile(ypos,25) - p.Results.outliers_Q * iqr(ypos) , prctile(ypos, 75) + p.Results.outliers_Q * iqr(ypos)];   %[lower upper] bounds of outliers
            outlier_idx = logical(ypos<ybounds(1) | ypos>ybounds(2) |  xpos<xbounds(1) | xpos>xbounds(2));

        elseif sum(strcmp(varargin, 'outliers_N')) == 1
            if p.Results.outliers_N<1;
                N = round(length(xpos) * p.Results.outliers_N);
            else
                N = min(p.Results.outliers_N, length(xpos));        %ensures that user cannot label more outliers than coordinates.
            end
             medianpoint = repmat([median(xpos) median(ypos)], [length(xpos),1]);
             paired = horzcat(xpos', ypos');
             distances = (((medianpoint(:,1)-paired(:,1)).^2)  +  ((medianpoint(:,2)-paired(:,2)).^2)).^(1/2);       %all distances from median
             [~, idx] = sort(distances, 'descend');
             outlier_idx = false(1,length(xpos));
             outlier_idx(idx(1:N))=1;
        end

        xpos = xpos(outlier_idx);
        ypos = ypos(outlier_idx);
        labels = labels(outlier_idx);

        if any(outlier_idx) == 0;           %dispay msg if there are no outliers to label
            disp(['There are no outliers to label in ', mfile,'.'])
            disp('Change outlier value for less sensitivity; See help file.');
        end
    end

%Label points
    h = text(xpos+u1 , ypos+u2, labels, 'VerticalAlignment',va, 'HorizontalAlignment',ha, 'FontSize', p.Results.FontSize, 'color', p.Results.Color);

%Rotate text if specified
    if sum(strcmp(varargin, 'rotation')) == 1               %if rotation parameter is called in inputs
        xl = xlim;      yl = ylim;                          %In case text rotation auto adjusts axes.
        curr_extent = get(h, 'extent');                     %Need to store current center point of all labels since text rotation relocates position
        if iscell(curr_extent); cell2mat(curr_extent); end
        hold on
        curr_position = [curr_extent(:,1)+(curr_extent(:,3)/2),curr_extent(:,2)+(curr_extent(:,4)/2)];          %uses extent to locate center of label
        set(h, 'rotation', p.Results.rotation, 'VerticalAlignment','middle', 'HorizontalAlignment','center');  	%note: text rotation changes alignment which is why they need to be centered back to specifications.
        for i = 1:length(h)                                 %after rotation, reposition lables back to desired location
            set(h(i), 'position', curr_position(i,:))
        end
        set(gca, 'xlim', xl); set(gca, 'ylim', yl);         %In case text rotation auto adjusts axes.
    end

%Determine if any labels go beyond axis limits and adjust if desired  (adjust_axes = 0 or 1)
    if p.Results.adjust_axes == 1   &&   ~isempty(h)
        x_adj = sign(u1+0.0000001);                 %the addition is to avoid '0'
        y_adj = sign(u2+0.0000001);                 %the addition is to avoid '0'

        labelextent = get(h, 'extent');
        if isequal(class(labelextent),'cell')
           labelextent = cat(1, labelextent{:});
        end
        xl = xlim;      yl = ylim;
        lablimX = [min(labelextent(:,1)), max(labelextent(:,1)+(labelextent(:,3).*x_adj))] +u1;
        lablimY = [min(labelextent(:,2)), max(labelextent(:,2)+(labelextent(:,4).*y_adj))] +u2;

        xlim([min(min(xl), min(lablimX)), max(max(xl), max(lablimX))])
        ylim([min(min(yl), min(lablimY)), max(max(yl), max(lablimY))])
    end

end

%% Notes
% a video (not mine) explaining this method:  http://blogs.mathworks.com/videos/2012/05/30/how-to-label-a-series-of-points-on-a-plot-in-matlab/
% Text properties :  http://www.mathworks.com/help/matlab/ref/text_props.html
% info on input parsing:  http://www.mathworks.com/help/matlab/matlab_prog/parse-function-inputs.html
%                   and   http://www.mathworks.com/help/matlab/ref/inputparser-class.html
% Outlier info : https://docs.oracle.com/cd/E17236_01/epm.1112/cb_statistical/frameset.htm?ch07s02s10s01.html