function [nav_e] = ins(imu, gnss, att_mode)
% ins: inertial navigation system.
%
% INPUT
%   imu, IMU data structure.
%         t: Ix1 time vector (seconds).
%        fb: Ix3 accelerations vector in body frame XYZ (m/s^2).
%        wb: Ix3 turn rates vector in body frame XYZ (radians/s).
%       arw: 1x3 angle random walks (rad/s/root-Hz).
%       vrw: 1x3 velocity random walks (m/s^2/root-Hz).
%     g_std: 1x3 gyros standard deviations (radians/s).
%     a_std: 1x3 accrs standard deviations (m/s^2).
%    gb_sta: 1x3 gyros static biases or turn-on biases (radians/s).
%    ab_sta: 1x3 accrs static biases or turn-on biases (m/s^2).
%    gb_dyn: 1x3 gyros dynamic biases or bias instabilities (radians/s).
%    ab_dyn: 1x3 accrs dynamic biases or bias instabilities (m/s^2).
%   gb_corr: 1x3 gyros correlation times (seconds).
%   ab_corr: 1x3 accrs correlation times (seconds).
%    gb_psd: 1x3 gyros dynamic biases root-PSD (rad/s/root-Hz).
%    ab_psd: 1x3 accrs dynamic biases root-PSD (m/s^2/root-Hz);
%      freq: 1x1 sampling frequency (Hz).
% ini_align: 1x3 initial attitude at t(1).
% ini_align_err: 1x3 initial attitude errors at t(1).
%
%	gnss, GNSS data structure.
%         t: Gx1 time vector (seconds).
%       lat: Gx1 latitude (radians).
%       lon: Gx1 longitude (radians).
%         h: Gx1 altitude (m).
%       vel: Gx3 NED velocities (m/s).
%       std: 1x3 position standard deviations (rad, rad, m).
%      stdm: 1x3 position standard deviations (m, m, m).
%      stdv: 1x3 velocity standard deviations (m/s).
%      larm: 3x1 lever arm from IMU to GNSS antenna (x-fwd, y-right, z-down) (m).
%      freq: 1x1 sampling frequency (Hz).
%   zupt_th: 1x1 ZUPT threshold (m/s).
%  zupt_win: 1x1 ZUPT time window (seconds).
%       eps: 1x1 time interval to compare current IMU time to current GNSS time vector (s).
%
%  att_mode: attitude mode string.
%      'quaternion': attitude updated in quaternion format. Default value.
%             'dcm': attitude updated in Direct Cosine Matrix format.
%
% OUTPUT
%   nav_e, INS/GNSS navigation estimates data structure.
%         t: Ix1 INS time vector (seconds).
%        tg: Gx1 GNSS time vector, when Kalman filter was executed (seconds).
%      roll: Ix1 roll (radians).
%     pitch: Ix1 pitch (radians).
%       yaw: Ix1 yaw (radians).
%       vel: Ix3 NED velocities (m/s).
%       lat: Ix1 latitude (radians).
%       lon: Ix1 longitude (radians).
%         h: Ix1 altitude (m).
%        xi: Gxn Kalman filter a priori states.
%        xp: Gxn Kalman filter a posteriori states.
%         z: Gxr INS/GNSS measurements
%         v: Gxr Kalman filter innovations.
%         b: Gxr Kalman filter biases compensations, [gb_dyn ab_dyn].
%         A: Gxn^2 Kalman filter transition-state matrices, one matrix per
%            row ordered by columns.
%        Pp: Gxn^2 Kalman filter a posteriori covariance matrices, one
%         matrix per row ordered by columns.
%        Pi: Gxn^2 Kalman filter a priori covariance matrices, one matrix
%            per row ordered by columns.
%         K: Gx(n*r) Kalman gain matrices
%         S: Gxr^2 Innovation matrices
%        ob: Gx1 Number of observable states after each GNSS data arriving
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%   R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
% simulation framework for low-cost integrated navigation systems,
% Journal of Control Engineering and Applied Informatics, vol. 17,
% issue 2, pp. 110-120, 2015. Alg. 2.
%
%   Groves, P.D. (2013), Principles of GNSS, Inertial, and
% Multisensor Integrated Navigation Systems (2nd Ed.). Artech House.
%
%   Crassidis, J.L. and Junkins, J.L. (2011). Optimal Esti-
% mation of Dynamic Systems, 2nd Ed. Chapman and Hall/CRC, USA.
%
%   ZUPT algothim based on Groves, Chapter 15, "INS Alignment, Zero Updates,
% and Motion Constraints".
%
%   ins_gps.m, ins_gnss function is based on that previous NaveGo function.
%
% Version: 012
% Date:    2022/07/19
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

if nargin < 3, att_mode  = 'quaternion'; end

%% ZUPT ALGORITHM

% zupt_flag = false;

%% PREALLOCATION

% Kalman filter dimensions
n = 15; % number of states
r = 6;  % number of sensors
% q = 12; % number of inputs

% Constant matrices
% I = eye(3);
% O = zeros(3);

% Length of INS time vector
LI = length(imu.t);

% Length of GNSS time vector
LG = length(gnss.t);

% Preallocation of attitude vectors
roll_e  = zeros (LI, 1);
pitch_e = zeros (LI, 1);
yaw_e   = zeros (LI, 1);

% Preallocation of velocity vector
vel_e   = zeros (LI, 3);

% Preallocation of gravity vector
gn_e   = zeros (LI, 3);

% Preallocation of position vectors
lat_e    = zeros (LI, 1);
lon_e    = zeros (LI, 1);
h_e      = zeros (LI, 1);

% Preallocation of Kalman filter matrices for later performance analysis
xi = zeros(LG, n);      % Evolution of Kalman filter a priori states
xp = zeros(LG, n);      % Evolution of Kalman filter a posteriori states
z = zeros(LG, r);       % INS/GNSS measurements
v = zeros(LG, r);       % Kalman filter innovations

A  = zeros(LG, n^2);    % Transition-state matrices
Pi = zeros(LG, n^2);    % A priori covariance matrices
Pp = zeros(LG, n^2);    % A posteriori covariance matrices
K  = zeros(LG, n*r);    % Kalman gain matrices
S  = zeros(LG, r^2);    % Innovation matrices
ob = zeros(LG, 1);      % Number of observable states at each GNSS data arriving

b = zeros(LG, r);       % Biases compensantions after Kalman filter correction

%% INITIAL VALUES AT INS TIME = 1

% Initial attitude
roll_e(1)  = imu.ini_align(1);
pitch_e(1) = imu.ini_align(2);
yaw_e(1)   = imu.ini_align(3);
DCMnb = euler2dcm([roll_e(1); pitch_e(1); yaw_e(1);]);
DCMbn = DCMnb';
qua   = euler2qua([roll_e(1) pitch_e(1) yaw_e(1)]);

% Initial velocity
vel_e(1,:) = gnss.vel(1,:);

% Initial position
lat_e(1) = gnss.lat(1);
lon_e(1) = gnss.lon(1);
h_e(1)   = gnss.h(1);

% Initial dynamic biases
gb_dyn = imu.gb_dyn';
ab_dyn = imu.ab_dyn';

% Turn-rates update with both updated velocity and position
omega_ie_n = earth_rate(lat_e(1));
omega_en_n = transport_rate(lat_e(1), vel_e(1,1), vel_e(1,2), h_e(1));

% Gravity update
gn_e(1,:) = gravity(lat_e(1), h_e(1));

%% INITIALIZATION OF KALMAN FILTER MATRICES

% Prior estimates
% kf.xi = [ zeros(1,9), imu.gb_dyn, imu.ab_dyn ]';  % Error vector state
% kf.Pi = diag([imu.ini_align_err, gnss.stdv, gnss.std, imu.gb_dyn, imu.ab_dyn].^2);
%
% kf.Q  = diag([imu.arw, imu.vrw, imu.gb_psd, imu.ab_psd].^2);
%
% fn = DCMbn * (imu.fb(1,:)' - ab_dyn - imu.ab_sta');
% wn = DCMbn * (imu.wb(1,:)' - gb_dyn - imu.gb_sta');
%
% % Vector to update matrix F
% upd = [gnss.vel(1,:) gnss.lat(1) gnss.h(1) fn' wn'];
%
% % Update matrices F and G
% [kf.F, kf.G] = F_update(upd, DCMbn, imu);
%
% [RM,RN] = radius(gnss.lat(1));
% Tpr = diag([(RM + gnss.h(1)), (RN + gnss.h(1)) * cos(gnss.lat(1)), -1]);  % radians-to-meters
%
% % Update matrix H
% kf.H = [ O I O O O ;
%     O O Tpr O O ; ];
% kf.R = diag([gnss.stdv gnss.stdm]).^2;
% kf.z = [ gnss.stdv, gnss.stdm ]';
%
% % Propagate prior estimates to get xp(1) and Pp(1)
% kf = kf_update( kf );
%
% % Initial matrices for Kalman filter performance analysis
% xi(1,:) = kf.xi';
% xp(1,:) = kf.xp';
% Pi(1,:) = reshape(kf.Pi, 1, n^2);
% Pp(1,:) = reshape(kf.Pp, 1, n^2);
% K(1,:)  = reshape(kf.K, 1, n*r);
% S(1,:)  = reshape(kf.S, 1, r^2);
% v(1,:)  = kf.v';
% z(1,:)  = kf.z';
% b(1,:) = [gb_dyn', ab_dyn'];

%% INS (IMU) TIME IS THE MASTER CLOCK
for i = 2:LI

    %% INERTIAL NAVIGATION SYSTEM (INS)

    % Print a dot on console every 10,000 INS executions
    if (mod(i,10000) == 0), fprintf('. ');  end
    % Print a return on console every 200,000 INS executions
    if (mod(i,200000) == 0), fprintf('\n'); end

    % IMU sampling interval
    dti = imu.t(i) - imu.t(i-1);

    % Inertial sensors corrected with a posteriori KF biases estimation and
    % deterministic static biases
    wb_corrected = imu.wb(i,:)' - gb_dyn - imu.gb_sta';
    fb_corrected = imu.fb(i,:)' - ab_dyn - imu.ab_sta';
    fn = DCMbn * fb_corrected;
