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


function textscatter(varargin)
% textscatter(cx,cy,markersize,markerstyle,textcolor, textoffsetx, textoffsety)
% Uses scatter plot and each marker is labeled by the number of the row in
% the [cx, cy] matrix. Text is offste by [textoffsetx, textoffsety] from
% the markers.
% Examples:
% textscatter(100*rand(1,30), 100*rand(1,30))
% textscatter(100*rand(1,30), 100*rand(1,30),[],'d')
% textscatter(100*rand(1,30), 100*rand(1,30),100,'gx','r', 2,3)

args = varargin;
if nargin<5 % textcolor
%     args{5} = [0 0 0]; % black
    args{5} = [1 0 0]; % red
end
if nargin<6 % textoffset
    args{6}= .5;
    args{7}= .5;
end

cx = args{1};
cy = args{2};
textcolor = args{5};
textoffsetx = args{6};
textoffsety = args{7};

scatter(args{1:min(nargin,4)})
text(cx+textoffsetx, cy+textoffsety, num2str([1:length(cx)]' ),'color' , textcolor)


function Y = nnsoft(X, T)
%% Soft threshold function
%% Copyright (c) 2017 Yan Yang
%% All rights reserved.
TH = abs(T);
 B = X >= TH;
 S = X <= -TH;
 Y = (X - TH) .* B + (X + TH) .* S;
end


function [projOrder, d, dRow, dVar] = getProjOrder(L)
%GETPROJORDER   Get projection order of a LINOP.
%   Each boundary and continuity constraint in a LINOP forces a reduction in the
%   total number of rows in the discrete operator, so that the composite is
%   square. The reduction is found by down-projection of the result of applying
%   the operator.
%
%   GETPROJORDER(DISC) returns a matrix of dimensions by which each column of
%   the system should be down-projected in order to end with a square system.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

d = L.diffOrder;
dRow = max(d, [], 2);
dVar = max(d, [], 1);
projOrder = max(dVar, 0);

end


function sphere_triangle_quad_test ( )

%*****************************************************************************80
%
%% SPHERE_TRIANGLE_QUAD_TEST tests the SPHERE_TRIANGLE_QUAD library.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 April 2014
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERE_TRIANGLE_QUAD_TEST\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Test the SPHERE_TRIANGLE_QUAD library.\n' );

  sphere_triangle_quad_test01 ( );
  sphere_triangle_quad_test02 ( );
  sphere_triangle_quad_test03 ( );
  sphere_triangle_quad_test04 ( );
  sphere_triangle_quad_test05 ( );
  sphere_triangle_quad_test06 ( );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPHERE_TRIANGLE_QUAD_TEST\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function params = IEEE80211a_settings(...
    symbolPeriod, ...
    OFDMSymPerFrame, ...
    OFDMTrainPerFrame, ...
    vtbd);

% Note: A sample period of 0.08e-6 corresponds to approx. 12 Msym/s,
% which, in turn, corresponds to 54 Mb/s for 3/4-coded 64-QAM

% IEEE 802.11a - fundamental sizes
p.NSD = 48;  % number of data symbols in OFDM symbol (variable name as in 802.11a standard)
p.NST = 52;  % number of data symbols and pilots in OFDM symbol (variable name as in 802.11a standard)
p.NFFT = 64; % number of points on FFT
p.NcyclicPrefix = 16;

p.NFFT2 = p.NFFT + p.NcyclicPrefix;

% TX indices
p.TXFFTShiftIndices = [p.NST/2+1:p.NFFT 1:p.NST/2];
p.TXCyclicPrefixIndices = [p.NFFT-[p.NcyclicPrefix-1:-1:0] 1:p.NFFT];

% RX indices
p.RXCyclicPrefixIndices = [p.NcyclicPrefix+1:p.NFFT2];
p.RXSelectFFTIndices = [p.NFFT-[p.NST/2-1:-1:0] 1:p.NST/2+1];

% OFDM symbols
p.OFDMSymPerFrame = OFDMSymPerFrame;
p.OFDMTrainPerFrame = OFDMTrainPerFrame;
p.OFDMTotSymPerFrame = OFDMSymPerFrame + OFDMTrainPerFrame;

% Constellation symbols
p.numTxSymbols = p.NSD * OFDMSymPerFrame;
p.numTrainingSymbols = p.NSD * OFDMTrainPerFrame;

% Training sequence
p.long_training_seq = ...
    [1 1 -1 -1 1 1 -1 1 -1 1 1 1 1 1 1 -1 -1 1 1 -1 1 -1 1 1 1 1 0 ...
     1 -1 -1 1 1 -1 1 -1 1 -1 -1 -1 -1 -1 1 1 -1 -1 1 -1 1 -1 1 1 1 1].';

% Modulator/demodulator banks
p.numModulators = 8;
p.txBitsPerSymbol = [1 1 2 2 4 4 6 6];
p.txBitsPerBlock = p.numTxSymbols * p.txBitsPerSymbol;
p.modOrder = 2.^p.txBitsPerSymbol;
p.codeRate = [1/2 3/4 1/2 3/4 1/2 3/4 2/3 3/4];
p.bitsPerBlock = p.txBitsPerBlock .* p.codeRate;
p.bitsPerSymbol = p.txBitsPerSymbol .* p.codeRate;
p.maxBitsPerBlock = max(p.bitsPerBlock);

% Frame size for variable rate source
p.nSource = min( gcd( min(p.bitsPerBlock), p.bitsPerBlock ) );

% Source blocks per TX frame
p.nS = p.bitsPerBlock/p.nSource;

% Timing-related parameters
p.symbolPeriod = symbolPeriod;
p.blockPeriod = p.numTxSymbols * symbolPeriod;
p.bitPeriod = symbolPeriod ./ p.bitsPerSymbol;
p.minBitPeriod =  min(p.bitPeriod);
p.chanSamplePeriod = p.blockPeriod/(p.OFDMTotSymPerFrame * p.NFFT2);

% Viterbi trace back depth and link delay
vtbd_set = vtbd(ones(1, p.numModulators));
p.vtbd_set = vtbd_set;
p.link_delay = vtbd_set;

params = p;


classdef GFMMOEA < ALGORITHM
% <multi/many> <real/integer/label/binary/permutation>
% Generic front modeling based multi-objective evolutionary algorithm
% theta --- 0.2 --- Penalty parameter
% fPFE  --- 0.1 --- Frequency of employing generic front modeling

%------------------------------- Reference --------------------------------
% Y. Tian, X. Zhang, R. Cheng, C. He, and Y. Jin, Guiding evolutionary
% multi-objective optimization with generic front modeling, IEEE
% Transactions on Cybernetics, 2020, 50(3): 1106-1119.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [theta,fPFE] = Algorithm.ParameterSet(0.2,0.1);

            %% Generate random population
            Population = Problem.Initialization();
            FrontNo    = NDSort(Population.objs,inf);
            zmin       = min(Population.objs,[],1);
            % Calculate the fitness of each solution
            [P,A]     = deal(ones(1,Problem.M));
            [App,Dis] = CalFitness(Population.objs-repmat(zmin,length(Population),1),P,A);
            Dis       = sort(Dis,2);
            Crowd     = Dis(:,1) + 0.1*Dis(:,2);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = TournamentSelection(2,Problem.N,FrontNo,-theta*App-(1-theta)*Crowd);
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                zmin       = min([zmin;Offspring.objs],[],1);
                if ~mod(ceil(Problem.FE/Problem.N),ceil(fPFE*ceil(Problem.maxFE/Problem.N))) || fPFE == 0
                    [P,A] = GFM(Population(FrontNo==1).objs-repmat(zmin,sum(FrontNo==1),1));
                end
                [Population,FrontNo,App,Crowd] = EnvironmentalSelection([Population,Offspring],P,A,zmin,theta,Problem.N);
            end
        end
    end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Max manipulability index ALONG A LINE.
% Use stomp like to optimize along a surface/line
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function experiment1B
close all;
global robot
global parameters
global hfigures

%STOMP PARAMETERS
%conversion from cost to Prob factor
parameters.lambda = .4;
parameters.lambda_obstacles = .2;
%height of the obstacle
parameters.yo = 2.5;
%cost function starts at this distance
%must be below 0.3 for the 4 DOF robot
parameters.epsilon = 0.2;
%multiply noise by this facto
%parameters.noise_k = 5;
%parameters.noise_sigma_null_space = 0.01;
parameters.alpha=0.02;
parameters.time_step=0.01;

%number of waypoints
parameters.N = 12;
%number of particles
parameters.K = 5;
parameters.n_repeat = 30;
parameters.experiment_name = 'experiment1B_K5_N30.mat';

parameters.animate = 0;

parameters.obstacles = [];

%LINE 1
x1 = -1.5;
y1 = .5; %m
x2 = 0;
y2 = 2; %m
phi = 3*pi/4;
p0 = [x1 y1 0]';
pf = [x2 y2 0]';
T0 = build_T_4dof(p0, phi);
parameters.obstacles{1}.line = [p0 pf];
parameters.obstacles{1}.T0 = T0;

%repeat the experiment E times
random_manips=[];
Gout = [];
for i=1:parameters.n_repeat
    close all
    [pk, final_manip] = stomp_null_space(robot);
    Gout{i}=pk;
    random_manips = [random_manips; final_manip];
    save(parameters.experiment_name)
end

function T = build_T_4dof(p, phi)
T = [cos(phi) -sin(phi) 0 p(1);
     sin(phi) cos(phi) 0 p(2);
     0            0     1  p(3);
     0             0    0   1];

 function T = build_T_sawyer(p, phi)
T = [1  -sin(phi) 0 p(1);
     0  cos(phi) 0 p(2);
     0            0     1  p(3);
     0             0    0   1];


classdef prtKernelRbfFixed < prtKernel

    properties (SetAccess = private)
        name = 'RBF Kernel'; % RBF Kernel
        nameAbbreviation = 'RBF'; % RBF
    end

    properties
        sigma = 1; % The inverse kernel width
        x0 = 0;
    end

    methods (Hidden = true)

        function nDimensions = nDimensions(self)
            nDimensions = 1;
        end

    end
    methods (Access = protected, Hidden = true)
        function self = trainAction(self,ds)
            self.internalDataSet = ds;
            self.isTrained = true;
        end

        function dsOut = runAction(self,ds)
            if ~self.isTrained
                error('prtKernelRbf:run','Attempt to run an untrained kernel; use kernel.train(ds) to train');
            end
            if self.internalDataSet.nObservations == 0
                dsOut = prtDataSetClass;
            else
                gram = prtKernelRbf.kernelFn(ds.getObservations,self.x0,self.sigma);
                dsOut = ds.setObservations(gram);
            end
        end
    end

    methods
        function self = prtKernelRbfFixed(varargin)
            self = prtUtilAssignStringValuePairs(self,varargin{:});
        end

        function self = set.sigma(self,value)
            if ~prtUtilIsPositiveScalar(value)
                assert(isnumeric(value) && all(value>0) && isvector(value),'sigma must be a positive numeric vector')
                if isempty(self.internalDataSet) || self.internalDataSet.nObservations==0
                    error('prtKernelRbf:set','Value of sigma must be a positive scalar');
                else
                    assert(self.internalDataSet.nObservations==numel(value),'When setting sigma to be an array of values the internalDataSet must be set and the number of observations and the length of sigma must match');
                end
            end
            self.sigma = value(:);
        end
    end

    methods(Hidden = true)
        function varargout = plot(obj)
            x = obj.internalDataSet.getObservations;

            if size(x,2) <= 3
                if size(x,2) == 1 && obj.internalDataSet.isLabeled
                    xy = cat(2,x,obj.internalDataSet.getTargets);
                    h = prtPlotUtilScatter(xy, {}, obj.plotOptions.symbol, obj.plotOptions.markerFaceColor, obj.plotOptions.color, obj.plotOptions.symbolLineWidth, obj.plotOptions.symbolSize);
                else
                    h = prtPlotUtilScatter(x, {}, obj.plotOptions.symbol, obj.plotOptions.markerFaceColor, obj.plotOptions.color, obj.plotOptions.symbolLineWidth, obj.plotOptions.symbolSize);
                end
            else
                h = nan;
            end

            varargout = {};
            if nargout
                varargout = {h};
            end
        end
    end

    methods (Static, Hidden = true)
        function gram = kernelFn(x,y,sigma)
            [n1, d] = size(x);
            [n2, nin] = size(y);
            %             if d ~= nin
            %                 error('size(x,2) must equal size(y,2)');
            %             end
            %             keyboard
            %dist2 = prtDistanceLNorm(x,y,2);
            dist2 = repmat(sum((x.^2), 2), [1 n2]) + repmat(sum((y.^2),2), [1 n1]).' - 2*x*(y.');

            if numel(sigma) == 1
                gram = exp(-dist2/(sigma.^2));
            else
                gram = exp(-bsxfun(@rdivide,dist2,(sigma.^2)'));
            end
        end
    end
end


function mono_between_next_grlex_test ( )

%*****************************************************************************80
%
%% MONO_BETWEEN_NEXT_GRLEX_TEST tests MONO_BETWEEN_NEXT_GRLEX.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 November 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MONO_BETWEEN_NEXT_GRLEX_TEST\n' );
  fprintf ( 1, '  MONO_BETWEEN_NEXT_GRLEX can list the monomials\n' );
  fprintf ( 1, '  in M variables, of total degree N between N1 and N2,\n' );
  fprintf ( 1, '  one at a time, in graded lexicographic order.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We start the process with (0,0,...,0,N1).\n' );
  fprintf ( 1, '  The process ends with (N2,0,...,0,0)\n' );

  n1 = 2;
  n2 = 3;
  m = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Let M =  %d\n', m );
  fprintf ( 1, '      N1 = %d\n', n1 );
  fprintf ( 1, '      N2 = %d\n', n2 );
  fprintf ( 1, '\n' );

  x = [ 0, 0, n1 ];
  i = 1;

  while ( 1 )

    fprintf ( 1, '  %2d:', i );
    for j = 1 : m
      fprintf ( 1, '  %1d', x(j) );
    end
    fprintf ( 1, '\n' );

    if ( x(1) == n2 )
      break
    end

    x = mono_between_next_grlex ( m, n1, n2, x );
    i = i + 1;

  end

  return
end


function rx = rxIn(rx);
%
% rx = rxIn(rx);
%
% For mrRx, nudge the prescription 'in' (along the vector pointing from
% the middle of the last slice toward the middle of the first slice) a
% certain delta according to the value of the nudge slider.
%
% ras, 08/05.
if ieNotDefined('rx')
    cfig = findobj('Tag','rxControlFig');
    rx = get(cfig,'UserData');
end

%%%%%get params
nudgeVal = get(rx.ui.nudge.sliderHandle,'Value')/6; % make a small nudge

%%%%%compute a vector pointing 'in'
% middle X, Y coords
midX = rx.rxDims(2)/2;
midY = rx.rxDims(1)/2;

% cols of coords are: middle of last slice, middle of first slice
coords(:,1) = [midX; midY; rx.rxDims(3)];
coords(:,2) = [midX; midY; 1];

% convert coords into volume coordinate space
volCoords = rx2vol(rx,coords);

% get vector as diff. b/w these two points
vec = diff(volCoords');

%%%%%modify translation accordingly
[trans rot scale skew] = affineDecompose(rx.xform);
trans = trans + nudgeVal.*vec;
newXform = affineBuild(trans,rot,scale,skew);
rx = rxSetXform(rx,newXform);

return


% 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


function value = r8vec_insignificant ( n, r, s )

%*****************************************************************************80
%
%% R8VEC_INSIGNIFICANT determines if an R8VEC is insignificant.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 November 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the vectors.
%
%    Input, real R(N), the vector to be compared against.
%
%    Input, real S(N), the vector to be compared.
%
%    Output, logical R8VEC_INSIGNIFICANT, is TRUE if S is insignificant
%    compared to R.
%
  value = 1;

  for i = 1 : n

    t = r(i) + s(i);
    tol = eps * abs ( r(i) );

    if ( tol < abs ( r(i) - t ) )
      value = 0;
      exit;
    end

  end

  return
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 [gKern, gVarmeans, gVarcovars] = linard2VardistPsi0Gradient(linard2Kern, vardist, covGrad)

% LINARD2VARDISTPSI0GRADIENT description.

% VARGPLVM

A = linard2Kern.inputScales;
gKern = covGrad*sum((vardist.means.*vardist.means) + vardist.covars,1);

gVarmeans = 2*(vardist.means*sparse(diag(A)));
%gVarmeans1 = 2*(repmat(A,size(vardist.means,1),1).*vardist.means);

gVarcovars = ones(size(vardist.means,1),1)*A;

gVarmeans = covGrad*gVarmeans(:)';
gVarcovars = covGrad*gVarcovars(:)';


%==============================================================================
% This code is part of the VAMPIRE app for the Matlab-based toolbox
%  FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR/VAMPIRE.m
%==============================================================================
%
% 2D Example for Multilevel Mass-Preserving Image Registration using VAMPIRE
%
% 	- data                 synthetic 2D Gaussian blobs (level 3:8, full
%                          resolution: 256x256)
%   - viewer               viewImage2D
%   - interpolation        splineInter
%   - distance             SSD
%   - pre-registration     none
%   - regularizer          mfHyperElastic
%   - optimizer            Gauss-Newton with ArmijoBacktrack linesearch

setup2DGaussianData;

% prepare the plot
FAIRplots('clear')
Dshow = @(T,R,omega,m) viewImage2D(128+(T-R)/2,omega,m,'colormap',gray(256));
FAIRplots('set','Dshow',Dshow);

% initialize the regularizer for the non-parametric part
alpha       = 100;
alphaLength = 1;
alphaArea   = 0;
alphaVolume = 1;
[reg,regOptn] = regularizer('reset', 'regularizer', 'mfHyperElastic', ...
    'alpha',alpha, 'alphaLength', alphaLength, 'alphaArea', alphaArea, ...
    'alphaVolume', alphaVolume);

% finally: run the Mass-Preserving Non-Parametric Image Registration
NPIRpara            = optPara('NPIR-GN');
NPIRpara.lineSearch = @ArmijoDiffeomorphic;
NPIRpara.solver     = @VAMPIREsolveGN_PCG;

[yc,wc,his] = MLIR(ML, 'NPIRobj', @VAMPIRENPIRobjFctn, ...
    'parametric', false, 'NPIRpara', NPIRpara, 'minLevel', 5);

% [reg,regOptn] = regularizer('reset','regularizer','mfElastic',...
%   'alpha',alpha,'alphaLength',alphaLength,'alphaArea',alphaArea,...
%   'alphaVolume',alphaVolume);
%
% % finally: run the  Non-Parametric Image Registration
% [yc,wc,his] = MLIR(ML, 'NPIRobj', @NPIRobjFctn, ...
%   'parametric', false, 'minLevel', 4);

%% Plot Results
% Compute resulting image: dataT(yc) * det(D(yc))
Topt = reshape(linearInter(dataT,omega,center(yc,m)) .* geometry(yc,m,'Jac','omega',omega), m);
figure;
subplot(2,2,1);
viewImage2D(dataT,omega,m,'colormap', 'gray(256)');
hold on; plotGrid(center(yc, m), omega, m, 'spacing', [5 5]); axis off; hold off;
title('Template (T) & Grid')
subplot(2,2,2);
viewImage2D(dataR,omega,m,'colormap', 'gray(256)'); axis off;
title('Reference (R)')
subplot(2,2,3);
viewImage2D(Topt,omega,m,'colormap', 'gray(256)'); axis off;
title('VAMPIRE result (Topt)')
subplot(2,2,4);
viewImage2D(abs(Topt-dataR),omega,m,'colormap', 'gray(256)'); axis off;
title('Absolute difference image of R and Topt');


%%**********************************************************************
%% HSDNTpred: Compute (dX,dy,dZ) for NT direction.
%%
%% compute SVD of Xchol*Zchol via eigenvalue decompostion of
%%     Zchol * X * Zchol' = V * diag(sv2) * V'.
%% compute W satisfying W*Z*W = X.
%%     W = G'*G,  where G = diag(sqrt(sv)) * (invZchol*V)'
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%**********************************************************************

function [par,dX,dy,dZ,coeff,L,hRd] = ...
    HSDNTpred(blk,At,par,rp,Rd,sigmu,X,Z,Zchol,invZchol)

global schurfun schurfun_par
%%
%% compute NT scaling matrix
%%
[par.W,par.G,par.sv,par.gamx,par.gamz,par.dd,par.ee,par.ff] = ...
    NTscaling(blk,X,Z,Zchol,invZchol);
%%
%% compute schur matrix
%%
m = par.m;
schur = sparse(m+2,m+2);
UU = []; EE = [];
%%
for p = 1:size(blk,1)
    pblk = blk(p,:);
    if strcmp(pblk{1},'l')
        [schur,UU,EE] = schurmat_lblk(blk,At,par,schur,UU,EE,p,par.dd);
    elseif strcmp(pblk{1},'q');
        [schur,UU,EE] = schurmat_qblk(blk,At,par,schur,UU,EE,p,par.dd,par.ee);
    elseif strcmp(pblk{1},'s')
        if isempty(schurfun{p})
            schur = schurmat_sblk(blk,At,par,schur,p,par.W);
        elseif ischar(schurfun{p})
            if ~isempty(par.permZ{p})
                Wp = par.W{p}(par.permZ{p},par.permZ{p});
            else
                Wp = par.W{p};
            end
            schurtmp = feval(schurfun{p},Wp,Wp,schurfun_par(p,:));
            schur = schur + schurtmp;
        end
    end
end
%%
%% compute rhs
%%
[rhs,EinvRc,hRd] = HSDNTrhsfun(blk,At,par,X,Z,rp,Rd,sigmu);
%%
%% solve linear system
%%
par.addschur   = par.kap/par.tau;
schur(m+1,m+1) = schur(m+1,m+1) + par.kap/par.tau;
schur(m+2,m+2) = schur(m+2,m+2) + par.addschur;
[xx,coeff,L] = HSDlinsysolve(par,schur,UU,EE,par.Umat,rhs);
%%
%% compute (dX,dZ)
%%
[par,dX,dy,dZ] = HSDNTdirfun(blk,At,par,Rd,EinvRc,xx);
%%**********************************************************************


function prediction = BasicCifarNet(varargin)
%BASICCIFARNET Returns a simple network for CIFAR10
%   M = models.BasicCifarNet() returns a model inspired by:
%
%     LeCun et al., "Gradient-Based Learning Applied to Document
%     Recognition", Proceedings of the IEEE, 1998.
%
%   models.BasicCifarNet(..., 'option', value, ...) accepts the following
%   options:
%
%   `input`:: default input
%     Specifies an input (images) layer for the network. If unspecified, a
%     new one is created.
%
%   `numClasses`:: 10
%     Number of output classes.
%
%   `batchNorm`:: true
%     Whether to use batch normalization.
%
%   Any other options will be passed to models.ConvBlock(), and can be used
%   to change the activation function, weight initialization, etc.
%
%   Suggested SGD training options are also returned in the struct M.meta.

  % parse options. unknown arguments will be passed to ConvBlock (e.g.
  % activation).
  opts.input = Input('name', 'images', 'gpu', true) ;  % default input layer
  opts.numClasses = 10 ;
  opts.batchNorm = true ;  % whether to use batch normalization
  [opts, convBlockArgs] = vl_argparse(opts, varargin, 'nonrecursive') ;

  % get conv block generator with the given options. default activation is
  % ReLU, with pre-activation batch normalization (can be overriden).
  conv = models.ConvBlock('batchNorm', opts.batchNorm, ...
    'preActivationBatchNorm', true, convBlockArgs{:}) ;

  % build network
  images = opts.input ;

  x = conv(images, 'size', [5, 5, 3, 32], 'pad', 2, 'weightScale', 0.01) ;
  x = vl_nnpool(x, 3, 'stride', 2, 'method', 'max', 'pad', 1) ;

  x = conv(x, 'size', [5, 5, 32, 32], 'pad', 2, 'weightScale', 0.05) ;
  x = vl_nnpool(x, 3, 'stride', 2, 'method', 'avg', 'pad', 1) ;

  x = conv(x, 'size', [5, 5, 32, 64], 'pad', 2, 'weightScale', 0.05) ;
  x = vl_nnpool(x, 3, 'stride', 2, 'method', 'avg', 'pad', 1) ;

  x = conv(x, 'size', [4, 4, 64, 64], 'weightScale', 0.05) ;

  prediction = conv(x, 'size', [1, 1, 64, opts.numClasses], 'weightScale', 0.05, ...
    'batchNorm', false, 'activation', 'none') ;

  % default training options for this network
  defaults.batchSize = 128 ;
  defaults.weightDecay = 0.0005 ;
  if ~opts.batchNorm
    defaults.learningRate = 0.01 ;
    defaults.numEpochs = 100 ;
  else
    defaults.learningRate = 0.1 ;
    defaults.numEpochs = 40 ;
  end
  defaults.imageSize = [32, 32, 3] ;
  prediction.meta = defaults ;

end


function y = previewsong(frequencies, durations, gap, Fs)
% PREVIEWSONG plays the output of song() on the host PC.
%
%   PREVIEWSONG converts frequency domain data in the
%   form of a frequency vector and a duration vector
%   into a single time domain vector and plays it over
%   the PC speaker using the sound command.
%
%   frequencies - Vector of frequencies.
%   durations   - Vector of times.
%   gap         - Pause between notes. Default 0.01.
%   Fs          - Sample rate. Default 8192.
%
%   Example:
%      [sng.tune, sng.rhythm, sng.message, sng.stats] = song('random');
%      previewsong(sng.tune, sng.rhythm);

   if nargin < 4, Fs          = 8192;     end
   if nargin < 3, gap         = 0.01;     end
   if nargin < 2, durations   = 1;        end
   if nargin < 1, frequencies = 261.6256; end

   if length(durations) < length(frequencies)
      for i=length(durations)+1:length(frequencies)
         durations(i) = 0;
      end
   end

   if gap
      paddedFrequencies = [];
      paddedDurations = [];
      for i=1:length(frequencies)
         paddedFrequencies = [paddedFrequencies frequencies(i) 0]; %#ok<AGROW>
         paddedDurations = [paddedDurations durations(i) gap]; %#ok<AGROW>
      end
   else
      paddedFrequencies = frequencies;
      paddedDurations = durations;
   end

   deltaT = 1.0/Fs;

   startTime = 0;
   y = [];
   for i=1:length(paddedFrequencies)
%     timeIntervals = 0 : deltaT : paddedDurations(i);
      timeIntervals = startTime : deltaT : startTime+paddedDurations(i);
      startTime = timeIntervals(1)+deltaT;
      yCurrent = sin(2*pi*paddedFrequencies(i)*timeIntervals);
      y = [y yCurrent]; %#ok<AGROW>
   end

   sound(y, Fs);

return   % end of previewsong()


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   ABB IRB1600iD.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Authors:Daniel Vivancos Unica
%        Jose David Martinez Exposito
%        Maria Jose Martinez Liza
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

function robot = parameters()

robot.name= 'ABB_IRB1600ID';

%Path where everything is stored for this robot
robot.path = 'robots/abb/IRB1600ID';

robot.DH.theta= '[q(1) q(2)-pi/2 q(3) q(4) q(5) q(6)+pi]';
robot.DH.d='[0.4865 0 0 0.640 0 0.2]';
robot.DH.a='[0.15 0.7 0.11 0 0 0]';
robot.DH.alpha= '[-pi/2 0 -pi/2 pi/2 -pi/2 0]';
robot.J=[];

robot.inversekinematic_fn = 'inversekinematic_irb1600id(robot, T)';

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

%rotational: 0, translational: 1
robot.kind=['R' 'R' 'R' 'R' 'R' 'R'];

%minimum and maximum rotation angle in rad
robot.maxangle =[-pi pi; %Axis 1, minimum, maximum
                deg2rad(-90) deg2rad(150); %Axis 2, minimum, maximum
                deg2rad(-238) deg2rad(79); %Axis 3
                deg2rad(-155) deg2rad(155); %Axis 4
                deg2rad(-90) deg2rad(135); %Axis 5
                deg2rad(-200) deg2rad(200)]; %Axis 6

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(180); %Axis 1, rad/s
                deg2rad(180); %Axis 2, rad/s
                deg2rad(180); %Axis 3, rad/s
                deg2rad(320); %Axis 4, rad/s
                deg2rad(380); %Axis 5, rad/s
                deg2rad(460)];%Axis 6, rad/s
robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time

            % end effectors maximum velocity
robot.linear_velmax = 2.5; %m/s

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

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

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%DYNAMICS

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
robot.has_dynamics=1;

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

%link masses (kg)
%Tenemos que repartir 250kg (peso total robot IRB1600ID entre los 6 eslabones)
%Utilizando como material Aluminio, los datos obtenidos del programa Inventor seran:

robot.dynamics.masses=[0 109.440 28.242 31.079 6.936 1.430 0.249];
%COM of each link with respect to own reference system
robot.dynamics.r_com=[0.052       -0.012    0.343; %(rx, ry, rz) link 1
         0.150       0.791     -0.183;%(rx, ry, rz) link 2
         1.172	     0.140	   0.017; %(rx, ry, rz) link 3
         1.296       0.063     0.561; %(rx, ry, rz) link 4
         1.296       0.870     0.027; %(rx, ry, rz) link 5
         0           1.296     0.966];%(rx, ry, rz) link 6

%Inertia matrices of each link with respect to its D-H reference system.
% Ixx	Iyy	Izz	Ixy	Iyz	Ixz, for each row
robot.dynamics.Inertia=[0      0	    0   	0	0	0;
         2.305  2.696	3.802	0	0	0;
         .0700	1.765	1.784	0	0	0;
         0.267  0.324	0.332	0	0	0;
         0.297	0.290	.023	0	0	0;
         .008	.002	.008	0   0	0;
         0      0       0       0   0   0];
%Los momentos de Inercia y los centros de gravedad quedan corregidos de los
%obtenidos del programa Inventor para ser ajustados al sistema de
%coordenadas designado en nuestro robot.

robot.motors=load_motors([5 5 5 4 4 4]);
%Speed reductor at each joint
robot.motors.G=[300 300 300 300 300 300];


function X = skew(x)

X=[0 -x(3) x(2) ; x(3) 0 -x(1) ; -x(2) x(1) 0 ];

end


function [node,elem,bdFlag,HB] = uniformbisect3(node,elem,bdFlag,HB)
%% UNIFORMBISECT3 uniformly bisect a 3-D triangulation.
%
% [node,elem] = uniformbisect3(node,elem) divides each tetrahedron into 8 small
% tetrahedrons using longest vertex bisection.
%
% [node,elem,bdFlag,HB] = uniformbisect3(node,elem,bdFlag,HB) update HB and
% boundary conditions.
%
% Example
%
%     node = [-1,-1,-1; 1,-1,-1; 1,1,-1; -1,1,-1; -1,-1,1; 1,-1,1; 1,1,1; -1,1,1];
%     elem = [1,2,3,7; 1,6,2,7; 1,5,6,7; 1,8,5,7; 1,4,8,7; 1,3,4,7];
%     elem = label3(node,elem);
%     figure(1); subplot(1,3,1);
%     set(gcf,'Units','normal'); set(gcf,'Position',[0.25,0.25,0.5,0.3]);
%     showmesh3(node,elem,[],'FaceAlpha',0.35); view([210 8]);
%     [node,elem] = uniformbisect3(node,elem);
%     figure(1); subplot(1,3,2);
%     showmesh3(node,elem,[],'FaceAlpha',0.35); view([210 8]);
%     bdFlag = setboundary3(node,elem,'Dirichlet');
%     [node,elem,bdFlag] = uniformbisect3(node,elem,bdFlag);
%     figure(1); subplot(1,3,3);
%     showmesh3(node,elem,[],'FaceAlpha',0.35); view([210 8]);
%
% See also uniformbisect, uniformrefine, bisect, bisect3
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('bdFlag','var')
    bdFlag =[];
end
if ~exist('HB','var')
    HB = [];
end
[node,elem,bdFlag,HB] = bisect3(node,elem,'all',bdFlag,HB);
[node,elem,bdFlag,HB] = bisect3(node,elem,'all',bdFlag,HB);
[node,elem,bdFlag,HB] = bisect3(node,elem,'all',bdFlag,HB);


function [nm] = A2nm(A)
% Convert length from angstroms to nanometers.
% Chad A. Greene 2012
nm = A*0.1;


classdef ZDT5 < PROBLEM
% <multi> <binary> <large/none> <expensive/none>
% Benchmark MOP proposed by Zitzler, Deb, and Thiele

%------------------------------- Reference --------------------------------
% E. Zitzler, K. Deb, and L. Thiele, Comparison of multiobjective
% evolutionary algorithms: Empirical results, Evolutionary computation,
% 2000, 8(2): 173-195.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 80; end
            obj.D        = ceil(max(obj.D-30,1)/5)*5 + 30;
            obj.encoding = 4 + zeros(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            u      = zeros(size(PopDec,1),1+(size(PopDec,2)-30)/5);
            u(:,1) = sum(PopDec(:,1:30),2);
            for i = 2 : size(u,2)
                u(:,i) = sum(PopDec(:,(i-2)*5+31:(i-2)*5+35),2);
            end
            v           = zeros(size(u));
            v(u<5)      = 2 + u(u<5);
            v(u==5)     = 1;
            PopObj(:,1) = 1 + u(:,1);
            g           = sum(v(:,2:end),2);
            h           = 1./PopObj(:,1);
            PopObj(:,2) = g.*h;
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = 1 : 31;
            R(:,2) = (obj.D-30)./5./R(:,1);
        end
    end
end


%GETNEXTCLUSTERS Takes in a cluster graph and returns the indices
%   of the nodes between which the next message should be passed.
%
%   [i j] = GetNextClusters(P,Messages,oldMessages,m,useSmart)
%
%   INPUT
%     P - our cluster graph
%     Messages - the current values of all messages in P
%     oldMessages - the previous values of all messages in P. Thus,
%         oldMessages(i,j) contains the value that Messages(i,j) contained
%         immediately before it was updated to its current value
%     m - the index of the message we are passing (ie, m=0 indicates we have
%         passed 0 messages prior to this one. m=5 means we've passed 5 messages
%     useSmart - indicates whether we should use the Naive or Smart message
%         passing order
%
%
%   Output [i j]
%     i = the origin of the m+1th message
%     j = the destination of the m+1th message
%
% Copyright (C) Daphne Koller, Stanford University, 2012

function [i j] = GetNextClusters(P,Messages,oldMessages,m,useSmart)

if(~exist('useSmart','var')||~useSmart)
  [i j] = NaiveGetNextClusters(P,m);
else
  [i j] = SmartGetNextClusters(P,Messages,oldMessages,m);
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 [A, B, C, D, tau, kappa, epsilon] = theta2abcd(theta, idx, R, L )
% Transform DCM parameters from vectorized to structured format
%
% This is a protected method of the tapas_Huge class. It cannot be called
% from outside the class.
%

% Author: Yu Yao (yao@biomed.ee.ethz.ch)
% Copyright (C) 2019 Translational Neuromodeling Unit
%                    Institute for Biomedical Engineering,
%                    University of Zurich and ETH Zurich.
%
% This file is part of TAPAS, which is released under the terms of the GNU
% General Public Licence (GPL), version 3. For further details, see
% <https://www.gnu.org/licenses/>.
%
% This software is provided "as is", without warranty of any kind, express
% or implied, including, but not limited to the warranties of
% merchantability, fitness for a particular purpose and non-infringement.
%
% This software is intended for research only. Do not use for clinical
% purpose. Please note that this toolbox is under active development.
% Considerable changes may occur in future releases. For support please
% refer to:
% https://github.com/translationalneuromodeling/tapas/issues
%

tmp = zeros(idx.P_f, 1);
tmp([idx.clustering; idx.homogenous]) = theta;
% hemodynamic parameters
iEd = numel(tmp);
epsilon = tmp(iEd); % ratio of intra- and extravascular signal
iEd = iEd - 1;
kappa = tmp(iEd-R+1:iEd); % decay of vasodilatory signal
iEd = iEd - R;
tau = tmp(iEd-R+1:iEd); % transit time
iEd = iEd - R;

% connectivity parameters
iSt = 0;
A = reshape(tmp(iSt+1:iSt+R^2), R, R);
iSt = iSt + R^2;
B = reshape(tmp(iSt+1:iSt+R^2*L), R, R, L);
iSt = iSt + R^2*L;
C = reshape(tmp(iSt+1:iSt+R*L), R, L);
iSt = iSt + R*L;
% nonlinear connections
if iEd - iSt == R^3
    D = reshape(tmp(iSt+1:iEd), R, R, R);
else
    D = zeros(R, R, 0);
end

end


function M_hat = run_mc(params)
  M = params.M;
  Idx = params.Idx;
  Ms = sparse(M);
  [~,~,VALS] = find(Ms);
  Known = logical(Idx);
  [U,S,V,hist] = ScGrassMC(Known, VALS, 2,...
                    'tol', 1.e-6,...
                    'maxit',100,...
                    'grad_type','scaled',...
                    'beta_type','P-R',...
                    'sigma_type','approx',... %'tol_reschg', tol_reschg,...
                    'verbose', 1);
  M_hat = (U*S*V');
end


clear all; close all;
N=0:4
X=fftshift(N)
Y=fftshift(fftshift(N))
Z=ifftshift(fftshift(N))


function camAAr2c = projICP(camAAr2c)

    global VMap;
    global NMap;
    global XYZcamBilateral;
    global NcamBilateral;
    global K;

    global NMapCam;
    global VMapCam;
    global Vku;

    for iterID = 1:10

        %% data association

        % transform the point cloud

        VMapCam = AngleAxisRotatePoint(camAAr2c, VMap) + repmat(camAAr2c(4:6)',1,size(VMap,2));
        NMapCam = AngleAxisRotatePoint(camAAr2c, NMap);

        % projective association

        px = round(K(1,1)*(VMapCam(1,:)./VMapCam(3,:)) + K(1,3));
        py = round(K(2,2)*(VMapCam(2,:)./VMapCam(3,:)) + K(2,3));

        isValid = (1<=px & px <= 640 & 1<=py & py<= 480);
        VMapCam = VMapCam(:,isValid);
        NMapCam = NMapCam(:,isValid);
        px = px(isValid);
        py = py(isValid);

        ind = sub2ind([480 640],py,px);

        %{
        cla(subplot(6,3,12))
        validMap = zeros(480,640);
        validMap(ind) = 1;
        imagesc(validMap);
        axis equal;
        axis tight;
        title(sprintf('Iteration %d: init valid Map',iterID))
        %}

        isValid = XYZcamBilateral(640*480*2+ind)~=0;
        VMapCam = VMapCam(:,isValid);
        NMapCam = NMapCam(:,isValid);
        ind = ind(isValid);

        % outlier rejection

        diffD = (VMapCam(1,:) - XYZcamBilateral(ind)).^2 + (VMapCam(2,:) - XYZcamBilateral(640*480+ind)).^2 + (VMapCam(3,:) - XYZcamBilateral(640*480*2+ind)).^2 ;
        dotProdN = sum([NcamBilateral(ind); NcamBilateral(640*480+ind); NcamBilateral(640*480*2+ind)] .* NMapCam,1);

        subplot(6,3,17)
        validMap = zeros(480,640);
        validMap(ind) = diffD;
        imagesc(validMap);
        axis equal;
        axis tight;
        title(sprintf('Iteration %d: diffD',iterID))

        subplot(6,3,18)
        validMap = zeros(480,640);
        validMap(ind) = dotProdN;
        imagesc(validMap);
        axis equal;
        axis tight;
        title(sprintf('Iteration %d: dotProdN',iterID))

        %isValid = (diffD<0.1^2) & (dotProdN > cos(pi/3));
        isValid = (diffD<1^2);

        VMapCam = VMapCam(:,isValid);
        NMapCam = NMapCam(:,isValid);
        ind = ind(isValid);
        Vku = [XYZcamBilateral(ind); XYZcamBilateral(640*480+ind); XYZcamBilateral(640*480*2+ind)];

        subplot(6,3,15)
        validMap = zeros(480,640);
        validMap(ind) = 1;
        imagesc(validMap);
        axis equal;
        axis tight;
        title(sprintf('Iteration %d: valid Map',iterID))

        %% optimization
        % objective function
        E = sum(( Vku - VMapCam ) .* NMapCam,1);

        subplot(6,3,16)
        validMap = zeros(480,640);
        validMap(ind) = E;
        imagesc(validMap);
        axis equal;
        axis tight;
        title(sprintf('Iteration %d: Distance Map',iterID))

        fprintf('initial error: #inliers = %.2f(%d/%d), sum = %f, mean = %f, median = %f\n', sum(isValid)/length(isValid), sum(isValid), length(isValid), sum(E.^2), mean(E.^2), median(E.^2));

        %options = optimset('Display','iter', 'Algorithm','levenberg-marquardt');
        options = optimset('display','off','Algorithm','levenberg-marquardt');
        [AA_gk, resnorm, residual, exitflag, output] = lsqnonlin(@residualFunction, [0 0 0 0 0 0],[],[],options);

        camAAr2c = cameraRt2AngleAxis(concatenateCameraRt(transformCameraRt(cameraAngleAxis2Rt(AA_gk)), cameraAngleAxis2Rt(camAAr2c)));
    end

end

function residuals = residualFunction(Tgk)
    global NMapCam;
    global VMapCam;
    global Vku;

    VkuTran = AngleAxisRotatePoint(Tgk, Vku) + repmat(Tgk(4:6)',1,size(Vku,2));

    residuals = ( VkuTran - VMapCam ) .* NMapCam;
end

%{
% for visualization

figure
image2show = zeros(480,640);
image2show(ind) = distance(1,:);
imagesc(image2show); axis equal; axis tight;
title('Vm(1) - Vd(1)')

figure
image2show = zeros(480,640);
image2show(ind) = distance(2,:);
imagesc(image2show); axis equal; axis tight;
title('Vm(2) - Vd(2)')

figure
image2show = zeros(480,640);
image2show(ind) = distance(3,:);
imagesc(image2show); axis equal; axis tight;
title('Vm(3) - Vd(3)')

figure
image2show = zeros(480,640);
image2show(ind) = sum(distance.^2,1);
imagesc(image2show); axis equal; axis tight;
title('(Vm - Vd)^2')

figure
image2show = zeros(480,640);
image2show(ind) = sum(distance .* NMapCam,1).^2;
imagesc(image2show); axis equal; axis tight;
title('E')

%}

% coarse to fine


function I2 = undistortImage(I,K,kc)
% function to undo lens distortion with perspective camera intrinsics K and kc
% This is the model from Oulu university and described clearly in Bouguet's Matlab camera calibration toolbox
% (http://www.vision.caltech.edu/bouguetj/calib_doc/). This is done using
% Bouguet's toolbox (rect). You need to download it.

if ~exist('rect.m','file')
    error('To undistort an image you need to download Bouguets Matlab camera calibration toolbox (http://www.vision.caltech.edu/bouguetj/calib_doc/) and add it to the path.')
end
if isa(I,'uint8')
   I = im2double(I);
end
for i=1:size(I,3)
    I2(:,:,i) =rect((I(:,:,i)),eye(3),[K(1,1),K(2,2)],[K(1,3),K(2,3)],kc,K(1,2)/K(1,1),K);
end
I2 = uint8(I2*255);


function [Pt,a,b] = mci_lds_lat2par (P,M)
% Convert latent params to params
% FORMAT [Pt,a,b] = mci_lds_lat2par (P,M)
%
% P         Parameters (latent)
% M         model structure
%
% Pt        Parameters (transformed)
% a         diagonal values
% b         off-diagonal values
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: mci_lds_lat2par.m 6548 2015-09-11 12:39:47Z will $

% Diagonal entries
s=exp(P(1:M.d));
a=s*M.a_typical;

% Off-diagonal entries
b=P(M.d+1:end);

Pt=[a(:);b(:)];


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


function rot = power(rot,n)
% r.^n
%
% Syntax
%   rot = rot^(-1) % inverse rotation
%   rot = rot.^2
%   rot = rot.^[0,1,2,3]
%
%
% Input
%  rot - @rotation
%
% Output
%  rot - @rotation
%
% See also
% rotation/log

rot = power@quaternion(rot,n);

% change inversion
rot.i = (1-(1-2*rot.i).^n)./2;


function i4vec_permute_test ( )

%*****************************************************************************80
%
%% I4VEC_PERMUTE_TEST tests I4VEC_PERMUTE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 May 2015
%
%  Author:
%
%    John Burkardt
%
  n = 12;
  b = 0;
  c = n;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_PERMUTE_TEST\n' );
  fprintf ( 1, '  I4VEC_PERMUTE reorders an integer vector\n' );
  fprintf ( 1, '  according to a given permutation.\n' );
  fprintf ( 1, '  Using initial random number seed = %d\n', seed );

  [ a, seed ] = i4vec_uniform_ab ( n, b, c, seed );

  i4vec_print ( n, a, '  A, before rearrangement:' );

  p = perm1_uniform ( n, seed );

  i4vec_print ( n, p, '  Permutation vector P:' );

  a = i4vec_permute ( n, a, p );

  i4vec_print ( n, a, '  A, after rearrangement:' );

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function F= GetMeyerBasedFilter(level,dBand,dataClass)
% Generates windowing filters
%Input:
%        level      : In multilevel decomposition number of level
%        dataClass  : 'single' or 'double'
%Output:
%        F          : Windowing filter at different level
%                   :
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    function F= GetMeyerBasedFilter(level,dBand,filterSize,dataClass)

F=cell(3,level);

for l=1:level

cubeSize=filterSize(l);

numDir=dBand{level}{l}(1,1);

[P,PF]=GeneratePyramidSection(cubeSize);
SF=PF{1}+PF{2}+PF{3};
PF{1}=SF;
PF{2}=SF;
PF{3}=SF;
shift=floor(cubeSize/numDir);
F{3,l}=cell(numDir,numDir);

A=zeros(cubeSize,cubeSize,cubeSize,dataClass);

for c=1:3
	mRadial=ones(shift+1,cubeSize,shift+1,dataClass);
  mRadial( [1 shift+1],1:cubeSize,1: shift+1)=.5;
  mRadial(1: shift+1,1: cubeSize,[1 shift+1])=.5;
  mRadial(1,1: cubeSize,1)=1/3;

  l2=1;
  l1=1;
  mRadialIdx=[(l1-1)*shift+1  l1*shift+1 ;1 cubeSize ; (l2-1)*shift+1  l2*shift+1  ];

  F{c,l}{l2,l1}=PolarToRec(mRadialIdx,mRadial,cubeSize,P{c},PF{c} );
  A=A+F{c,l}{l2,l1};

  mRadial=ones(shift+1,cubeSize,shift+1,dataClass);
  mRadial( [1 shift+1],1:cubeSize,1: shift+1)=.5;
  mRadial(1: shift+1,1: cubeSize,[1 shift+1])=.5;

   for l2=2:numDir-1
    for l1=2:numDir-1
      mRadialIdx=[(l1-1)*shift+1  l1*shift+1 ;1 cubeSize ; (l2-1)*shift+1  l2*shift+1  ];

      F{c,l}{l2,l1}=PolarToRec(mRadialIdx,mRadial,cubeSize,P{c},PF{c} );
      A=A+F{c,l}{l2,l1};
    end
   end

  mRadial=ones(shift,cubeSize,shift+1,dataClass);
  mRadial( [1 shift],1:cubeSize,1: shift+1)=.5;
  mRadial(1: shift,1: cubeSize,[1 shift+1])=.5;
  mRadial(shift,1: cubeSize,1)=1/3;

  l2=1;
  l1=numDir;
  mRadialIdx=[(l1-1)*shift+1  l1*shift ;1 cubeSize ; (l2-1)*shift+1  l2*shift+1  ];

  F{c,l}{l2,l1}=PolarToRec(mRadialIdx,mRadial,cubeSize,P{c},PF{c} );
    A=A+F{c,l}{l2,l1};

  mRadial=ones(shift,cubeSize,shift+1,dataClass);
  mRadial( [1 shift],1:cubeSize,1: shift+1)=.5;
  mRadial(1: shift,1: cubeSize,[1 shift+1])=.5;

  for l2=2:numDir-1
     l1=numDir;
      mRadialIdx=[(l1-1)*shift+1  l1*shift ;1 cubeSize ; (l2-1)*shift+1  l2*shift+1  ];

      F{c,l}{l2,l1}=PolarToRec(mRadialIdx,mRadial,cubeSize,P{c},PF{c} );
      A=A+F{c,l}{l2,l1};
  end
  mRadial=ones(shift+1,cubeSize,shift+1,dataClass);
  mRadial( [1 shift+1],1:cubeSize,1: shift+1)=.5;
  mRadial(1: shift+1,1: cubeSize,[1 shift+1])=.5;
  for l2=2:numDir-1
     l1=1;
      mRadialIdx=[(l1-1)*shift+1  l1*shift+1 ;1 cubeSize ; (l2-1)*shift+1  l2*shift+1  ];

      F{c,l}{l2,l1}=PolarToRec(mRadialIdx,mRadial,cubeSize,P{c},PF{c} );
      A=A+F{c,l}{l2,l1};
   end

  mRadial=ones(shift+1,cubeSize,shift,dataClass);
  mRadial( [1 shift+1],1:cubeSize,1: shift)=.5;
  mRadial(1: shift+1,1: cubeSize,[1 shift])=.5;
  mRadial(1,1: cubeSize,shift)=1/3;

  l1=1;
  l2=numDir;
  mRadialIdx=[(l1-1)*shift+1  l1*shift+1 ;1 cubeSize ; (l2-1)*shift+1  l2*shift ];

  F{c,l}{l2,l1}=PolarToRec(mRadialIdx,mRadial,cubeSize,P{c},PF{c} );
  A=A+F{c,l}{l2,l1};

  mRadial=ones(shift+1,cubeSize,shift,dataClass);
  mRadial( [1 shift+1],1:cubeSize,1: shift)=.5;
  mRadial(1: shift+1,1: cubeSize,[1 shift])=.5;

  for l1=2:numDir-1
    l2=numDir;
    mRadialIdx=[(l1-1)*shift+1  l1*shift+1 ;1 cubeSize ; (l2-1)*shift+1  l2*shift ];

    F{c,l}{l2,l1}=PolarToRec(mRadialIdx,mRadial,cubeSize,P{c},PF{c} );
    A=A+F{c,l}{l2,l1};
  end
  mRadial=ones(shift+1,cubeSize,shift+1,dataClass);
  mRadial( [1 shift+1],1:cubeSize,1: shift+1)=.5;
  mRadial(1: shift+1,1: cubeSize,[1 shift+1])=.5;

  for l1=2:numDir-1
  l2=1;
     mRadialIdx=[(l1-1)*shift+1  l1*shift+1 ;1 cubeSize ; (l2-1)*shift+1  l2*shift+1 ];

      F{c,l}{l2,l1}=PolarToRec(mRadialIdx,mRadial,cubeSize,P{c},PF{c} );
      A=A+F{c,l}{l2,l1};
  end

  mRadial=ones(shift,cubeSize,shift,dataClass);
  mRadial( [1 shift],1:cubeSize,1: shift)=.5;
  mRadial(1: shift,1: cubeSize,[1 shift])=.5;
  mRadial( shift,1: cubeSize,shift)=1/3;
  l1=numDir;
  l2=numDir;
  mRadialIdx=[(l1-1)*shift+1  l1*shift ;1 cubeSize ; (l2-1)*shift+1  l2*shift ];

  F{c,l}{l2,l1}=PolarToRec(mRadialIdx,mRadial,cubeSize,P{c},PF{c} );
  A=A+F{c,l}{l2,l1};
end

for c=1:3
   for l2=1:numDir
     for l1=1:numDir
       F{c,l}{l2,l1}=F{c,l}{l2,l1}./A;
        F{c,l}{l2,l1}=squeeze(real(fftshift(ifftn(fftshift(F{c,l}{l2,l1})))));
     end
   end
end
end
Temp=F(1,:);
F(1,:)=F(2,:);
F(2,:)=Temp;


% op_combinesubspecs.m
% Jamie Near, McGill University 2014.
%
% USAGE:
% out=op_combinesubspecs(in,mode);
%
% DESCRIPTION:
% Combine the subspectra in an acquisition either by addition or
% subtraction.
%
% INPUTS:
% in        = input data in matlab structure format.
% mode      = -"diff" adds the subspectra together (this is counter intuitive,
%             but the reason is that many "difference editing" sequences use phase
%             cycling of the readout ADC to achieve "subtraction by addition".
%             -"summ" performs a subtraction of the subspectra.
%
% OUTPUTS:
% out       = Output following combination of subspectra.

function out=op_combinesubspecs(in,mode);

if in.flags.subtracted
    error('ERROR:  Subspectra have already been combined!  Aborting!');
end
if in.flags.isFourSteps
    error('ERROR: data with four steps must first be converted using op_fourStepCombine.m!  Aborting!');
end

% if ~in.flags.freqcorrected
%     disp('WARNING:  Frequency correction has not yet been performed!');
% end
% if ~in.flags.phasecorrected
%     disp('WARNING:  Phase correction has not yet been performed!');
% end

if mode=='diff'
    %add the spectrum along the subSpecs dimension;
    fids=sum(in.fids,in.dims.subSpecs);
    fids=fids/in.sz(in.dims.subSpecs); %divide by number of subspecs so that this is an averaging operation;
elseif mode=='summ'
    %subtract the spectrum along the subSpecs dimension;
    fids=diff(in.fids,1,in.dims.subSpecs);
    fids=fids/in.sz(in.dims.subSpecs); %divide by nymber of subspecs so that this is an averaging operation;
end

fids=squeeze(fids);

%re-calculate Specs using fft
specs=fftshift(ifft(fids,[],in.dims.t),in.dims.t);

%change the dims variables
if in.dims.t>in.dims.subSpecs
    dims.t=in.dims.t-1;
else
    dims.t=in.dims.t;
end
if in.dims.coils>in.dims.subSpecs
    dims.coils=in.dims.coils-1;
else
    dims.coils=in.dims.coils;
end
if in.dims.averages>in.dims.subSpecs
    dims.averages=in.dims.averages-1;
else
    dims.averages=in.dims.averages;
end
dims.subSpecs=0;
if in.dims.extras>in.dims.subSpecs
    dims.extras=in.dims.extras-1;
else
    dims.extras=in.dims.extras;
end

%re-calculate the sz variable
sz=size(fids);

%FILLING IN DATA STRUCTURE
out=in;
out.fids=fids;
out.specs=specs;
out.sz=sz;
out.dims=dims;
out.subspecs=1;
out.averages=in.averages/2;

%FILLING IN THE FLAGS
out.flags=in.flags;
out.flags.writtentostruct=1;
out.flags.subtracted=1;


% 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 Y = utl_picktimes(X,wnd)
% Average the input array within the given index ranges, for each row and plane.
% Result = utl_picktimes(Data, Windows)
%
% In:
%   Data    : [Channels x Samples x Trials] data array
%   Windows : [Ranges x 2] array specifying the beginning and end of each range, in samples
%
% Out:
%   Result  : averaged sub-ranges of the original data array; sized [Channels x Ranges x Trials]
%
% Examples:
%   % for a given 3d data array, average intervals of 20:30 samples, 50:100 samples, and 100:200 samples
%   % for each epoch and each channel
%   averages = utl_picktimes(EEG.data,[20 30;50 100;100 200])
%
% See also:
%   set_picktimes
%
%                               Christian Kothe, Swartz Center for Computational Neuroscience, UCSD
%                               2010-04-20

[C,S,T] = size(X);

if any(wnd(:,1) > wnd(:,2))
    error('Some of the windows are malformed: %s',hlp_tostring(wnd(wnd(:,1) > wnd(:,2),:))); end
if any(wnd(:,1)>S | wnd(:,2)<1)
    error('Some windows are entirely outside the data dimensions (%i): %s',S,hlp_tostring(wnd(wnd(1,:)>S|wnd(2,:)<1,:))); end
if any(wnd(:)>S | wnd(:)<1)
    disp_once('WARNING: some windows passed to utl_picktimes exceed the data dimensions; windows trimmed.'); end

% fix the windows, if necessary
wnd = min(max(wnd,1),S);
wnd(:,2) = wnd(:,2)+1;
W = size(wnd,1);

% calc offset & coverage values for first & last sample
fo = floor(wnd(:,1));
fc = min(fo+1,wnd(:,2)) - wnd(:,1);
lo = ceil(wnd(:,2)-1);
lc = wnd(:,2) - max(lo,wnd(:,1));
% calc fully overlapped sample range, inverse length
for r=1:W
    full{r} = fo(r)+1:lo(r)-1; end %#ok<AGROW>
ilen = 1./(fc+cellfun('length',full)'+lc);

Y = zeros(C,W,T);
% accumulate for every range...
for r=1:W
    Y(:,r,:) = (X(:,fo(r),:)*fc(r)+sum(X(:,full{r},:),2)+X(:,lo(r),:)*lc(r))*ilen(r); end


function handles = select_plane_from_point_cloud(handles,preview_filenames,ind_preview)

fig = figure;
dcm_obj = datacursormode(fig);

h = uicontrol('Position',[20 20 100 40],'String','Continue',...
              'Callback',@continue_callback);
h1 = uicontrol('Position',[220 20 100 40],'String','back',...
              'Enable','off','Callback',@back_callback);

for i=1:numel(preview_filenames)
    disp(preview_filenames{i});
    done = 0;
    pc = import_point_cloud_from_file(preview_filenames{i});
    pcshow(pc);
    title(preview_filenames{i}, 'interpreter', 'none');

    uiwait(gcf);

end
close(gcf);

function continue_callback(objectHandle , eventData )
    if done == 0
        cur_info = getCursorInfo(dcm_obj);
        if isfield(cur_info,'Position')
            arrayPos=find(handles.active_image_numbers==ind_preview(i));

            [theta, alpha, minError, inliers] = ObtainPlanePoint(pc.Location,cur_info.Position);
%             [theta,alpha,minError] = tls_robust(inliers);
            handles.user_selected_planes{arrayPos}.theta = theta;
            handles.user_selected_planes{arrayPos}.alpha = alpha;
            handles.user_selected_planes{arrayPos}.e = minError;
            handles.user_selected_planes{arrayPos}.inliers = inliers;

            pcshow(pointCloud(inliers));
            h1.Enable = 'on';
            done = 1;
        end
    else
        h1.Enable = 'off';
        uiresume(gcbf);
    end

end
function back_callback(objectHandle , eventData )

    pcshow(pc);
    done = 0;
    objectHandle.Enable = 'off';
end

end


function linplus_test25 ( )

%*****************************************************************************80
%
%% TEST25 tests R8GB_PRINT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  m = 8;
  n = 10;
  ml = 1;
  mu = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST25\n' );
  fprintf ( 1, '  For a general banded matrix,\n' );
  fprintf ( 1, '  R8GB_PRINT prints the matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix rows M =      %d\n', m );
  fprintf ( 1, '  Matrix columns N =   %d\n', n );
  fprintf ( 1, '  Lower bandwidth ML = %d\n', ml );
  fprintf ( 1, '  Upper bandwidth MU = %d\n', mu );

  a = r8gb_indicator ( m, n, ml, mu );

  r8gb_print ( m, n, ml, mu, a, '  The R8GB matrix:' );

  return
end