%     wn = DCMbn * wb_corrected;

    % Velocity update
    vel = vel_update(fn, vel_e(i-1,:), omega_ie_n, omega_en_n, gn_e(i-1,:)', dti);
    vel_e (i,:) = vel;

    % Position update
    pos = pos_update([lat_e(i-1) lon_e(i-1) h_e(i-1)], vel_e(i,:), dti);
    lat_e(i) = pos(1);
    lon_e(i) = pos(2);
    h_e(i)   = pos(3);

    if ( h_e(i) < 0)
        h_e(i) = 10;
    end

    % Turn-rates update with both updated velocity and position
    omega_ie_n = earth_rate(lat_e(i));
    omega_en_n = transport_rate(lat_e(i), vel_e(i,1), vel_e(i,2), h_e(i));

    % Gravity update
    gn_e(i,:) = gravity(lat_e(i), h_e(i));

    % Attitude update
    [qua, DCMbn, euler] = att_update(wb_corrected, DCMbn, qua, ...
        omega_ie_n, omega_en_n, dti, att_mode);
    roll_e(i) = euler(1);
    pitch_e(i)= euler(2);
    yaw_e(i)  = euler(3);

    %% ZUPT DETECTION ALGORITHM
    idz = floor( gnss.zupt_win / dti ); % Index to set ZUPT window time

    if ( i > idz )

        % Mean velocity value for the ZUPT window time
        vel_m = mean (vel_e(i-idz:i , :));

        % If mean velocity value is under the ZUPT threshold velocity...
        if (abs(vel_m) < gnss.zupt_th)

            % Current attitude is equal to the mean of previous attitudes
            % inside the ZUPT window time
            roll_e(i)  = mean (roll_e(i-idz:i , :));
            pitch_e(i) = mean (pitch_e(i-idz:i , :));
            yaw_e(i)   = mean (yaw_e(i-idz:i , :));

            % Current position is equal to the mean of previous positions
            % inside the ZUPT window time
            lat_e(i) = mean (lat_e(i-idz:i , :));
            lon_e(i) = mean (lon_e(i-idz:i , :));
            h_e(i)   = mean (h_e(i-idz:i , :));

            % Alternative attitude ZUPT correction
            % roll_e(i)  = (roll_e(i-idz , :));
            % pitch_e(i) = (pitch_e(i-idz , :));
            % yaw_e(i)   = (yaw_e(i-idz, :));
            % lat_e(i) = (lat_e(i-idz:i , :));
            % lon_e(i) = (lon_e(i-idz:i , :));
            % h_e(i)   = (h_e(i-idz:i , :));

%             zupt_flag = true;

            % fprintf(' z\n')       % DEBUG
        end
    end

%     %% KALMAN FILTER UPDATE
%
%     % Check if there is a new GNSS measurement to process at current INS time
%     gdx =  find (gnss.t >= (imu.t(i) - gnss.eps) & gnss.t < (imu.t(i) + gnss.eps));
%
%     if ( ~isempty(gdx) && gdx > 1)
%
%         %  gdx       % DEBUG
%
%         %% MEASUREMENTS
%
%         % Meridian and normal radii of curvature update
%         [RM,RN] = radius(lat_e(i));
%
%         % Radians-to-meters matrix
%         Tpr = diag([(RM + h_e(i)), (RN + h_e(i)) * cos(lat_e(i)), -1]);
%
%         % Position innovations in meters with lever arm correction
%         zp = Tpr * ([lat_e(i); lon_e(i); h_e(i);] - [gnss.lat(gdx); gnss.lon(gdx); gnss.h(gdx);]) ...
%             + (DCMbn * gnss.larm);
%
%         % Velocity innovations with lever arm correction
%         zv = (vel_e(i,:) - gnss.vel(gdx,:) - ((omega_ie_n + omega_en_n) * (DCMbn * gnss.larm ))' ...
%             + (DCMbn * skewm(wb_corrected) * gnss.larm )' )';
%
%         %% KALMAN FILTER
%
%         % GNSS sampling interval
%         dtg = gnss.t(gdx) - gnss.t(gdx-1);
%
%         % Vector to update matrix F
%         upd = [vel_e(i,:) lat_e(i) h_e(i) fn' wn'];
%
%         % Matrices F and G update
%         [kf.F, kf.G] = F_update(upd, DCMbn, imu);
%
%         % Matrix H update
%         if(zupt_flag == false)
%             kf.H = [ O I O O O ;
%                 O O Tpr O O ; ];
%             kf.R = diag([gnss.stdv gnss.stdm]).^2;
%             kf.z = [ zv' zp' ]';
%         else
%             kf.H = [ O I O O O ; ];
%             kf.R = diag([gnss.stdv]).^2;
%             kf.z = zv;
%         end
%
%         % a posteriori states are forced to be zero (error-state approach)
%         kf.xp = zeros(n , 1);
%         % Execution of the extended Kalman filter
%         kf = kalman(kf, dtg);
%
%         %% OBSERVABILITY
%
%         % Number the observable states at current GNSS time
%         ob(gdx) = rank(obsv(kf.F, kf.H));
%
%         %% INS/GNSS CORRECTIONS
%
%         % Quaternion correction
%         qua_skew = -skewm(qua(1:3));    % According to Crassidis, qua_skew should be
%                                         % positive, but if positive NaveGo diverges.
%         % Crassidis, Eq. A.174a
%         Xi = [qua(4)*eye(3) + qua_skew; -qua(1:3)'];
%
%         % Crassidis, Eq. 7.34
%         qua = qua + 0.5 .* Xi * kf.xp(1:3);
%         qua = qua / norm(qua);          % Brute-force normalization
%
%         % DCM correction
%         DCMbn = qua2dcm(qua);
%
%         % Attitude correction, method 1
%         %         euler = qua2euler(qua);
%         %         roll_e(i) = euler(1);
%         %         pitch_e(i)= euler(2);
%         %         yaw_e(i)  = euler(3);
%
%         % Attitude correction, method 2
%         roll_e(i)  = roll_e(i)  - kf.xp(1);
%         pitch_e(i) = pitch_e(i) - kf.xp(2);
%         yaw_e(i)   = yaw_e(i)   - kf.xp(3);
%
%         % Velocity correction
%         vel_e(i,1) = vel_e(i,1) - kf.xp(4);
%         vel_e(i,2) = vel_e(i,2) - kf.xp(5);
%         vel_e(i,3) = vel_e(i,3) - kf.xp(6);
%
%         % Position correction
%         lat_e(i) = lat_e(i) - kf.xp(7);
%         lon_e(i) = lon_e(i) - kf.xp(8);
%         h_e(i)   = h_e(i)   - kf.xp(9);
%
%         % Biases estimation
%         gb_dyn   = -kf.xp(10:12);
%         ab_dyn   = -kf.xp(13:15);
%
%         % Matrices for later Kalman filter performance analysis
%         xi(gdx,:) = kf.xi';
%         xp(gdx,:) = kf.xp';
%         b(gdx,:) = [gb_dyn', ab_dyn'];
%         A(gdx,:)  = reshape(kf.A,  1, n^2);
%         Pi(gdx,:) = reshape(kf.Pi, 1, n^2);
%         Pp(gdx,:) = reshape(kf.Pp, 1, n^2);
%
%         if(zupt_flag == false)
%             v(gdx,:)  = kf.v';
%             z(gdx,:)  = kf.z';
%             K(gdx,:)  = reshape(kf.K, 1, n*r);
%             S(gdx,:)  = reshape(kf.S, 1, r^2);
%         else
%             zupt_flag = false;
%             z(gdx,:)  = [ kf.z' 0 0 0 ]';
%             v(gdx,:)  = [ kf.v' 0 0 0 ]';
%             K(gdx,1:n*3) = reshape(kf.K, 1, n*3);
%             S(gdx,1:9)  = reshape(kf.S, 1, 3^2);
%         end
%     end
end

%% Summary from INS/GNSS integration

nav_e.t     = imu.t(1:i, :);    % INS time vector
nav_e.tg    = gnss.t;           % GNSS time vector, which is the time vector when the Kalman filter was executed
nav_e.roll  = roll_e(1:i, :);   % Roll
nav_e.pitch = pitch_e(1:i, :);  % Pitch
nav_e.yaw   = yaw_e(1:i, :);    % Yaw
nav_e.vel   = vel_e(1:i, :);    % NED velocities
nav_e.lat   = lat_e(1:i, :);    % Latitude
nav_e.lon   = lon_e(1:i, :);    % Longitude
nav_e.h     = h_e(1:i, :);      % Altitude
nav_e.gn    = gn_e(1:i, :);     % Gravity estimation in the nav-frame.

nav_e.xi    = xi;       % A priori states
nav_e.xp    = xp;       % A posteriori states
nav_e.z     = z;        % INS/GNSS measurements
nav_e.v     = v;        % Kalman filter innovations
nav_e.b     = b;        % Biases compensations

nav_e.A     = A;        % Transition matrices
nav_e.Pi    = Pi;       % A priori covariance matrices
nav_e.Pp    = Pp;       % A posteriori covariance matrices
nav_e.K     = K;        % Kalman gain matrices
nav_e.S     = S;        % Innovation matrices
nav_e.ob    = ob;       % Number of observable states after each GNSS data arriving

fprintf('\n');

end


% LOGSPEC - plot mean log power spectra of submitted data on loglog scale
%             using PLOTDATA or PLOTTOPO formats
%
% Usage:
%        >> [spectra,freqs] = logspec(data,frames,srate);
%        >> [spectra,freqs] = logspec(data,frames,srate,'title',...
%                                    [loHz-hiHz],'chan_locs',rm_mean);
% Inputs:
%    data   = input data (chans,frames*epochs)
%    frames = data samples per epoch   {default length(data)}
%    srate  = data sampling rate in Hz {default 256 Hz};
%    'title' = plot title {default: none}
%    [loHz-hiHz] = [loHz hiHz] plotting limits
%       {default: [srate/fftlength srate/2]}
%    'chan_locs' = channel location file (ala TOPOPLOT)
%                   Else [rows cols] to plot data in a grid array
%    rm_mean = [0/1] 1 -> remove log mean spectrum from all
%
% Author: Scott Makeig, SCCN/INC/UCSD, La Jolla, 11-07-97
%
% See also: PLOTDATA, PLOTTOPO

% Copyright (C) 11-07-97 Scott Makeig, SCCN/INC/UCSD, scott@sccn.ucsd.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

% Changed PLOTDATA below to PLOTTOPO 11/12/99 -sm
% Mentioned new grid array option 12/22/99 -sm
% 01-25-02 reformated help & license, added links -ad

function [spectra,freqs] = logspec(data,frames,srate,titl,Hzlimits,chanlocs,rmmean)

if nargin < 1
  help logspec
  return
end
[rows,cols] = size(data);

icadefs  % read plotdata chan limit

if rows > MAXPLOTDATACHANS
   fprintf('logspec(): max plotdata() channels is %d.\n',MAXPLOTDATACHANS);
   return
end
if rows < 2
   fprintf('logspec(): min plotdata() channels is %d.\n',2);
   return
end
if nargin<7
  rmmean = 0; % default
end

if nargin < 5
 Hzlimits = 0;
end

if nargin < 4
  titl = ' ';
end

if nargin < 3
  srate = 256;
end

if nargin < 2,
  frames = cols;
end

epochs = fix(cols/frames);
if epochs*frames ~= cols
    fprintf('logspec() - frames does not divide data length.\n');
    return
end

fftlength = 2^floor(log(frames)/log(2));

spectra = zeros(rows,epochs*fftlength/2);
f2 = fftlength/2;
dB = 10/log(10);

if length(Hzlimits) < 2
 Hzlimits = [srate/fftlength srate/2];
end
if Hzlimits(2) <= Hzlimits(1)
   help logspec
   return
end

for e=1:epochs
  for r=1:rows
    [Pxx,freqs] = pwelch(data(r,(e-1)*frames+1:e*frames),fftlength,...
                          fftlength/4,fftlength,srate);
    spectra(r,(e-1)*f2+1:e*f2) = Pxx(2:f2+1)'; % omit DC bin
  end
end

clf
freqs = freqs(2:f2+1);
fsi = find(freqs >= Hzlimits(1) & freqs <= Hzlimits(2));
minf = freqs(fsi(1));
maxf = freqs(fsi(length(fsi)));
nfs = length(fsi);

showspec = zeros(rows,length(fsi)*epochs);
for e = 1:epochs
 showspec(:,(e-1)*nfs+1:e*nfs) = dB*log(spectra(:,(e-1)*f2+fsi));
end
% minspec = min(min(showspec));
% showspec = showspec-minspec; % make minimum 0 dB

showspec = blockave(showspec,nfs);

% meanspec = mean(showspec);
% showspec = showspec - ones(rows,1)*meanspec;

% >> plotdata(data,frames,limits,title,channames,colors,rtitle)
% diff = 0;
% MINUEND = 6;
% for r=1:rows
%  diff = diff - MINUEND;
%  showspec(r,:) = showspec(r<:)-diff;
% end
% semilogx(freqs(fsi),showspec');
% ax = axis;
% axis([minf maxf ax(3) ax(4)]);
% title(titl);

if nargin<6
% >> plotdata(data,frames,limits,title,channames,colors,rtitle,ydir)
     if rmmean
        showspec = showspec - ones(rows,1)*mean(showspec);
     end
     plotdata(showspec,nfs,[minf maxf 0 0],titl);
else
% >> plottopo(data,'chan_locs',frames,limits,title,channels,axsize,colors,ydir)
     if rmmean
        showspec = showspec - ones(rows,1)*mean(showspec);
     end
     plottopo(showspec,chanlocs,nfs,[minf maxf 0 0],titl);
end
ax = get(gcf,'children');
for a = ax
  set(a,'XScale','log')
end


classdef KRVEA < ALGORITHM
% <multi/many> <real/integer> <expensive>
% Surrogate-assisted RVEA
% alpha ---  2 --- The parameter controlling the rate of change of penalty
% wmax  --- 20 --- Number of generations before updating Kriging models
% mu    ---  5 --- Number of re-evaluated solutions at each generation

%------------------------------- Reference --------------------------------
% T. Chugh, Y. Jin, K. Miettinen, J. Hakanen, and K. Sindhya, A surrogate-
% assisted reference vector guided evolutionary algorithm for
% computationally expensive many-objective optimization, IEEE Transactions
% on Evolutionary Computation, 2018, 22(1): 129-142.
%------------------------------- 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 Cheng He

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [alpha,wmax,mu] = Algorithm.ParameterSet(2,20,5);

            %% Generate the reference points and population
            [V0,Problem.N] = UniformPoint(Problem.N,Problem.M);
            V     = V0;
            NI    = 11*Problem.D-1;
            P     = UniformPoint(NI,Problem.D,'Latin');
            A2    = Problem.Evaluation(repmat(Problem.upper-Problem.lower,NI,1).*P+repmat(Problem.lower,NI,1));
            A1    = A2;
            THETA = 5.*ones(Problem.M,Problem.D);
            Model = cell(1,Problem.M);

            %% Optimization
            while Algorithm.NotTerminated(A2)
                % Refresh the model and generate promising solutions
                A1Dec = A1.decs;
                A1Obj = A1.objs;
                for i = 1 : Problem.M
                    % The parameter 'regpoly1' refers to one-order polynomial
                    % function, and 'regpoly0' refers to constant function. The
                    % former function has better fitting performance but lower
                    % efficiency than the latter one
                    dmodel     = dacefit(A1Dec,A1Obj(:,i),'regpoly1','corrgauss',THETA(i,:),1e-5.*ones(1,Problem.D),100.*ones(1,Problem.D));
                    Model{i}   = dmodel;
                    THETA(i,:) = dmodel.theta;
                end
                PopDec = A1Dec;
                w      = 1;
                while w <= wmax
                    drawnow('limitrate');
                    OffDec = OperatorGA(Problem,PopDec);
                    PopDec = [PopDec;OffDec];
                    [N,~]  = size(PopDec);
                    PopObj = zeros(N,Problem.M);
                    MSE    = zeros(N,Problem.M);
                    for i = 1: N
                        for j = 1 : Problem.M
                            [PopObj(i,j),~,MSE(i,j)] = predictor(PopDec(i,:),Model{j});
                        end
                    end
                    index  = KEnvironmentalSelection(PopObj,V,(w/wmax)^alpha);
                    PopDec = PopDec(index,:);
                    PopObj = PopObj(index,:);
                    % Adapt referece vectors
                    if ~mod(w,ceil(wmax*0.1))
                        V(1:Problem.N,:) = V0.*repmat(max(PopObj,[],1)-min(PopObj,[],1),size(V0,1),1);
                    end
                    w = w + 1;
                end

                % Select mu solutions for re-evaluation
                [NumVf,~] = NoActive(A1Obj,V0);
                PopNew    = KrigingSelect(PopDec,PopObj,MSE(index,:),V,V0,NumVf,0.05*Problem.N,mu,(w/wmax)^alpha);
                New       = Problem.Evaluation(PopNew);
                A2        = [A2,New];
                A1        = UpdataArchive(A1,New,V,mu,NI);
            end
        end
    end
end


function e_global = error_cam_proj(param);

global n_ima x_1 X_1 xproj_1 x_proj_1 x_2 X_2 xproj_2 x_proj_2 x_3 X_3 xproj_3 x_proj_3 x_4 X_4 xproj_4 x_proj_4 x_5 X_5 xproj_5 x_proj_5 x_6 X_6 xproj_6 x_proj_6 x_7 X_7 xproj_7 x_proj_7 x_8 X_8 xproj_8 x_proj_8 x_9 X_9 xproj_9 x_proj_9 x_10 X_10 xproj_10 x_proj_10 x_11 X_11 xproj_11 x_proj_11 x_12 X_12 xproj_12 x_proj_12 x_13 X_13 xproj_13 x_proj_13 x_14 X_14 xproj_14 x_proj_14 x_15 X_15 xproj_15 x_proj_15 x_16 X_16 xproj_16 x_proj_16  x_17 X_17 xproj_17 x_proj_17 x_18 X_18 xproj_18 x_proj_18 x_19 X_19 xproj_19 x_proj_19 x_20 X_20 xproj_20 x_proj_20 x_21 X_21 xproj_21 x_proj_21 x_22 X_22 xproj_22 x_proj_22 x_23 X_23 xproj_23 x_proj_23 x_24 X_24 xproj_24 x_proj_24 x_25 X_25 xproj_25 x_proj_25 x_26 X_26 xproj_26 x_proj_26  x_27 X_27 xproj_27 x_proj_27 x_28 X_28 xproj_28 x_proj_28 x_29 X_29 xproj_29 x_proj_29 x_30 X_30 xproj_30 x_proj_30

% This is the same model, but with a simpler distortion model (no 6th order)

% Computation of the errors:

fc = param(1:2);
cc = param(3:4);
alpha_c = param(5);
kc = [param(6:9);0];

e_cam = [];

for kk = 1:n_ima,
   omckk = param(11+(kk-1)*6-1:11+(kk-1)*6+2-1);
   Tckk = param(11+(kk-1)*6+3-1:11+(kk-1)*6+3+2-1);

   eval(['Xkk = X_' num2str(kk) ';']);
   eval(['xkk = x_' num2str(kk) ';']);

   ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c);

   Rckk = rodrigues(omckk);
   eval(['omc_' num2str(kk) '= omckk;']);
   eval(['Tc_' num2str(kk) '= Tckk;']);
   eval(['Rc_' num2str(kk) '= Rckk;']);

   e_cam = [e_cam ekk];

end;

X_proj = [];
x_proj = [];

for kk = 1:n_ima,
   eval(['xproj = xproj_' num2str(kk) ';']);
   xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c);
   eval(['Rc = Rc_' num2str(kk) ';']);
   eval(['Tc = Tc_' num2str(kk) ';']);
   Np_proj = size(xproj,2);
	Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)])));
	Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame
   eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the
   eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']);
   eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']);
end;

fp = param((1:2)+n_ima * 6 + 10-1);
cp = param((3:4)+n_ima * 6 + 10-1);
alpha_p = param((5)+n_ima * 6 + 10-1);
kp = [param((6-1:10-2)+n_ima * 6 + 10);0];

om = param(10+n_ima*6+10+1-2:10+n_ima*6+10+1+2-2);
T = param(10+n_ima*6+10+1+2+1-2:10+n_ima*6+10+1+2+1+2-2);

e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p);

e_global = [e_cam e_proj];


function DEM = ADEM_eyeblink(OPTION)
% Simulation of eyeblink conditioning
% FORMAT DEM = ADEM_eyeblink(OPTION)
%
%  OPTION:
%     case{'EYEBLINK'}   : spontaneous eye blinking
%     case{'AIRPUFF'}    : unconditioned eyeblink response to air puff
%     case{'STARTLE'}    : unconditioned  startle response to a sound
%     case{'TRACE'}      : trace conditioning to the sound
%     case{'DELAY'}      : delay conditioning to the sound
%     case{'EXTINCTION'} : extinction of trace conditioning to sound
%
%__________________________________________________________________________
%
% This demonstration routine illustrates Pavlovian learning under active
% inference. It uses the eyeblink conditioning paradigm to model startle
% responses and the subsequent acquisition of an eyeblink - under delay and
% trace learning. The various options above determine the nature of the
% simulation (or edit the OPTION below). The generative model, in this
% example, starts with a heteroclinic cycle with an inset. The cycle per se
% generates autonomous eyeblinks periodically, while the inset is
% activated by a conditioned stimulus (CS). The subsequent unstable fixed
% points play the role of an echo-state and enables the learning or
% association of a high-level hidden cause with subsequent unconditioned
% responses (UR).
%
% In active inference, an unconditioned response corresponds to a prior
% belief that a hidden state will generate action and the unconditioned
% stimulus (US). Pavlovian conditioning is the learning of the Association
% between a conditioned stimulus (CS) and the unconditioned response. The
% dynamics entailed by the heteroclinic cycle enable trace conditioning,
% which may be related to hippocampal function.
%
% In this example, there are two levels with the hidden states at the first
% level modelling beliefs about eyeblinks, unconditioned responses and
% unconditioned stimuli.  Proprioceptive predictions are generated by
% beliefs about ensuing eyeblinks and unconditioned responses (which
% also predict the conditioned stimulus. Hidden states at the second level
% embody a sense of time through Lotka-Volterra dynamics. Successive epochs
% of time are passed to the first level via a softmax transform. Learning
% corresponds to Hebbian plasticity (that minimises free energy) in the
% connections between the state unit encoding expectations about a UR and
% expectations about the CS (for delay conditioning) and heteroclinic
% states (for trace conditioning): see the functions at the end of this
% routine.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: ADEM_eyeblink.m 7679 2019-10-24 15:54:07Z spm $

% paradigm and stimuli
%==========================================================================

% options:
%--------------------------------------------------------------------------
if ~nargin, OPTION = 'STARTLE'; end

P.x    = 0;                                          % connection strengths
P.v    = [0 0 0];                                    % connection strengths
PC     = 0;                                          % prior covariance
V      = [8 4 0];                                    % sensory precision

switch OPTION

    case{'EYEBLINK'}  % spontaneous eye blinking
        N      = 256;                                % number of time bins
        NT     = 1;                                  % number of trials
        C(:,1) = sparse(1,N);                        % CS: loud sound
        C(:,2) = sparse(1,N);                        % US: air puff

    case{'AIRPUFF'}   % unconditional response to US
        N      = 256;                                % number of time bins
        NT     = 1;                                  % number of trials
        CS     = 128;                                % onset of CS (bins)
        C(:,1) = sparse(1,N);                        % CS: loud sound
        C(:,2) = exp(-((1:N) - CS).^2/(2*4^2));      % US: air puff

    case{'STARTLE'}   % startle response to CS
        N      = 256;                                % number of time bins
        NT     = 1;                                  % number of trials
        CS     = 128;                                % onset of CS (bins)
        C(:,1) = exp(-((1:N) - CS).^2/(2*4^2));      % CS: loud sound
        C(:,2) = sparse(1,N);                        % US: air puff

    case{'DELAY'}     % delay learning of association
        N      = 64;                                 % number of time bins
        NT     = 16;                                 % number of trials
        CS     = 48;                                 % onset of CS (bins)
        C(:,1) = exp(-((1:N) - CS).^2/(2*8^2));      % CS: loud sound
        C(:,2) = exp(-((1:N) - CS - 4).^2/(2*4^2));  % US: air puff
        PC     = 1;                                  % enable learning

    case{'TRACE'}     % trace learning of association (over time)
        N      = 128;                                % number of time bins
        NT     = 32;                                 % number of trials
        CS     = 48;                                 % onset of CS (bins)
        C(:,1) = exp(-((1:N) - CS).^2/(2*4^2));      % CS: loud sound
        C(:,2) = exp(-((1:N) - CS - 32).^2/(2*4^2)); % US: air puff
        PC     = 1;                                  % enable learning

    case{'EXTINCTION'} % normalextinction
        N      = 128;                                % number of time bins
        NT     = 8;                                  % number of trials
        CS     = 48;                                 % onset of CS (bins)
        C(:,1) = exp(-((1:N) - CS).^2/(2*4^2));      % CS: loud sound
        C(:,2) = sparse(1,N);                        % US: air puff
        P.x    = 0;                                  % connection strengths
        P.v    = [0 0 0.6];                          % connection strengths
        PC     = 1;                                  % enable learning
        V      = [8 0 0];                            % pirotoxin lesion
        V      = [8 2 0];                            % sensory precision

    otherwise
end

% generative process
%==========================================================================
M(1).E.n  = 2;
M(1).E.d  = 2;
M(1).E.s  = 1;
M(1).E.nN = 2;
M(1).E.nE = 1;

% level 1
%--------------------------------------------------------------------------
G(1).g  = @(x,v,a,P) G1g(x,v,a,P);
G(1).V  = exp(16);                                % error precision
G(1).U  = exp(4);                                 % motor gain

% level 2
%--------------------------------------------------------------------------
G(2).v  = [0;0];                                  % stimulus (CS and US)
G(2).a  = 0;                                      % action
G(2).V  = exp(16);

% generative model
%==========================================================================

% positions associated with each state (on unit circle)
%--------------------------------------------------------------------------
x.CS    = 0;
x.UR    = 0;
x.EB    = 0;

% level 1
%--------------------------------------------------------------------------
M(1).x  = x;
M(1).f  = @(x,v,P) M1f(x,v,P);
M(1).g  = @(x,v,P) M1g(x,v,P);
M(1).pE = P;
M(1).pC = PC;
M(1).W  = diag(exp([4 0 4]));                     % cerebellar (IPN) lesion
M(1).W  = exp(4);                                 % error precision
M(1).V  = exp(V);                                 % sensory attenuation

% for use with spm_ALAP (for state-dependent sensory attenuation)
%--------------------------------------------------------------------------
M(1).E.method.x = 0;
M(1).ph = @(x,v,h,M) [8 4 4] - 2*(x.UR + x.EB);   % sensory attenuation

% level 2
%--------------------------------------------------------------------------
P = [
    +1   -0.5  0.2  0.2  0.2  0.2;
    +0.5  1   -0.5  0    0    0  ;
    -0.5  0.5  1   -0.5  0    0  ;
    -0.5  0    0.5  1    0    0.5;
    -0.5  0    0    0.5  1   -0.5;
    -0.5  0    0   -0.5  0.5  1  ] - 1;

M(2).x  = [0 0 0 4 0 0]' - 4;
M(2).f  = @(x,v,P) spm_lotka_volterra(x,v,P);
M(2).g  = @(x,v,P) spm_softmax(x);
M(2).pE = P;
M(2).V  = exp(0);                                 % hippocampal lesion
M(2).V  = exp(4);                                 % error precision
M(2).W  = exp(4);                                 % error precision

% level 3
%--------------------------------------------------------------------------
M(3).v  = 0;                                      % inputs (null)
M(3).V  = exp(16);

% ADEM
%==========================================================================
DEM.U  = sparse(N,1);
DEM.C  = C;
DEM.G  = G;
DEM.M  = M;
DEM.db = 0;
for i  = 1:NT

    % integrate active inference scheme
    %----------------------------------------------------------------------
    DEM  = spm_ADEM(DEM);

    if i == 1
        spm_figure('GetWin','Before learning');
    else
        spm_figure('GetWin','After learning');
    end
    spm_DEM_qU(DEM.qU,DEM.pU);

    if NT == 1, break, end

    % latency and vigour of CR
    %----------------------------------------------------------------------
    a       = DEM.qU.a{2};
    a       = abs(a(1:min(end,(CS + 32))));
    if max(a) > 1/4;
        q = sum((1:length(a)).*a/sum(a));
    else
        q = NaN;
    end

    CR(1,i) = q;
    CR(2,i) = max(a);

    % Baysian belief update
    %----------------------------------------------------------------------
    p           = DEM.qP.P{1};
    DEM.M(1).pE = p;
    qP(:,i)     = spm_vec(p);

    % plot
    %----------------------------------------------------------------------
    spm_figure('GetWin','Figure 1'); clf

    subplot(2,2,1),cla
    plot((1:i),CR(1,:) - CR(1,1)), hold on
    plot((1:i),qP',':b'),          hold on
    plot((1:i),CR(2,:),'--'),      hold off
    set(gca,'XLim',[0 (NT + 1)])
    title('latency  and vigour','FontSize',16);
    xlabel('trials')
    ylabel('response')
    axis square

    subplot(2,2,2),cla
    imagesc(qP)
    title('plasticity','FontSize',16);
    set(gca,'XLim',[0.5 (NT + .5)])
    xlabel('trials')
    ylabel('connection')
    axis square

end

% functions of generative process and model (at the first level)
%==========================================================================
function s = G1g(x,v,a,P)                        % sensory process
s.CS = v(1);                                     % CS: amplitude sound
s.US = v(2);                                     % US: air puff
s.R  = a;                                        % proprioception

function s = M1g(x,v,P)                          % sensory prediction
s.CS = x.CS;                                     % prediction of CS
s.US = x.UR;                                     % prediction of US
s.R  = x.UR + x.EB;                              % prediction of UR

function f = M1f(x,v,P)                          % dynamics of states
f.CS = v(1) - x.CS;                              % dynamics of CS
f.UR = P.v*v(1:3) + P.x*x.CS - x.UR;             % dynamics of UR construct
f.EB = v(6) - x.EB;                              % dynamics of eyeblinks


% KNT_DERHAM: construct the knot vectors for discrete B-spline spaces in the De Rham diagram.
%
%  USAGE:
%
%     [knots, degree] = knt_derham (knots_h1, degree_h1, output_space)
%
% INPUTS:
%
%     knots_h1:     knot vectors for the H^1-conforming space, cell array of size (1 x ndim)
%     degree_h1:    degree for the H^1 space, vector of size (1 x ndim)
%     output_space: string, with one of the following: 'H1', 'Hcurl', 'Hdiv', 'L2'
%
% OUTPUT
%
%    The output are the knot vectors and the degrees for the chosen output space
%    Their size depend on the space:
%
% For H^1 and L^2 (scalar spaces)
%
%     NAME               TYPE          SIZE       DESCRIPTION
%     knots              cell-array  (1 x ndim)
%     degree             vector      (1 x ndim)
%
% For H(curl) and H(div) (vectorial spaces) each component of the
% cell-array contains the same information for one component of the space.
% That is, the knot vector is a cell-array of cell-arrays.
%
%     NAME               TYPE          SIZE
%     knots              cell-array  (1 x ndim)
%     degree             cell-array  (1 x ndim)
%
% Copyright (C) 2010, 2015 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 [knots, degree] = knt_derham (knots_h1, degree_h1, output_space)

  if (nargin < 3)
    output_space = 'Hcurl';
  end

  if (nargout > 2)
    error (['Wrong number of output parameters. The use of knt_derham has changed since version 2.1.' ...
    'Please read the help, or the examples in GeoPDEs, to understand its usage'])
  end

  if (~iscell (knots_h1))
    knots_h1 = {knots_h1};
  end

  ndim = numel (knots_h1);
  if (ndim ~= numel (degree_h1))
    error ('knt_derham: wrong input parameters. The dimension of the knot vector and the degree do not match')
  end

  for ii = 1:ndim
    knots_l2{ii} = knots_h1{ii}(2:end-1);
  end

  if (strcmpi (output_space, 'H1'))
    knots = knots_h1;
    degree = degree_h1;
  elseif (strcmpi (output_space, 'L2'))
    knots = knots_l2;
    degree = degree_h1 - 1;
  elseif (strcmpi (output_space, 'Hcurl'))
    if (ndim < 2 || ndim > 3)
      error ('knt_derham: Wrong dimension to compute the H(curl) space')
    end
    knots = cell (ndim, 1);
    degree = cell (ndim, 1);
    for idim = 1:ndim
      knots{idim} = knots_h1;
      degree{idim} = degree_h1;
      knots{idim}{idim} = knots_l2{idim};
      degree{idim}(idim) = degree_h1(idim) - 1;
    end
  elseif (strcmpi (output_space, 'Hdiv'))
    if (ndim < 2 || ndim > 3)
      error ('knt_derham: Wrong dimension to compute the H(div) space')
    end
    knots = cell (ndim, 1);
    degree = cell (ndim, 1);
    for idim = 1:ndim
      knots{idim} = knots_l2;
      degree{idim} = degree_h1 - 1;
      knots{idim}{idim} = knots_h1{idim};
      degree{idim}(idim) = degree_h1(idim);
    end
  end

%   if (numel (knots_h1) == 2)
%     knots_u1 = {knots_l2{1} knots_h1{2}};
%     knots_u2 = {knots_h1{1} knots_l2{2}};
%
%     if (nargout == 2)
%       varargout = {knots_u1 knots_u2};
%     elseif (nargout == 4 && nargin == 2)
%       degree1 = [degree(1) - 1, degree(2)];
%       degree2 = [degree(1), degree(2) - 1];
%       varargout = {knots_u1 knots_u2 degree1 degree2};
%     elseif (nargout == 3 && nargin == 1)
%       varargout = {knots_u1 knots_u2 knots_l2};
%     elseif (nargout == 6 && nargin == 2)
%       degree1 = [degree(1) - 1, degree(2)];
%       degree2 = [degree(1), degree(2) - 1];
%       degree_l2 = [degree(1) - 1, degree(2) - 1];
%       varargout = {knots_u1 knots_u2 knots_l2 degree1 degree2 degree_l2};
%     else
%       error ('knt_derham: wrong number of input or output parameters');
%     end
%
%   elseif (numel (knots_h1) == 3)
%     knots_u1 = {knots_l2{1} knots_h1{2} knots_h1{3}};
%     knots_u2 = {knots_h1{1} knots_l2{2} knots_h1{3}};
%     knots_u3 = {knots_h1{1} knots_h1{2} knots_l2{3}};
%
%     if (nargout == 3)
%       varargout = {knots_u1 knots_u2 knots_u3};
%     elseif (nargout == 6 && nargin == 2)
%       degree1 = [degree(1) - 1, degree(2), degree(3)];
%       degree2 = [degree(1), degree(2) - 1, degree(3)];
%       degree3 = [degree(1), degree(2), degree(3) - 1];
%       varargout = {knots_u1 knots_u2 knots_u3 degree1 degree2 degree3};
%     else
%       error ('knt_derham: wrong number of input or output parameters');
%     end
%   end
%
end


function model = gpsimMapUpdateG(model)

% GPSIMMAPUPDATEG Update the nonlinear transformation of f.
% FORMAT
% DESC updates the fields of model associated with the non-linear
% transformation of f, namely g, g_grad and g_grad2.
% ARG model : the model to be updated.
% ARG model : the model with the updated g representation.
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% MODIFIED : Pei Gao, 2008
%
% SEEALSO : gpsimMapFunctionalExpandParam, gpsimMapCreate

% SHEFFIELDML

% Remove mean function value from m (if mean function present).
if isfield(model, 'meanFunction') & ~isempty(model.meanFunction)
  f = model.f + modelOut(model.meanFunction, model.mapt);
else
  f = model.f;
end

% make sure f is a column vector
f = reshape(f, length(f), 1);

if model.ngParam == 0

switch model.nonLinearity
   case 'linear'
    model.g=f;        %linear case
    model.g_grad=ones(size(model.g));
    model.g_grad2=zeros(size(model.g));
    model.g_grad3=zeros(size(model.g));
    model.isConcave = true; % Is the log likelihood concave?
   case 'exp'
    model.g=exp(f);   %positive TF concentrations
%   model.gParam
%   if ~isempty(model.gParam)
%   model.ngParam = length(model.gParam(1,:));
%   model.dg = [];
%   model.dg2 = [];
%   end
    model.g_grad=model.g;
    model.g_grad2=model.g;
    model.g_grad3=model.g;
    model.isConcave = true;
   case 'quadratic'
    model.g=f.*f;
    model.g_grad=2*f;
    model.g_grad2=2*ones(size(f));
    model.isConcave = false;
   case 'negLogLogit'
    model.g=log(1+exp(f));   %positive TF concentrations
    model.g_grad=sigmoid(f);
    model.g_grad2=model.g_grad.*(1-model.g_grad);
    model.isConcave = true;
   case 'sigmoid'
    model.g = sigmoid(f);
    model.g_grad = model.g.*(1 - model.g);
    model.g_grad2 = model.g.*(1-model.g) - 2*model.g.*(model.g.*(1-model.g));
    model.isConcave = false;
   case 'repression'
    model.g = gpsimModelFunctions(model,'g');
    model.g_grad = gpsimModelFunctions(model,'grad');
    model.g_grad2 = gpsimModelFunctions(model,'grad2');
    model.g_grad3 = gpsimModelFunctions(model,'grad3');;
    model.isConcave = true; %% ??
   case 'activation'
    model.g = gpsimModelFunctions(model,'g');
    model.g_grad = gpsimModelFunctions(model,'grad');
    model.g_grad2 = gpsimModelFunctions(model,'grad2');
    model.g_grad3 = gpsimModelFunctions(model,'grad3');
    model.isConcave = true; %% ??
    if ~isempty(model.gParam)
      model.ngParam = length(model.gParam(1,:));
      model.dg = gpsimModelFunctions(model,'paramGrad');
      model.dg2 = gpsimModelFunctions(model,'paramGrad2');
    end
   otherwise
    error('Invalid non-linearity.')
end

else
  model.g = gpsimModelFunctions(model,'g');
  model.g_grad = gpsimModelFunctions(model,'grad');
  model.g_grad2 = gpsimModelFunctions(model,'grad2');
  model.g_grad3 = gpsimModelFunctions(model,'grad3');
  model.isConcave = true;
  model.dg = gpsimModelFunctions(model,'paramGrad');
  model.dggrad = gpsimModelFunctions(model,'paramGgrad');
  model.dggrad2 = gpsimModelFunctions(model,'paramGgrad2');
end


%% Grain Tutorial
%
%%
% The following script is a quick guide through the grain reconstruction
% capabilities of MTEX. It uses the same data set as in the corresponding
% publication
% <https://www.researchgate.net/publication/51806709_Grain_detection_from_2d_and_3d_EBSD_data-Specification_of_the_MTEX_algorithm
% Grain detection from 2d and 3d EBSD data>. Data courtesy of Daniel Rutte
% and Bret Hacker, Stanford.

mtexdata mylonite

% plot a phase map
plot(ebsd)

%%
% The phase map shows a multi-phase rock specimen with Andesina, Quartz,
% Biotite and Orthoclase. Lets restrict it to a smaller region of interest.
% The rectangle is defined by [xmin, ymin, xmax-xmin, ymax-ymin].

region = [19000 1500 4000 1500];
% overlay the selected region on the phase map
rectangle('position',region,'edgecolor','k','linewidth',2)

%%
% Now copy the EBSD data within the selected rectangle to a new variable

ebsd_region = ebsd(inpolygon(ebsd,region))

%% Grain Reconstruction
% Next we reconstruct the grains and grain boundaries in the region of
% interest, using a 15 degree orientation change threshold.

grains = calcGrains(ebsd_region,'angle',15*degree)

% plot a phase map of the region of interest
plot(ebsd_region)

% overlay the grain boundaries
hold on
plot(grains.boundary,'color','k','linewidth',1.5)
hold off

%%
% We may also visualize the different quarz orientations together with the
% grain boundaries.

% plot a phase map of three of the phases based on the grains data
plot(grains({'Andesina','Biotite','Orthoclase'}),'FaceAlpha',0.4)

hold on
% add the quarz orientations as ipf map based on EBSD data
plot(ebsd_region('Quartz'),ebsd_region('Quartz').orientations)

% plot grain boundaries so that those in the Quartz are shown
plot(grains.boundary,'color','black');
legend off
hold off

%%
% For the map created, most of the phases are coloured based on where they
% exist, while only the Quartz phase is colored according to the
% orientation. The quartz orientations are colured using the following ipf
% color key

close all
ipfKey = ipfColorKey(ebsd_region('Quartz'));
plot(ipfKey)

%%
% Alternatively, we may colorize each quarz grain according to its mean
% orientation.  Again, the other phases are colured based on where they
% exist.

plot(grains({'Andesina','Biotite','Orthoclase'}),'FaceAlpha',0.4)
hold on
plot(grains('Quartz'),grains('Quartz').meanOrientation)
legend off

%% Highlight specific boundaries
% We can create a phase map with certain grain boundaries highlighted.  In
% this case, we highlight where adjacent grains of Andesina and Orthoclase
% have a misorientation with rotational axis close to the c-axis.

close all
% copy all boundaries between Andesina Orthoclase to a new variable
AOboundary = grains.boundary('Andesina','Orthoclase');
% copy the misorientation angle of this boundary in radians to a new variable.
angle = AOboundary.misorientation.angle;

plot(grains,'FaceAlpha',0.4)
hold on
% highlight boundaries where the angle between the Andesina and Orthoclase phase is over 160 degrees
plot(AOboundary(angle>160*degree),'linewidth',2,'linecolor','red')
hold off

%%
% We can also represent the angular misorientation data between these two
% phases as a histogram.

figure;histogram(angle./degree)
xlabel('angle in degrees of boundary segment')
ylabel('count of boundary segments')
title('angular relationships between Andesina and Orthoclase')


function det = sgbdi ( abd, lda, n, ml, mu, ipvt )

%*****************************************************************************80
%
%% SGBDI computes the determinant of a band matrix factored by SGBCO or SGBFA.
%
%  Discussion:
%
%    If the inverse is needed, use SGBSL N times.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real ABD(LDA,N), the output from SGBCO or SGBFA.
%
%    Input, integer LDA, the leading dimension of the array ABD.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer ML, MU, the number of diagonals below and above the
%    main diagonal.  0 <= ML < N, 0 <= MU < N.
%
%    Input, integer IPVT(N), the pivot vector from SGBCO or SGBFA.
%
%    Output, real DET(2), the determinant of the original matrix.
%      determinant = DET(1) * 10.0**DET(2)
%    with  1.0 <= abs ( DET(1) ) < 10.0 or DET(1) = 0.0.
%
  ten = 10.0;

  m = ml + mu + 1;
  det(1) = 1.0;
  det(2) = 0.0;

  for i = 1 : n

    if ( ipvt(i) ~= i )
      det(1) = -det(1);
    end

    det(1) = abd(m,i) * det(1);

    if ( det(1) == 0.0 )
      return
    end

    while ( abs ( det(1) ) < 1.0 )
      det(1) = ten * det(1);
      det(2) = det(2) - 1.0;
    end

    while ( ten <= abs ( det(1) ) )
      det(1) = det(1) / ten;
      det(2) = det(2) + 1.0;
    end

  end

  return
end


function best_weights = GetBestMixingWeightForHarmonics(standard_deviation_list)
% Best mixing of instantaneous frequencies with common mean
% and built-in constraint (safeguard version designed for harmonics information mixing)
%
% best_weights = GetBestMixingWeightForHarmonics(sdListOrg)
%
% Input argument
%   standard_deviation_list    : standard deviation of each variable
%
% Output argument
%   best_weights   : best set of weights for mixing random variables
%
% Note
%   This mixing consists of heuristics. Re-design is needed.

% Copyright 2016 Google Inc. All Rights Reserved.
% Author: hidekik@google.com (Hideki Kawahara)

original_list_length = length(standard_deviation_list);
safe_list = (1:original_list_length)';
safe_list = safe_list(standard_deviation_list > 0 & ...
  standard_deviation_list < 100 * min(abs(standard_deviation_list)));
sdLsafe_SD_list = standard_deviation_list(safe_list);
n_of_safe_SD = length(sdLsafe_SD_list);
if n_of_safe_SD > 0
  H = ones(n_of_safe_SD-1, n_of_safe_SD-1) * sdLsafe_SD_list(n_of_safe_SD) ^ 2;
  H = H + diag(sdLsafe_SD_list(1:n_of_safe_SD - 1) .^ 2);
  v = ones(n_of_safe_SD - 1, 1) * sdLsafe_SD_list(n_of_safe_SD) ^ 2;
  a = H \ v; % revised 23/May/2016 from inv(H) * v
  if sum(a) > 1
    a = a / sum(a);
  end;
  w = [a; 1 - sum(a)];
  best_weights = zeros(original_list_length, 1);
  best_weights(safe_list) = w;
else
  best_weights = zeros(original_list_length, 1);
  best_weights(1) = 1;
end;
end


function [dist,proj] = ref_3d_projection(ref,EAST,NORTH,h)

% SYNTAX:
%   [dist,proj] = ref_3d_projection(ref,EAST,NORTH,h)
%
% INPUT:
%   ref = reference path (X,Y,Z coordinates of the vertices)
%   EAST = estimated trajectory in UTM coordinates (EAST)
%   NORTH = estimated trajectory in UTM coordinates (NORTH)
%   h = ellipsoid height
%
% OUTPUT:
%   dist = 3D distance of each estimated point from the reference
%   proj = projected trajectory
%
% DESCRIPTION:
%   3D projection on a reference path.
%   At the moment working only for adjacency matrix as in the following
%   example:
%
%   adj_mat = [ 0 1 0 0 0 1
%               1 0 1 0 0 0
%               0 1 0 1 0 0
%               0 0 1 0 1 0
%               0 0 0 1 0 1
%               1 0 0 0 1 0 ];

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   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/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

%close the path by connecting the first and the last points
ref = [ref;ref(1,:)];

%computation of the angular coefficients
ax = ref(2:end,1) - ref(1:end-1,1);
ay = ref(2:end,2) - ref(1:end-1,2);
az = ref(2:end,3) - ref(1:end-1,3);

%normalization on the segment distance
ad = sqrt(ax.^2 + ay.^2 + az.^2);
ax = ax ./ ad;
ay = ay ./ ad;
az = az ./ ad;

%offset of the curvilinear coordinate
s0 = [0; cumsum(ad)];

for j = 1 : length(EAST)
    pos_R(1) = EAST(j);
    pos_R(2) = NORTH(j);
    pos_R(3) = h(j);

    d0 = sqrt((pos_R(1) - ref(:,1)).^2 + ...
              (pos_R(2) - ref(:,2)).^2 + ...
              (pos_R(3) - ref(:,3)).^2);

    [dmin0 i0] = min(d0);

    %projection on the reference path
    bx = pos_R(1) - ref(1:end-1,1) + ax.*s0(1:end-1);
    by = pos_R(2) - ref(1:end-1,2) + ay.*s0(1:end-1);
    bz = pos_R(3) - ref(1:end-1,3) + az.*s0(1:end-1);

    s_R = (ax.*bx + ay.*by + az.*bz) ./ (ax.^2 + ay.^2 + az.^2);

    pos_R_proj(:,1) = ref(1:end-1,1) + ax .* (s_R - s0(1:end-1));
    pos_R_proj(:,2) = ref(1:end-1,2) + ay .* (s_R - s0(1:end-1));
    pos_R_proj(:,3) = ref(1:end-1,3) + az .* (s_R - s0(1:end-1));

    %computation of the minimum distance
    d = sqrt((pos_R(1) - pos_R_proj(:,1)).^2 + ...
        (pos_R(2) - pos_R_proj(:,2)).^2 + ...
        (pos_R(3) - pos_R_proj(:,3)).^2);

    [dmin i] = min(d);

    %position in cartesian coordinates
    while (dmin < dmin0) & ((pos_R_proj(i,1) < min(ref(i,1),ref(i+1,1))) | (pos_R_proj(i,1) > max(ref(i,1),ref(i+1,1))) | ...
            (pos_R_proj(i,2) < min(ref(i,2),ref(i+1,2))) | (pos_R_proj(i,2) > max(ref(i,2),ref(i+1,2))) | ...
            (pos_R_proj(i,3) < min(ref(i,3),ref(i+1,3))) | (pos_R_proj(i,3) > max(ref(i,3),ref(i+1,3))))

        d(i) = 9e99;
        [dmin i] = min(d);

    end

    if dmin0 < dmin
        dist(j,1) = dmin0;

        proj(j,1) = ref(i0,1);
        proj(j,2) = ref(i0,2);
        proj(j,3) = ref(i0,3);

    else
        dist(j,1) = dmin;

        proj(j,1) = pos_R_proj(i,1);
        proj(j,2) = pos_R_proj(i,2);
        proj(j,3) = pos_R_proj(i,3);
    end
end


function outpoints = mni2tal(inpoints)
% Converts coordinates from MNI brain to best guess
% for equivalent Talairach coordinates
% FORMAT outpoints = mni2tal(inpoints)
% Where inpoints is N by 3 or 3 by N matrix of coordinates
%  (N being the number of points)
% outpoints is the coordinate matrix with Talairach points
% Matthew Brett 10/8/99

dimdim = find(size(inpoints) == 3);
if isempty(dimdim)
  ft_error('input must be a N by 3 or 3 by N matrix')
end
if dimdim == 2
  inpoints = inpoints';
end

% Transformation matrices, different zooms above/below AC
upT = [
    0.9900    0.0000    0.0000    0.0000
    0.0000    0.9688    0.0460    0.0000
    0.0000   -0.0485    0.9189    0.0000
    0.0000    0.0000    0.0000    1.0000
      ];

downT = [
    0.9900    0.0000    0.0000    0.0000
    0.0000    0.9688    0.0420    0.0000
    0.0000   -0.0485    0.8390    0.0000
    0.0000    0.0000    0.0000    1.0000
    ];

tmp = inpoints(3,:)<0;  % 1 if below AC
inpoints = [inpoints; ones(1, size(inpoints, 2))];
inpoints(:, tmp) = downT * inpoints(:, tmp);
inpoints(:, ~tmp) = upT * inpoints(:, ~tmp);
outpoints = inpoints(1:3, :);
if dimdim == 2
  outpoints = outpoints';
end


function  dC=dervar3(C,n)

%DERVAR3 for core rotations
%
%function dC=dervar3(C,n)
%
%This function determines the derivative of the nth mode of
%the core rotation expression for the 3-way case w.r.t.
%maximization of the variance of the core
%
%This version has been optimized for speed, see below for a
%more easy to read scheme.

% Copyright (C) 1995-2006  Rasmus Bro & Claus Andersson
% Copenhagen University, DK-1958 Frederiksberg, Denmark, rb@life.ku.dk
%
% 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.

% $ Version 2.00 $ May 2001 $ Changed to array notation $ RB $ Not compiled $

W = size(C);
C = reshape(C,W(1),prod(W(2:end)));

mc = mean(mean(C.^2));
E = (C.^2 - mc).*C;
dC = zeros(W(n),W(n));
W1 = W(1);
W2 = W(2);
W3 = W(3);

if n==1,
   for a=1:W1,
      for b=1:W1,

         dc=0;
         for j=1:W3,
            tmp_0 = W2*(j-1);
            idxja = [1:W2] + tmp_0;
            dc = dc + sum(C(b,idxja).*E(a,idxja));
         end;

         dC(a,b) = dc;
      end;
   end;
end;

if n==2,
   for a=1:W2,
      for b=1:W2,

         dc=0;
         for j=1:W3,
            tmp_1 = W2*(j-1);
            idxja = a + tmp_1;
            idxjb = b + tmp_1;
            dc = dc + sum(C(:,idxjb).*E(:,idxja));
         end;

         dC(a,b) = dc;
      end;
   end;
end;

if n==3,
   for a=1:W3,
      tmp_2 = W2*(a-1);
      for b=1:W3,
         tmp_3 = W2*(b-1);

         dc=0;
         for j=1:W2,
            idxja = j + tmp_2;
            idxjb = j + tmp_3;
            dc = dc + sum(C(:,idxjb).*E(:,idxja));
         end;

         dC(a,b) = dc;
      end;
   end;
end;

%----------------------------------------------------------------------------------------
%function  dC=dervar3(C,W,n)
%
%%function dC=dervar3(C,W,n)
%
%%This function determines the derivative of the nth mode of
%%the core rotation expression for the 3-way case w.r.t.
%%maximization of the variance of the core
%
%mc=mean(mean(C.^2));
%dC=zeros(W(n),W(n));
%
%if n==1,
%   for a=1:W(1),
%      for b=1:W(1),
%
%         dc=0;
%         for i=1:W(2),
%            for j=1:W(3),
%               [idxia idxja]=getindxn(W,[a i j]);
%               [idxib idxjb]=getindxn(W,[b i j]);
%               dc = dc + (C(idxia,idxja)^2 - mc)*C(idxib,idxjb)*C(idxia,idxja);
%            end;
%         end;
%
%         dC(a,b) = dc;
%      end;
%   end;
%end;
%
%if n==2,
%   for a=1:W(2),
%      for b=1:W(2),
%
%         dc=0;
%         for i=1:W(1),
%            for j=1:W(3),
%               [idxia idxja]=getindxn(W,[i a j]);
%               [idxib idxjb]=getindxn(W,[i b j]);
%               dc = dc + (C(idxia,idxja)^2 - mc)*C(idxib,idxjb)*C(idxia,idxja);
%            end;
%         end;
%
%         dC(a,b) = dc;
%      end;
%   end;
%end;
%
%if n==3,
%   for a=1:W(3),
%      for b=1:W(3),
%
%         dc=0;
%         for i=1:W(1),
%            for j=1:W(2),
%               [idxia idxja]=getindxn(W,[i j a]);
%               [idxib idxjb]=getindxn(W,[i j b]);
%               dc = dc + (C(idxia,idxja)^2 - mc)*C(idxib,idxjb)*C(idxia,idxja);
%            end;
%         end;
%
%         dC(a,b) = dc;
%      end;
%   end;
%end;


function Matdetec2 = findMatdetecFootprint(DetectFootPrint,XPsizeOut,YPsizeOut)
% Fixed the calculation of footprint of Sentinel-2 detector based on old
% version of Sentinel-2 data (17/12/2021 Shi)

clear Matdetec k
% sorted foot print from 1 to 4
[DetectFootPrint.Ndect, idnews] = sort(DetectFootPrint.Ndect);
DetectFootPrint.Nband = DetectFootPrint.Nband(idnews);
DetectFootPrint.Data = DetectFootPrint.Data(idnews);

for i = 1:length(DetectFootPrint.Ndect)

  IDX = knnsearch(XPsizeOut',DetectFootPrint.Data{i,1}(:,1));
  IDY = knnsearch(YPsizeOut,DetectFootPrint.Data{i,1}(:,2));

  dum2 = single(poly2mask(double(IDX), double(IDY),length(XPsizeOut),length(XPsizeOut))) ;
  clear IDX IDY;
  dum2 = conv2(dum2,ones(3),'same')>0; % to fill boundary
  Matdetec(:,:,i) = dum2;
  clear dum*

  %   find orientation of detect + slope for computing perpendicular kernel
  I=nan(size(Matdetec,2),1);
  for ii=1:size(Matdetec,1)
    dum=find(Matdetec(ii,:,i)==1,1,'first');
    if ~isempty(dum)
      I(ii,1)=dum;
    end
  end
  clear dum;
  J = [1:size(Matdetec,1)]' ;
  test = ~isnan(I) & I > 1 & I < size(Matdetec,2);
  warning off all % if warning => not enough point => slope=0 => good because tile boundary
  k{i,1} = polyfit(J(test),I(test),1);
  clear test;

  I=nan(size(Matdetec,2),1);
  for ii=1:size(Matdetec,1)
    dum=find(Matdetec(ii,:,i)==1,1,'last');
    if ~isempty(dum)
      I(ii,1)=dum;
    end
  end
  J = [1:size(Matdetec,1)]' ;
  test = ~isnan(I) & I > 1 & I < size(Matdetec,2);
  k{i,2} = polyfit(J(test),I(test),1);
  clear test;
  warning on all

end

% mediane
for i = 1:length(DetectFootPrint.Ndect)-1
  mediane = mean( [k{i,2} ; k{i+1,1}] ) ;

  k{i,2} = mediane ;
  k{i+1,1} = mediane ;
  clear mediane;
end
J = [1:size(Matdetec,1)]' ;
I = [1:size(Matdetec,2)] ;

[Jmat Imat] = meshgrid(I,J);
clear I J;

Matdetec2 = nan(size(Matdetec,1),size(Matdetec,2));
clear Matdetec;
for i = 1:length(DetectFootPrint.Ndect)

  liminf = polyval(k{i,1},Jmat);
  limsup = polyval(k{i,2},Jmat);

  if sum(k{i,2}) == 0 % footprint at low-right corner
      Matdetec2(Imat>=liminf) = DetectFootPrint.Ndect(i) ;
  elseif sum(k{i,1}) == 0 % footprint at up-left corner
     Matdetec2(Imat<=limsup) = DetectFootPrint.Ndect(i) ;
  else
     Matdetec2(Imat>=liminf & Imat<=limsup) = DetectFootPrint.Ndect(i) ;
  end
  clear liminf limsup;
end
clear Imat ImatJ k;

Matdetec2 = Matdetec2';


%% DEMO_febio_0005_cube_tension_compression_shear
% Below is a demonstration for:
%
% * Building geometry for a cube with hexahedral elements
% * Defining the boundary conditions
% * Coding the febio structure
% * Running the model
% * Importing and visualizing the displacement and stress results

%% Keywords
%
% * febio_spec version 3.0
% * febio, FEBio
% * compression, tension, compressive, tensile, shear
% * displacement control, displacement boundary condition
% * hexahedral elements, hex8
% * cube, box, rectangular
% * static, solid
% * hyperelastic, Ogden
% * displacement logfile
% * stress logfile

%%

clear; close all; clc;

%% Plot settings
fontSize=20;
faceAlpha1=0.8;
markerSize=40;
markerSize2=20;
lineWidth=3;

%% Control parameters

% Path names
defaultFolder = fileparts(fileparts(mfilename('fullpath')));
savePath=fullfile(defaultFolder,'data','temp');

% 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_stress=[febioFebFileNamePart,'_stress_out.txt']; %Log file name for exporting stress

%Specifying dimensions and number of elements
cubeSize=10;
sampleWidth=cubeSize; %Width
sampleThickness=cubeSize; %Thickness
sampleHeight=cubeSize; %Height
pointSpacings=2*ones(1,3); %Desired point spacing between nodes
numElementsWidth=round(sampleWidth/pointSpacings(1)); %Number of elemens in dir 1
numElementsThickness=round(sampleThickness/pointSpacings(2)); %Number of elemens in dir 2
numElementsHeight=round(sampleHeight/pointSpacings(3)); %Number of elemens in dir 3

%Define applied displacement
stretchLoad=1.3;
displacementMagnitude=(stretchLoad*sampleHeight)-sampleHeight; %The displacement magnitude

%Material parameter set
c1=1e-3; %Shear-modulus-like parameter
m1=8; %Material parameter setting degree of non-linearity
k_factor=1e2; %Bulk modulus factor
k=c1*k_factor; %Bulk modulus

% FEA control settings
numTimeSteps=10; %Number of time steps desired
max_refs=25; %Max reforms
max_ups=0; %Set to zero to use full-Newton iterations
opt_iter=6; %Optimum number of iterations
max_retries=5; %Maximum number of retires
dtmin=(1/numTimeSteps)/100; %Minimum time step size
dtmax=1/numTimeSteps; %Maximum time step size

%% Creating model geometry and mesh
% A box is created with tri-linear hexahedral (hex8) elements using the
% |hexMeshBox| function. The function offers the boundary faces with
% seperate labels for the top, bottom, left, right, front, and back sides.
% As such these can be used to define boundary conditions on the exterior.

% Create a box with hexahedral elements
cubeDimensions=[sampleWidth sampleThickness sampleHeight]; %Dimensions
cubeElementNumbers=[numElementsWidth numElementsThickness numElementsHeight]; %Number of elements
outputStructType=2; %A structure compatible with mesh view
[meshStruct]=hexMeshBox(cubeDimensions,cubeElementNumbers,outputStructType);

%Access elements, nodes, and faces from the structure
E=meshStruct.elements; %The elements
V=meshStruct.nodes; %The nodes (vertices)
Fb=meshStruct.facesBoundary; %The boundary faces
Cb=meshStruct.boundaryMarker; %The "colors" or labels for the boundary faces
elementMaterialIndices=ones(size(E,1),1); %Element material indices

%%
% Plotting model boundary surfaces and a cut view

hFig=cFigure;

subplot(1,2,1); hold on;
title('Model boundary surfaces and labels','FontSize',fontSize);
gpatch(Fb,V,Cb,'k',faceAlpha1);
colormap(gjet(6)); icolorbar;
axisGeom(gca,fontSize);

hs=subplot(1,2,2); hold on;
title('Cut view of solid mesh','FontSize',fontSize);
optionStruct.hFig=[hFig hs];
meshView(meshStruct,optionStruct);
axisGeom(gca,fontSize);

drawnow;

%% Defining the boundary conditions
% The visualization of the model boundary shows colors for each side of the
% cube. These labels can be used to define boundary conditions.

%Define supported node sets
logicFace=Cb==5; %Logic for current face set
Fr=Fb(logicFace,:); %The current face set
bcSupportList=unique(Fr(:)); %Node set part of selected face

%Prescribed displacement nodes
logicPrescribe=Cb==6; %Logic for current face set
Fr=Fb(logicPrescribe,:); %The current face set
bcPrescribeList=unique(Fr(:)); %Node set part of selected face

%%
% Visualizing boundary conditions. Markers plotted on the semi-transparent
% model denote the nodes in the various boundary condition lists.

hf=cFigure;
title('Boundary conditions','FontSize',fontSize);
xlabel('X','FontSize',fontSize); ylabel('Y','FontSize',fontSize); zlabel('Z','FontSize',fontSize);
hold on;

gpatch(Fb,V,'kw','k',0.5);

hl(1)=plotV(V(bcSupportList,:),'k.','MarkerSize',markerSize);
hl(2)=plotV(V(bcPrescribeList,:),'r.','MarkerSize',markerSize);

legend(hl,{'BC support','BC prescribe'});

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

%% 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';

%Create control structure for use by all steps
stepStruct.Control.time_steps=numTimeSteps;
stepStruct.Control.step_size=1/numTimeSteps;
stepStruct.Control.solver.max_refs=max_refs;
stepStruct.Control.solver.max_ups=max_ups;
stepStruct.Control.time_stepper.dtmin=dtmin;
stepStruct.Control.time_stepper.dtmax=dtmax;
stepStruct.Control.time_stepper.max_retries=max_retries;
stepStruct.Control.time_stepper.opt_iter=opt_iter;

%Add template based default settings to proposed control section
[stepStruct.Control]=structComplete(stepStruct.Control,febio_spec.Control,1); %Complement provided with default if missing

%Remove control field (part of template) since step specific control sections are used
febio_spec=rmfield(febio_spec,'Control');

febio_spec.Step.step{1}.Control=stepStruct.Control;
febio_spec.Step.step{1}.ATTR.id=1;
febio_spec.Step.step{2}.Control=stepStruct.Control;
febio_spec.Step.step{2}.ATTR.id=2;
febio_spec.Step.step{3}.Control=stepStruct.Control;
febio_spec.Step.step{3}.ATTR.id=3;
febio_spec.Step.step{4}.Control=stepStruct.Control;
febio_spec.Step.step{4}.ATTR.id=4;
febio_spec.Step.step{5}.Control=stepStruct.Control;
febio_spec.Step.step{5}.ATTR.id=5;

%Material section
materialName1='Material1';
febio_spec.Material.material{1}.ATTR.name=materialName1;
febio_spec.Material.material{1}.ATTR.type='Ogden';
febio_spec.Material.material{1}.ATTR.id=1;
febio_spec.Material.material{1}.c1=c1;
febio_spec.Material.material{1}.m1=m1;
febio_spec.Material.material{1}.c2=c1;
febio_spec.Material.material{1}.m2=-m1;
febio_spec.Material.material{1}.k=k;

% Mesh section
% -> Nodes
febio_spec.Mesh.Nodes{1}.ATTR.name='Object1'; %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='Part1';
febio_spec.Mesh.Elements{1}.ATTR.name=partName1; %Name of this part
febio_spec.Mesh.Elements{1}.ATTR.type='hex8'; %Element type
febio_spec.Mesh.Elements{1}.elem.ATTR.id=(1:1:size(E,1))'; %Element id's
febio_spec.Mesh.Elements{1}.elem.VAL=E; %The element matrix

% -> NodeSets
nodeSetName1='bcSupportList';
nodeSetName2='bcPrescribeList';

febio_spec.Mesh.NodeSet{1}.ATTR.name=nodeSetName1;
febio_spec.Mesh.NodeSet{1}.node.ATTR.id=bcSupportList(:);

febio_spec.Mesh.NodeSet{2}.ATTR.name=nodeSetName2;
febio_spec.Mesh.NodeSet{2}.node.ATTR.id=bcPrescribeList(:);

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

%Boundary condition section
%-> Fix boundary conditions
febio_spec.Boundary.bc{1}.ATTR.type='fix';
febio_spec.Boundary.bc{1}.ATTR.node_set=nodeSetName1;
febio_spec.Boundary.bc{1}.dofs='x,y,z';

%STEP 1 Tension
febio_spec.Step.step{1}.Boundary.bc{1}.ATTR.type='prescribe';
febio_spec.Step.step{1}.Boundary.bc{1}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{1}.Boundary.bc{1}.dof='z';
febio_spec.Step.step{1}.Boundary.bc{1}.scale.ATTR.lc=1;
febio_spec.Step.step{1}.Boundary.bc{1}.scale.VAL=displacementMagnitude;
febio_spec.Step.step{1}.Boundary.bc{1}.relative=1;

febio_spec.Step.step{1}.Boundary.bc{2}.ATTR.type='fix';
febio_spec.Step.step{1}.Boundary.bc{2}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{1}.Boundary.bc{2}.dofs='x,y';

%STEP 2 Return form tension
febio_spec.Step.step{2}.Boundary.bc{1}.ATTR.type='prescribe';
febio_spec.Step.step{2}.Boundary.bc{1}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{2}.Boundary.bc{1}.dof='z';
febio_spec.Step.step{2}.Boundary.bc{1}.scale.ATTR.lc=2;
febio_spec.Step.step{2}.Boundary.bc{1}.scale.VAL=-displacementMagnitude;
febio_spec.Step.step{2}.Boundary.bc{1}.relative=1;

febio_spec.Step.step{2}.Boundary.bc{2}.ATTR.type='fix';
febio_spec.Step.step{2}.Boundary.bc{2}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{2}.Boundary.bc{2}.dofs='x,y';

%STEP 3 Compression
febio_spec.Step.step{3}.Boundary.bc{1}.ATTR.type='prescribe';
febio_spec.Step.step{3}.Boundary.bc{1}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{3}.Boundary.bc{1}.dof='z';
febio_spec.Step.step{3}.Boundary.bc{1}.scale.ATTR.lc=3;
febio_spec.Step.step{3}.Boundary.bc{1}.scale.VAL=-displacementMagnitude;
febio_spec.Step.step{3}.Boundary.bc{1}.relative=1;

febio_spec.Step.step{3}.Boundary.bc{2}.ATTR.type='fix';
febio_spec.Step.step{3}.Boundary.bc{2}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{3}.Boundary.bc{2}.dofs='x,y';

%STEP 4 Return from compression
febio_spec.Step.step{4}.Boundary.bc{1}.ATTR.type='prescribe';
febio_spec.Step.step{4}.Boundary.bc{1}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{4}.Boundary.bc{1}.dof='z';
febio_spec.Step.step{4}.Boundary.bc{1}.scale.ATTR.lc=4;
febio_spec.Step.step{4}.Boundary.bc{1}.scale.VAL=displacementMagnitude;
febio_spec.Step.step{4}.Boundary.bc{1}.relative=1;

febio_spec.Step.step{4}.Boundary.bc{2}.ATTR.type='fix';
febio_spec.Step.step{4}.Boundary.bc{2}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{4}.Boundary.bc{2}.dofs='x,y';

%STEP 5 Shear
febio_spec.Step.step{5}.Boundary.bc{1}.ATTR.type='prescribe';
febio_spec.Step.step{5}.Boundary.bc{1}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{5}.Boundary.bc{1}.dof='x';
febio_spec.Step.step{5}.Boundary.bc{1}.scale.ATTR.lc=5;
febio_spec.Step.step{5}.Boundary.bc{1}.scale.VAL=displacementMagnitude;
febio_spec.Step.step{5}.Boundary.bc{1}.relative=1;

febio_spec.Step.step{5}.Boundary.bc{2}.ATTR.type='fix';
febio_spec.Step.step{5}.Boundary.bc{2}.ATTR.node_set=nodeSetName2;
febio_spec.Step.step{5}.Boundary.bc{2}.dofs='y,z';

%LoadData section
% -> load_controller
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=[0 0; 1 1];

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=[1 0; 2 1];

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=[2 0; 3 1];

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=[3 0; 4 1];

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=[4 0; 5 1];

%Output section
% -> log file
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.element_data{1}.ATTR.file=febioLogFileName_stress;
febio_spec.Output.logfile.element_data{1}.ATTR.data='s1';
febio_spec.Output.logfile.element_data{1}.ATTR.delim=',';
febio_spec.Output.logfile.element_data{1}.VAL=1:size(E,1);

%% Quick viewing of the FEBio input file structure
% The |febView| function can be used to view the xml structure in a MATLAB
% figure window.

%%
% |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.runMode='external';%'internal';

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

%% Import FEBio results

if runFlag==1 %i.e. a succesful run

    %%
    % Importing nodal displacements from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_disp),1,1);

    %Access data
    N_disp_mat=dataStruct.data; %Displacement
    timeVec=dataStruct.time; %Time

    %Create deformed coordinate set
    V_DEF=N_disp_mat+repmat(V,[1 1 size(N_disp_mat,3)]);

    %%
    % Importing element stress from a log file
    dataStruct=importFEBio_logfile(fullfile(savePath,febioLogFileName_stress),1,1);

    %Access data
    E_stress_mat=dataStruct.data;

    %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    DN_magnitude=sqrt(sum(N_disp_mat(:,:,end).^2,2)); %Current displacement magnitude

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure
    gtitle([febioFebFileNamePart,': Press play to animate']);
    title('Displacement magnitude [mm]','Interpreter','Latex')
    hp=gpatch(Fb,V_DEF(:,:,end),DN_magnitude,'k',1); %Add graphics object to animate
    hp.Marker='.';
    hp.MarkerSize=markerSize2;
    hp.FaceColor='interp';
    gpatch(Fb,V,0.5*ones(1,3),'k',0.25); %A static graphics object

    axisGeom(gca,fontSize);
    colormap(gjet(250)); colorbar;
    caxis([0 max(DN_magnitude)]);
    axis(axisLim(V_DEF)); %Set axis limits statically
        camlight headlight;

    % Set up animation features
    animStruct.Time=timeVec; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments
        DN_magnitude=sqrt(sum(N_disp_mat(:,:,qt).^2,2)); %Current displacement magnitude

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp hp]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData'}; %Properties of objects to animate
        animStruct.Set{qt}={V_DEF(:,:,qt),DN_magnitude}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

        %%
    % Plotting the simulated results using |anim8| to visualize and animate
    % deformations

    [CV]=faceToVertexMeasure(E,V,E_stress_mat(:,:,end));

    % Create basic view and store graphics handle to initiate animation
    hf=cFigure; %Open figure
    gtitle([febioFebFileNamePart,': Press play to animate']);
    title('$\sigma_{1}$ [MPa]','Interpreter','Latex')
    hp=gpatch(Fb,V_DEF(:,:,end),CV,'k',1); %Add graphics object to animate
    hp.Marker='.';
    hp.MarkerSize=markerSize2;
    hp.FaceColor='interp';
    gpatch(Fb,V,0.5*ones(1,3),'k',0.25); %A static graphics object

    axisGeom(gca,fontSize);
    colormap(gjet(250)); colorbar;
%     caxis([min(E_stress_mat(:)) max(E_stress_mat(:))]);
    axis(axisLim(V_DEF)); %Set axis limits statically
    camlight headlight;

    % Set up animation features
    animStruct.Time=timeVec; %The time vector
    for qt=1:1:size(N_disp_mat,3) %Loop over time increments

        [CV]=faceToVertexMeasure(E,V,E_stress_mat(:,:,qt));

        %Set entries in animation structure
        animStruct.Handles{qt}=[hp hp]; %Handles of objects to animate
        animStruct.Props{qt}={'Vertices','CData'}; %Properties of objects to animate
        animStruct.Set{qt}={V_DEF(:,:,qt),CV}; %Property values for to set in order to animate
    end
    anim8(hf,animStruct); %Initiate animation feature
    drawnow;

end

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

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


%% faceToVertexMeasure
% Below is a demonstration of the features of the |patchPathAngles| function

%%
clear; close all; clc;

%% Syntax
% |[indAngles]=faceToVertexMeasure(F,V,ind,isClosedLoop);|

%% Description
% The |faceToVertexMeasure| function converts data for faces to data on
% vertices through averaging.

%% Examples

%%
% Plot settings
markerSize=150;

%% Example 1: Convert face data to vertex data
[F,V]=geoSphere(2,1);
VF=patchCentre(F,V);
CF=VF(:,1);
[CV]=faceToVertexMeasure(F,V,CF);

%%

cFigure;
subplot(1,2,1); hold on;
title('Color data on faces')
gpatch(F,V,CF);
scatterV(VF,markerSize,CF,'filled');
colormap gjet; colorbar;
axisGeom;
camlight headlight;

subplot(1,2,2); hold on;
title('Converted data on vertices')
hp=gpatch(F,V,CV);
hp.FaceColor='Interp';
scatterV(V,markerSize,CV,'filled');
colormap gjet; colorbar;
axisGeom;
camlight headlight;

drawnow;

%% Example 2: Convert face data on a mixed mesh to vertex data

%%
% Create an example of a mixed mesh consisting of triangles and
% quadrilateral faces.

% Create a triangular mesh
[F,V]=geoSphere(2,1);

% Converting to a quadrilateral mesh
optionStruct.maxAngleDeviation=45*(pi/180);
optionStruct.selectionMethod='best';
optionStruct.triangleConvert=0;
optionStruct.fourConnectConvert=0;
[F,V]=tri2quadGroupSplit(F,V,optionStruct);

VF=patchCentre(F,V);
CF=VF;
for q=1:1:numel(VF)
    CF{q}=VF{q}(:,1);
end
[CV]=faceToVertexMeasure(F,V,CF);

%%

cFigure;
subplot(1,2,1); hold on;
title('Color data on faces')
gpatch(F,V,CF);
for q=1:1:numel(VF)
    scatterV(VF{q},markerSize,CF{q},'filled');
end
colormap gjet; colorbar;
axisGeom;
camlight headlight;

subplot(1,2,2); hold on;
title('Converted data on vertices')
hp=gpatch(F,V,CV);
% hp.FaceColor='Interp';
scatterV(V,markerSize,CV,'filled');
colormap gjet; colorbar;
axisGeom;
camlight headlight;
drawnow;

%% Example 3: Convert multi-dimensional face data (e.g. on a mixed mesh) to vertex data

N=patchNormal(F,V); %get face normals
NV=faceToVertexMeasure(F,V,N); %Convert to vertex normals

%%

VF=patchCentre(F,V); %Get face centres for plotting

cFigure;
subplot(1,2,1); hold on;
title('Vector data on faces')
gpatch(F,V,CF);
for q=1:1:numel(F)
    quiverVec(VF{q},N{q},0.25,'k');
end
colormap gjet; colorbar;
axisGeom;
camlight headlight;

subplot(1,2,2); hold on;
title('Converted data on vertices')
gpatch(F,V,CV);
quiverVec(V,NV,0.25,'k');
colormap gjet; colorbar;
axisGeom;
camlight headlight;
drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

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


%LAMBDA2RGB RGB chromaticity coordinates
%
% RGB = LAMBDA2RG(LAMBDA) is the rg-chromaticity coordinate (1x2) for
% illumination at the specific wavelength LAMBDA [m]. If LAMBDA is a
% vector (Nx1), then P (Nx2) is a vector whose elements are the chromaticity
% coordinates at the corresponding elements of LAMBDA.
%
% RGB = LAMBDA2RG(LAMBDA, E) is the rg-chromaticity coordinate (1x2) for an
% illumination spectrum E (Nx1) defined at corresponding wavelengths
% LAMBDA (Nx1).
%
% References::
%  - Robotics, Vision & Control, Section 10.2,
%    P. Corke, Springer 2011.
%
% See also CMFRGB, LAMBDA2XY.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.
function [r,g] = lambda2rg(lambda, e)
    if nargin == 1,
        RGB = cmfrgb(lambda);
    elseif nargin == 2,
        RGB = cmfrgb(lambda, e);
    end
    cc = tristim2cc(RGB);

    if nargout == 1
        r = cc;
    elseif nargout == 2
        r = cc(:,1);
        g = cc(:,2);
    end


function [lf] = eeg_halfspace_medium_leadfield(rd, elc, vol)

% HALFSPACE_MEDIUM_LEADFIELD calculate the halfspace medium leadfield
% on positions pnt for a dipole at position rd and conductivity cond
% The halfspace solution requires a plane dividing a conductive zone of
% conductivity cond, from a non coductive zone (cond = 0)
%
% [lf] = halfspace_medium_leadfield(rd, elc, cond)

% Copyright (C) 2011, Cristiano Micheli and Robert Oostenveld
%
% This file is part of FieldTrip, see http://www.ru.nl/neuroimaging/fieldtrip
% 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: eeg_halfspace_medium_leadfield.m 2781 2011-02-03 10:48:53Z roboos $

siz = size(rd);
if any(siz==1)
  % positions are specified as a single vector
  Ndipoles = prod(siz)/3;
  rd = rd(:)'; % ensure that it is a row vector
elseif siz(2)==3
  % positions are specified as a Nx3 matrix -> reformat to a single vector
  Ndipoles = siz(1);
  rd = rd';
  rd = rd(:)'; % ensure that it is a row vector
else
  error('incorrect specification of dipole locations');
end

Nelc     = size(elc,1);
lf       = zeros(Nelc,3*Ndipoles);

for i=1:Ndipoles
  % this is the position of dipole "i"
  dip1 = rd((1:3) + 3*(i-1));

  % distances electrodes - dipole
  r1 = elc - ones(Nelc,1) * dip1;

  % Method of mirror dipoles:
  % Defines the position of mirror dipoles being symmetric to the plane
  dip2 = get_mirror_pos(dip1,vol);

  % distances electrodes - mirror dipole
  r2 = elc - ones(Nelc,1) * dip2;

  % denominator
  R1 =  (4*pi*vol.cond) * (sum(r1' .^2 ) .^ 1.5)';
  % denominator, mirror term
  R2 = -(4*pi*vol.cond) * (sum(r2' .^2 ) .^ 1.5)';

  % condition of dipoles falling in the non conductive halfspace
  invacuum = acos(dot(vol.ori,(dip1-vol.pnt)./norm(dip1-vol.pnt))) < pi/2;

  if invacuum
    warning('dipole lies on the vacuum side of the plane');
    lf(:,(1:3) + 3*(i-1)) = NaN(Nelc,3);
  elseif any(R1)==0
    warning('dipole coincides with one of the electrodes');
    lf(:,(1:3) + 3*(i-1)) = NaN(Nelc,3);
  else
    lf(:,(1:3) + 3*(i-1)) = (r1 ./ [R1 R1 R1]) + (r2 ./ [R2 R2 R2]);
  end
end


function g = orderedNoiseGradientParam(noise, mu, varsigma, y)

% ORDEREDNOISEGRADIENTPARAM Gradient of ORDERED noise's parameters.
% FORMAT
% DESC computes the gradient of the log Z of the ordered categorical noise model with respect to the of functions with respect to the
% ordered categorical
% noise's parameters.
% ARG noise : the noise structure for which the gradients are being
% computed.
% ARG mu : the input means for which the gradients are being computed.
% ARG varSigma : the input variances for which the gradients are being computed.
% ARG y : the target values for the noise model.
% RETURN g : gradients of the log Z with respect to
% the noise parameters. The ordering of the vector should match
% that provided by the function noiseExtractParam.
%
%
% SEEALSO orderedNoiseParamInit, orderednoiseGradVals, noiseGradientParam
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005

% NOISE

D = size(y, 2);
c = 1./sqrt(noise.variance + varsigma);
gnoise.bias = zeros(1, D);
gnoise.widths = zeros(noise.C-2, 1);
for j = 1:D
  % Do lowest category first
  index = find(y(:, j)==0);
  if ~isempty(index)
    mu(index, j) = mu(index, j) + noise.bias(j) ;
    mu(index, j) = mu(index, j).*c(index, j);
    gnoise.bias(j) = gnoise.bias(j) - sum(c(index, j).*gradLogCumGaussian(-mu(index, j)));
  end

  % Intermediate categories
  index = find(y(:, j)>0 & y(:, j) <noise.C-1);
  if ~isempty(index)
    for i = index'
      mu(i, j) = mu(i, j) + noise.bias(j) - sum(noise.widths(1:y(i, j)-1));
    end
    u = mu(index, j).*c(index, j);
    uprime = (mu(index, j) - noise.widths(y(index, j))).*c(index, j);
    B1 = gaussOverDiffCumGaussian(u, uprime, 1);
    B2 = gaussOverDiffCumGaussian(u, uprime, 2);
    gnoise.bias(j) = gnoise.bias(j) + sum(c(index, j).*(B1 - B2));
    for cat = 1:noise.C-2

      subIndex = find(y(index, j) == cat);
      if ~isempty(subIndex)
        addpart = sum(c(index(subIndex), j)...
                      .*B2(subIndex));
        gnoise.widths(1:cat) = gnoise.widths(1:cat) ...
            + repmat(addpart, cat, 1);
        if(cat > 1)
          addpart = sum(c(index(subIndex), j)...
                        .*B1(subIndex));
          gnoise.widths(1:cat-1) = gnoise.widths(1:cat-1) ...
              - repmat(addpart, cat-1, 1);
        end
      end
    end
  end

  % Highest category
  index = find(y(:, j) == noise.C-1);
  if ~isempty(index)
    for i = index'
      mu(i, j) = mu(i, j) + noise.bias(j) - sum(noise.widths(1:y(i, j)-1));
    end
    mu(index, j) = mu(index, j).*c(index, j);
    addpart = sum(c(index, j).*gradLogCumGaussian(mu(index, j)));
    gnoise.bias(j) = gnoise.bias(j) + addpart;
    if length(noise.widths > 0)
      gnoise.widths = gnoise.widths ...
          - repmat(addpart, noise.C-2, 1);
    end
  end
end
if length(noise.widths>0)
  g = [gnoise.bias gnoise.widths(:)'];
else
  g = gnoise.bias;
end