% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

clear; clc;
%% Parameters
S = 100;                                   % Spot prices
T = 3;      % maturities

r = 0;                      % discount factors
d = 0;                              % dividends

titS = 'MC Bates - Asset';
titV =' MC Bates - Variance';

legend_base = 'Base scenario';

%% Specify the base model
vInst = 0.04;                  % instantanuous variance of base parameter set
vLong = 0.04;                  % long term variance of base parameter set
kappa = 0.2;                   % mean reversion speed of variance of base parameter set
omega = 0.1;                   % volatility of variance of base parameter set
rho = 0;                       % correlation of base parameter set
sigj = 0.25;
muj = 0.2;
lambda = 0.5;

%% Simulation parameters
NTime = 750; NSim = 1; NBatches = 1;
K = ones(NTime+1,1);
K = T*cumsum(K)/(NTime+1);
%rand('seed', 1);
UV = rand(1,2*NTime);              % precompute all randoms
K1 = K(2:end);

rstream = RandStream('mt19937ar','Seed',12345);
rstreamstate = rstream.State;

[PathS, PathV] = MC_QE_j(S,r,d,T,vInst,vLong,...
    kappa,omega,rho,muj,sigj,lambda,NTime,NSim,NBatches);
%MC_Bates_path(S,r,d,T,vInst,vLong,kappa,omega,rho,muj,sigj,lambda,UV);
%% Changing vInst
muj_low = 0.05;
muj_high = 0.5;

    PathS_low  = MC_QE_j(S,r,d,T,vInst,vLong,kappa,omega,rho,muj_low,sigj,lambda,NTime,NSim,NBatches);
    PathS_high = MC_QE_j(S,r,d,T,vInst,vLong,kappa,omega,rho,muj_high,sigj,lambda,NTime,NSim,NBatches);

legend_low = 'Changing \mu_j low';
legend_high = 'Changing \mu_j high';

createfigure_path(K,PathS_low, PathS, PathS_high, titS, legend_low, legend_base, legend_high);
%% Changing vLong
sigj_low = 0.1;
sigj_high = 0.5;

    PathS_low = MC_QE_j(S,r,d,T,vInst,vLong,kappa,omega,rho,muj,sigj_low,lambda,NTime,NSim,NBatches);
    PathS_high = MC_QE_j(S,r,d,T,vInst,vLong,kappa,omega,rho,muj,sigj_high,lambda,NTime,NSim,NBatches);

    legend_low  = 'Changing \sigma_j low';
    legend_high = 'Changing \sigma_j high';

createfigure_path(K,PathS_low, PathS, PathS_high, titS, legend_low, legend_base, legend_high);

%% Changing kappa
lambda_low = 0.2;
lambda_high = 0.8;

    PathS_low = MC_QE_j(S,r,d,T,vInst,vLong,kappa,omega,rho,muj,sigj,lambda_low,NTime,NSim,NBatches);
    PathS_high = MC_QE_j(S,r,d,T,vInst,vLong,kappa,omega,rho,muj,sigj,lambda_high,NTime,NSim,NBatches);

legend_low  = 'Changing \lambda low';
legend_high = 'Changing \lambda high';

createfigure_path(K,PathS_low, PathS, PathS_high, titS, legend_low, legend_base, legend_high);


function test_libphaseret_overlaynthframe
f = greasy;
a = 128;
M = 1024;
M2 = floor(M/2) + 1;
gl = 1024;
L = dgtlength(numel(f),a,M);
g = firwin('hann',gl);
gamma = 0.25645*gl^2;
gd = long2fir(gabdual(g,a,M),gl);
gshift = fftshift(g);
N = 10;
idx = 9;
g2 = repmat(g.*gd,1,N);
g1 = repmat(fftshift(g.*gd),1,N);
cout = zeros(gl,1);
coutPtr = libpointer('doublePtr',cout);

calllib('libphaseret','phaseret_overlaynthframe_d',g1,gl,N,a,idx,coutPtr);

[~,out2] = comp_overlayframes(g2,a,gl,idx);
[~,out3orig] = overlayframes(g2,a,gl,idx);
out1orig = M*coutPtr.Value;
out3orig = out3orig*M;

out1 = out1orig;
out3 = out3orig;
out1(out1orig==0) = 1;
out1(out1<1e-6) = 1e-6;
out3(out3orig==0) = 1;
out3(out3<1e-6) = 1e-6;

figure(1); plot([gshift./out1,gshift./out3])

function [partrec,frame] = overlayframes(cframes,a,M,n)

N = size(cframes,2);
bufLen = N*a - (a-1) + M-1;
partrec = zeros(bufLen,1);

startidx = ceil(M/2)-1;
idxrange = startidx + [0:floor(M/2),-ceil(M/2)+1:-1];
for ii=0:N-1
    idx = ii*a + idxrange + 1;
    partrec(idx) = partrec(idx) + cframes(:,ii+1);
end

frame = partrec(a*n+1:a*n + M);


function [success, C] = cornerDetection(I,saveFileName)
% function C = cornerDetection(moviefile,frameRange,showResult)
% Detects corners
% frameRange is the range of the frames to be used to detect corners.
% Show results shows the result of corner detection.

% [readfcn,nframes,fid,~] = get_readframe_fcn(...
%   moviefile);
% %% Read a frame.
%
% if isempty(frameRange),
%   frameRange = 1:nframes;
% end
%
% rfr = randsample(frameRange,1);
% I = readfcn(rfr);

%%
CM = cornermetric(I(:,:,1));
bw = CM>0.000005;
bw([1:10 end-10:end],:) = 0;
bw(:,[1:10 end-10:end]) = 0;
bwl = bwlabel(bw);
bwcenter = regionprops(bwl,'Centroid','Area');
small2rem = find([bwcenter.Area]<3);

xc = []; yc = [];
for ndx = 1:numel(bwcenter)
xc(ndx) = bwcenter(ndx).Centroid(1);
yc(ndx) = bwcenter(ndx).Centroid(2);
end

dd = zeros(numel(xc),numel(xc));
for ndx = 1:numel(xc)
  dd(ndx,:) = sqrt( (xc - xc(ndx)).^2 + (yc-yc(ndx)).^2);
end

Isz = size(I,1);

success = false;
selidx = [];
for tol = 1:0.5:7
  for sz = round(0.7*Isz):(tol/2):round(Isz)
      ff = abs(dd-sz)<tol;
      if nnz(sum(ff)>1) >= 4
        curidx = find(sum(ff)>1);
        allcombs = nchoosek(curidx,4);
        for combn = 1:size(allcombs,1);
          selidx = allcombs(combn,:);
          curm = dd(selidx,selidx);
          if (nnz( abs(curm-sz)<tol) == 8) && (nnz( curm>1.4*sz)==4) ,
            success = true;
            break
          end
        end
        if success, break; end
      end
  end
  if success; break; end

end

if ~success,
  C = [];
  return;
end

topleft = selidx( (xc(selidx)<Isz/2) & (yc(selidx)<Isz/2));
topright = selidx( (xc(selidx)>Isz/2) & (yc(selidx)<Isz/2));
bottomleft = selidx( (xc(selidx)<Isz/2) & (yc(selidx)>Isz/2));
bottomright = selidx( (xc(selidx)>Isz/2) & (yc(selidx)>Isz/2));

if isempty(topleft) || isempty(topright) || isempty(bottomleft) || isempty(bottomright)
 success = false;
 C = [];
 return;
end

%{
xc(small2rem) = size(I,2)/2;
yc(small2rem) = size(I,1)/2;
minx = min(xc);
miny = min(yc);
maxx = max(xc);
maxy = max(yc);

tol = 10;
topleft = find( (abs(xc-minx)<tol) & (abs(yc-miny)<tol));
topright = find( (abs(xc-maxx)<tol) & (abs(yc-miny)<tol));
bottomleft = find( (abs(xc-minx)<tol) & (abs(yc-maxy)<tol));
bottomright = find( (abs(xc-maxx)<tol) & (abs(yc-maxy)<tol));

% C(1,:) = bwcenter(topleft).Centroid;
% C(2,:) = bwcenter(topright).Centroid;
% C(3,:) = bwcenter(bottomleft).Centroid;
% C(4,:) = bwcenter(bottomright).Centroid;

if isempty(topleft) || isempty(topright) || isempty(bottomleft) || isempty(bottomright),
continue;
end

if numel(topleft)>1 || numel(topright) > 1 || numel(bottomleft)>1 || numel(bottomright)>1,
  topleft = topleft(argmax([bwcenter(topleft).Area]));
  topright = topright(argmax([bwcenter(topright).Area]));
  bottomleft = bottomleft(argmax([bwcenter(bottomleft).Area]));
  bottomright = bottomright(argmax([bwcenter(bottomright).Area]));
end
success = true; break;
end
%}

if ~success,
  C = [];
  return;
end

%{
% selImg = (bwl == topleft) | (bwl == bottomleft) | (bwl == topright) | (bwl == bottomright);
%
%
% CM(~selImg) = 0;
% corner_peaks = imregionalmax(CM);
% corner_idx = find(corner_peaks == true);
% [xx yy] = ind2sub(size(I(:,:,1)),corner_idx);
% C = [];
%}
C(:,1) = yc([topleft topright bottomleft bottomright]);
C(:,2) = xc([topleft topright bottomleft bottomright]);

for ndx = 1:4
  I(round(C(ndx,1) + [-1:1]),round(C(ndx,2)+[-1:1]),1) = 256;
  I(round(C(ndx,1) + [-1:1]),round(C(ndx,2)+[-1:1]),2) = 0;
  I(round(C(ndx,1) + [-1:1]),round(C(ndx,2)+[-1:1]),3) = 0;
end
imwrite(I,saveFileName);

%% Detect corners based on square constraint.

%{

%}

%% Detect corners

%{
C = corner(I(:,:,1),'Harris',4,'SensitivityFactor',0.001);

%% Show Corners
imshow(I);
hold on
plot(C(:,1), C(:,2), 'r*');

%% Corner metric

hx = [];
figure;
hx(1) = subplot(1,3,1);
imshow(I);
title('Original Image');
CM = cornermetric(I(:,:,1));

CM_adjusted = imadjust(CM);
hx(2) = subplot(1,3,2);
imshow(CM_adjusted);
title('Corner Metric');

corner_peaks = imregionalmax(CM);
corner_idx = find(corner_peaks == true);
[r g b] = deal(I(:,:,1));
r(corner_idx) = 255;
g(corner_idx) = 255;
b(corner_idx) = 0;
RGB = cat(3,r,g,b);
hx(3) = subplot(1,3,3);
imshow(RGB);
title('Corner Points');
linkaxes(hx);
%}


function U=NTds(U,X,Y,xflux,yflux,N,T,varargin)
%%
%   Performs dimensional splitting on the 2D scalar conservation law
%   U_t + xflux(U)_x + yflux(U)_y = 0 with initial data U.
%   Uses Strang splitting with timestep dt=T/N. The domain is the square
%   spanned by X and Y. varargin may contain 'wbar' if waitbar is desired,
%   'periodic' followd by [xmin xmax], [ymin ymax]. The one-dimensional
%   equations are solved using a second order central scheme.
%

%%
% Initial setup
addpath ../../AppendixA;
wb=0;
nv=length(varargin);
i=1;
boundary_cond='neumann';
while i<=nv,
	if strcmp(varargin(i),'wbar'),
		wb=1;
		i=i+1;
	elseif strcmp(varargin(i),'periodic'),
		boundary_cond='periodic';
		i=i+1;
	else
      display('Unknown option in NTds');
      i=i+1;
	end;
end;
dt=T/N;
xdir=2; ydir=1;
dx=X(2)-X(1);
dy=Y(2)-Y(1);
CFL=0.5;
limiter='superbee';
strang=dt*0.5;

%%
% The main loop
if wb
  wstr=sprintf('Computing %d steps.',N);
  wbar=waitbar(0,wstr);
end;
for k=1:N,
  U=central(xflux,U,strang,dx,xdir,boundary_cond,CFL,limiter);
  if wb,
    waitbar((k-0.5)/N,wbar);
  end;
  if (k==1),
    strang=dt;
  end;
  U=central(yflux,U,strang,dy,ydir,boundary_cond,CFL,limiter);
  if k==N,
    strang=0.5*dt;
    U=central(xflux,U,strang,dx,xdir,boundary_cond,CFL,limiter);
  end;
  if wb,
    waitbar(k/N,wbar);
  end;
end;

%%
%
if wb,
  close(wbar)
end;
rmpath ../../AppendixA;


function title = p16_title ( )

%*****************************************************************************80
%
%% P16_TITLE returns the title for problem 16.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, string TITLE, the title of the problem.
%
  title = 'cos(pi x / 2 ) / sqrt(x)';

  return
end


function r8poly3_root_test ( )

%*****************************************************************************80
%
%% R8POLY3_ROOT_TEST tests R8POLY3_ROOT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  test_num = 4;

  a_test = [ 1.0, 9.0, 1.0, 1.0 ];
  b_test = [ -6.0, -36.0, -5.0, -8.0  ];
  c_test = [ 11.0, 54.0, 8.0, 25.0  ];
  d_test = [ -6.0, -27.0, -4.0, -26.0  ];
%
%  1: Three distinct real roots, 1, 2, 3.
%  2: One repeated real root, 1.5, 1.5, 1.5.
%  3: Two real roots, one repeated, 1, 2, 2.
%  4: One real root, a complex conjugate pair, 2, 3+2I, 3-2I.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8POLY3_ROOT_TEST\n' );
  fprintf ( 1, '  R8POLY3_ROOT finds roots of cubic equations.\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

    a = a_test(test);
    b = b_test(test);
    c = c_test(test);
    d = d_test(test);

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Polynomial coefficients:\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  A = %f, B = %f, C = %f, D = %f\n', a, b, c, d );

    [ r1, r2, r3 ] = r8poly3_root ( a, b, c, d );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Roots:\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  %f\n', r1 );
    fprintf ( 1, '  %f\n', r2 );
    fprintf ( 1, '  %f\n', r3 );

  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   example 2 DOF planar robot.
%
%   Author: Arturo Gil. Universidad Miguel Hernandez de Elche.
%   email: arturo.gil@umh.es date:   03/03/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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='Example 2DOF planar arm';

%kinematic data DH parameters
robot.DH.theta='[q(1) q(2)]';
robot.DH.d='[0  0]';
robot.DH.a='[1  1]';
robot.DH.alpha='[0  0]';

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

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

%Jacobian matrix. It is easy to obtain the Jacobian matrix of this robot
% J is defined such that:
% v=[vn wn]', where vn is the linear speed of the end effector and wn in
% the angular speed. vn=[vx vy vz] and wn = [wx wy wz]. Being this a planar
% robot, vz=0 and wx=wy=0
robot.J=['[-a(1)*sin(q(1))-a(2)*sin(q(1)+q(2))  -a(2)*sin(q(1)+q(2));' ...
          'a(1)*cos(q(1))+a(2)*cos(q(1)+q(2))   a(2)*cos(q(1)+q(2));' ...
          '               0                                  0;' ...
          '               0                                  0;' ...
          '               0                                  0;' ...
          '               1                                  1]'];
%Function name to compute inverse kinematic
robot.inversekinematic_fn = 'inversekinematic_2dofplanar(robot, T)';
robot.directkinematic_fn = 'directkinematic(robot, q)';

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-180) deg2rad(180); %Axis 1, minimum, maximum
                deg2rad(-180) deg2rad(180)]; %Axis 2, minimum, maximum

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(100);
                deg2rad(100)];

robot.accelmax=robot.velmax/3; % 0.1 is here an acceleration time

% end effectors maximum velocity
robot.linear_velmax = 0.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);
robot.path = pwd;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GRAPHICS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%read graphics files
robot.graphical.has_graphics=1;
robot.graphical.color = [25 20 40];
%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 -2.2 2.2 0 2.2];
robot = read_graphics(robot);

% INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
% position, velocity and acceleration
robot.q=[0 0]';
robot.qd=[0 0]';
robot.qdd=[0 0]';
robot.time = [];

robot.q_vector=[];
robot.qd_vector=[];
robot.qdd_vector=[];

robot.last_target=directkinematic(robot, robot.q);
robot.last_zone_data = 'fine';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DYNAMIC PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
robot.has_dynamics=1;

%consider friction or not
robot.dynamics.friction=0;

%link masses (kg)
robot.dynamics.masses=[2 2];

%COM of each link with respect to own reference system
robot.dynamics.r_com=[-0.5      0         0; %(rx, ry, rz) link 1, w/r to reference system 1
                      -0.5      0         0];%(rx, ry, rz) link 2

%link masses
m1 = robot.dynamics.masses(1);
m2 = robot.dynamics.masses(2);

%eval a to obtain parameters.
a=eval(robot.DH.a);
L1 = a(1);
L2 = a(2);

%Inertia matrices of each link
% Ixx	Iyy	Izz	Ixy	Iyz	Ixz, or each row
robot.dynamics.Inertia=[0   m1*L1^2/12   m1*L1^2/12    0	0	0;
                        0   m2*L2^2/12   m2*L2^2/12    0	0	0];

%Inertia of the rotor
robot.motors.Inertia=[0 0];
%Reduction ratio: motor_speed/joint speed
robot.motors.G=[10  10];

%Viscous friction factor of the motor
robot.motors.Viscous = [0.9  0.9];
%Coulomb friction of the motor
%Tc+, Tc-
robot.motors.Coulomb = [0	0;
                        0	0];

%Obtained from motor catalog under practicals/inverse_dynamics
%                        R(Ohm)  L(H)      Kv (V/rad/s):speed constant     Kp (Nm/A):torque constant        Max_current (A)
robot.motors.constants=[0.345  0.273e-3       2.3474e-05               84.9e-3                 139];%these correspond to Maxon, 167132;


function h = bc_test02 ( x_num, x, t, h )

%*****************************************************************************80
%
%% BC_TEST02 evaluates the boundary conditions for problem 2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer X_NUM, the number of nodes.
%
%    Input, real X(X_NUM,1), the node coordinates.
%
%    Input, real T, the current time.
%
%    Input, real H(X_NUM,1), the current heat values.
%
%    Output, real H(X_NUM,1), the current heat values, after boundary
%    conditions have been imposed.
%
  h(1,1)     = exact_test02 ( 1, x(1),     t );
  h(x_num,1) = exact_test02 ( 1, x(x_num), t );

  return
end


function x = blend_rs_1dn ( r, s, n, bound_rs )

%*****************************************************************************80
%
%% BLEND_RS_1DN extends vector data along sides into a square.
%
%  Diagram:
%
%    01-----r1-----11
%     |      .      |
%     |      .      |
%    0s.....rs.....1s
%     |      .      |
%     |      .      |
%    00-----r0-----10
%
%  Discussion:
%
%    BLEND_RS_1DN is NOT equivalent to a bilinear finite element method,
%    since the data is sampled everywhere along the boundary lines,
%    rather than at a finite number of nodes.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    William Gordon,
%    Blending-Function Methods of Bivariate and Multivariate Interpolation
%    and Approximation,
%    SIAM Journal on Numerical Analysis,
%    Volume 8, Number 1, March 1971, pages 158-177.
%
%    William Gordon and Charles Hall,
%    Transfinite Element Methods: Blending-Function Interpolation over
%    Arbitrary Curved Element Domains,
%    Numerische Mathematik,
%    Volume 21, Number 1, 1973, pages 109-129.
%
%    William Gordon and Charles Hall,
%    Construction of Curvilinear Coordinate Systems and Application to
%    Mesh Generation,
%    International Journal of Numerical Methods in Engineering,
%    Volume 7, 1973, pages 461-477.
%
%    Joe Thompson, Bharat Soni, Nigel Weatherill,
%    Handbook of Grid Generation,
%    CRC Press, 1999.
%
%  Parameters:
%
%    Input, real R, S, the (R,S) coordinates of the point to be
%    evaluated.
%
%    Input, integer N, the dimension of the vector space.
%
%    External, BOUND_RS, is a function which is given (R,S)
%    coordinates and an component value I, and returns XI, the value
%    of the I-th component of the N-vector at that point.  BOUND_RS
%    will only be called for "sides", that is, for values (R,S) where
%    at least one of R and S is either 0.0 or 1.0.  BOUND_RS has the
%    form:
%      function xi = bound_rs ( r, s, i )
%
%    Output, real X(N), the interpolated value at the point (R,S).
%
  for i = 1 : n
%
%  Get the I-th coordinate component at the four corners.
%
    x00 = bound_rs ( 0.0, 0.0, i );
    x01 = bound_rs ( 0.0, 1.0, i );
    x10 = bound_rs ( 1.0, 0.0, i );
    x11 = bound_rs ( 1.0, 1.0, i );
%
%  Get the I-th coordinate component at the sides.
%
    xr0 = bound_rs ( r, 0.0, i );
    xr1 = bound_rs ( r, 1.0, i );
    x0s = bound_rs ( 0.0, s, i );
    x1s = bound_rs ( 1.0, s, i );
%
%  Interpolate the I-th coordinate component of the interior point.
%
    x(i) = blend_112 ( r, s, x00, x01, x10, x11, xr0, xr1, x0s, x1s );

  end

  return
end


function [x,P]=onePointCartInit(zCart,SRCart,higherDerivStdDev,matType)
%%ONEPOINTCARTINIT This function implements single-point initialization for
%              target states that consist of components of position,
%              velocity, acceleration, etc. This function initializes
%              tracks from a single measurement by setting the position
%              components to Cartesian measurement value with its
%              associated covariance and then setting the diagonal elements
%              of the rest of the components based on fixed standard
%              deviations. For example, in [1] it is suggested to make the
%              standard deviation for velocity vMax/3 and in Chapter 3.2.2
%              of [2], vMax/2. Similar ad-hoc values could be used for
%              higher moments. This function does not use Doppler/ range
%              rate.
%
%INPUTS: zCart A zDimXnumMeas set of Cartesian measurements for which
%              single-point differencing should be used to start tracks.
%       SRCart If matType is omitted or is 0, then this is a
%              zDimXzDimXnumMeas set of lower-triangular square root
%              covariance matrices associated with the measurements in
%              zCart. If all of the matrices are the same, then a single
%              zDimXzDim matrix can be passed. If matType=1, then this is a
%              set of covariance matrices.
% higherDerivStdDev A numMomentsX1 or 1XnumMoments vector containing the
%              standard deviations to use for each of the moments
%              (position, velocity, etc) that cannot be estimated from the
%              data. As mentioned in [1] and in and in Chapter 3.2.2
%              of [2], for velocity, this might be vMax/sqrt(2) or
%              vMax/sqrt(3).
%      matType An optional input specifying whether SRCart is a set of
%              lower-triangular square roots of the covariance matrix, or
%              whether it is the set of covariance matrices. Possible
%              values are:
%              0 (The default if omitted or an empty matrix is passed)
%                SRCart holds lower-triangular square root covariance
%                matrices.
%              1 SRCart holds covariance matrices.
%
%OUTPUTS: x The xDimXnumMeas set of target state estimates. All
%           non-position components are zero. xDim=zDim*(numMoments+1). The
%           components are arranged position, velocity, acceleration, etc.
%           For example, [x;y;z;xDot;yDot;zDot].
%         P The xDimXxDimXnumMeas set of initial target state covariance
%           matrices associated with x.
%
%One-point differencing is discussed in [1] and Chapter 3.2.2 of [2].
%
%REFERENCES:
%[1] M. Mallick and B. La Scala, "Comparison of single-point and two-point
%    difference track initiation algorithms using position measurements".
%    Acta Automatica Sinica, vol.34, no. 3, pp 258-265, Mar. 2008.
%[2] Y. Bar-Shalom, P. K. Willett, and X. Tian, Tracking and Data Fusion.
%    Storrs, CT: YBS Publishing, 2011.
%
%November 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(matType))
    matType=0;
end

zDim=size(zCart,1);
numMeas=size(zCart,2);

if(size(SRCart,3)==1)
    SRCart=repmat(SRCart,1,1,numMeas);
end

numMoments=length(higherDerivStdDev);
xDim=zDim*(numMoments+1);

x=zeros(xDim,numMeas);
P=zeros(xDim,xDim,numMeas);

x(1:zDim,:)=zCart;
switch(matType)
    case 0
        for curMeas=1:numMeas
            P(1:zDim,1:zDim,curMeas)=SRCart(:,:,curMeas)*SRCart(:,:,curMeas)';
        end
    case 1
        for curMeas=1:numMeas
            P(1:zDim,1:zDim,curMeas)=SRCart(:,:,curMeas);
        end
    otherwise
        error('Unknown matrix type specified.')
end

sel=(zDim+1):xDim;
P(sel,sel,:)=repmat(kron(diag(higherDerivStdDev(:).^2),eye(zDim,zDim)),1,1,numMeas);
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function fem2d_pack_test21 ( )

%*****************************************************************************80
%
%% TEST21 tests SPHERE_GRID_Q16.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  element_order = 16;
  nelemx = 2;
  nelemy = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST21\n' );
  fprintf ( 1, '  SPHERE_GRID_Q16_ELEMENT sets up a grid of\n' );
  fprintf ( 1, '    Q16 quadrilaterals on a sphere.\n' );
  fprintf ( 1, '  SPHERE_GRID_Q16_ELEMENT_NUM returns the number\n' );
  fprintf ( 1, '    of elements in the grid\n' );
  fprintf ( 1, '  SPHERE_GRID_Q16_NODE_NUM returns the number\n' );
  fprintf ( 1, '    of nodes in the grid.\n' );
  fprintf ( 1, '  SPHERE_GRID_Q16_NODE_XYZ returns the coordinates\n' );
  fprintf ( 1, '    of nodes in the grid.\n' );

  element_num = sphere_grid_q16_element_num ( nelemx, nelemy );
  node_num = sphere_grid_q16_node_num ( nelemx, nelemy );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Expected number of nodes =    %d\n', node_num );
  fprintf ( 1, '  Expected number of elements = %d\n', element_num );

  element_node = sphere_grid_q16_element ( nelemx, nelemy );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The elements and their nodes, listed in a way\n' );
  fprintf ( 1, '  that suggests their geometry:\n' );
  fprintf ( 1, '\n' );

  element = element_num;

  for j = 1 : nelemy
    for i = 1 : nelemx
      fprintf ( 1, '\n' );
      fprintf ( 1, '%4d  %4d%4d%4d%4d\n', element, element_node(13:16,element) );
      fprintf ( 1, '      %4d%4d%4d%4d\n',         element_node(9:12,element) );
      fprintf ( 1, '      %4d%4d%4d%4d\n',         element_node(5:8,element) );
      fprintf ( 1, '      %4d%4d%4d%4d\n',         element_node(1:4,element) );
      element = element - 1;
    end
  end

  node_xyz = sphere_grid_q16_node_xyz ( nelemx, nelemy );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The node coordinates:\n' );
  fprintf ( 1, '\n' );

  for node = 1 : node_num
    fprintf ( 1, '  %4d  %12f  %12f  %12f\n', node, node_xyz(1:3,node) );
  end
%
%  Write the elements and nodes to files.
%
  r8mat_write ( 'sphere_q16_nodes.txt', 3, node_num, node_xyz );

  i4mat_write ( 'sphere_q16_elements.txt', element_order, element_num, ...
    element_node );

  return
end


function VecInd  = bst_closest(VecGuess, VecRef)
% BST_CLOSEST: Find entries of closest elements between two vectors.
%
% USAGE:  VecInd  = bst_closest(VecGuess, VecRef);
%
% DESCRIPTION:
%     VecGuess is a vector for which one wants to find the closest entries in vector VecRef
%     VecInd is the vector of indices pointing atr the entries in vector VecRef that are the closest to VecWin
%     VecInd is of the length of VecGuess
%
%     In other words, VecRef(VecInd(i)) is the element of VecRef closest to VecGuess(j)
%
%     VecRef and VecGuess do not need to be the same length

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

if size(VecRef,1) == 1
    VecRef = VecRef';
end

tmp = repmat(VecRef,1,length(VecGuess));
[minn VecInd] = min(abs(repmat(VecGuess,length(VecRef),1) - tmp));


function output = knn_matting (input, trimap, lambda, level)

% [10;2] means 10 neighbors with default(level) spatial coherence and
% 2 neighbors with weak spatial coherence.
nn = [10; 2];
[m,n,d] = size(input);

foreground = trimap > 0.99;
background = trimap < 0.01;
all_constraints = foreground + background;

% the first part of the feature vector is the rgb or other color information,
% the second part is the spatial factor perturbed by a small amount,
% in the for loop below, the second part will be reduced really nonlocally
[a, b] = ind2sub([m n],1:m*n);
feature_vector = [ reshape(input,m*n,d)';[a;b]/sqrt(m*m+n*n)*level+rand(2,m*n)*1e-6];

now=0;
for i=1:size(nn,1)
    kdtree = vl_kdtreebuild(feature_vector);
    ind = vl_kdtreequery(kdtree, feature_vector,feature_vector,'NUMNEIGHBORS',nn(i),'MAXNUMCOMPARISONS',nn(i)*3);
    index1 = reshape(repmat(uint32(1:m*n),nn(i),1),[],1);
    index2 = reshape(ind,[],1);
    row(now+1:now+m*n*nn(i),:) = [min(index1, index2), max(index1, index2)];
    feature_vector(d+1:d+2,:) = feature_vector(d+1:d+2,:)/100;
    now = now+m*n*nn(i);
end

value = max(1-sum(abs(feature_vector(1:d+2,row(:,1))-feature_vector(1:d+2,row(:,2))))/(d+2),0);
A = sparse(double(row(:,1)), double(row(:,2)), value, m*n, m*n);
A = A + A';
D = spdiags(sum(A,2), 0, n*m, n*m);
L = D - A;
H = L +lambda*spdiags(all_constraints, 0, m*n, m*n);
iH = ichol(H);
x = pcg(H, lambda*foreground, [], 2000, iH, iH');

output = reshape(x,m,n);

end


function [indexes]=train_test_random_new(y,n,nall)
% function to ramdonly select training samples and testing samples from the
% whole set of ground truth.
% alltrain is the ground truth
% % clc
% alltrain = alltrain';
% y = alltrain(2,:); % Indiana
% y = alltrain(1,:); % Salinas
% yindex = [];
% train = [];
% lys = 0;
K = max(y);
% pK = 0.0001,

% generate the  training set
indexes = [];
for i = 1:K
    index1 = find(y == i);
    per_index1 = randperm(length(index1));
    if length(index1)>n
        indexes = [indexes ;index1(per_index1(1:n))'];
    else
        indexes = [indexes ;index1(per_index1(1:round(length(index1)/2)))'];
    end
end
indexes = indexes(:);
indexes_all = [1:length(y)];
indexes_all(indexes) = [];
n_new = nall - length(indexes);
per_indexall = randperm(length(indexes_all));
indexes_new = indexes_all(per_indexall(1:n_new));
indexes = [indexes;indexes_new'];
indexes = indexes(:);

% indexes = indexes';
% train = y(indexes);
% y(indexes) = [];
% test = y;

% for k_iter = 1:K
%     index_k = y == k_iter;
%     index_k = find(index_k);
%     if length(index_k) > n
%         index_k_random =  ceil(length(index_k).*rand(n,1));
%         index_k_random = sort(index_k_random);
%         index_k_random1 = [index_k_random(2:n);index_k_random(1)];
%         resid = index_k_random1 - index_k_random;
%         resid0 = resid == 0;
%         index_k_random(resid0) = [];
%         train_k = alltrain(:,index_k(index_k_random));
%         yindex = [yindex,index_k(index_k_random)];
%     else
%         n1 = ceil(length(index_k)/2);
%         index_k_random =  ceil(length(index_k).*rand(n1,1));
%         index_k_random = sort(index_k_random);
%         index_k_random1 = [index_k_random(2:n1);index_k_random(1)];
%         resid = index_k_random1 - index_k_random;
%         resid0 = resid == 0;
%         index_k_random(resid0) = [];
%         train_k = alltrain(:,index_k(index_k_random));
%         yindex = [yindex,index_k(index_k_random)];
%     end
%     train = [train,train_k];
% end
%
% trainold = alltrain;
% alltrain(:,yindex) = [];
% test = alltrain;
% for i = 1:K
%     ly = length(find(y==i));
%     nf = ceil(pK*ly);
%     f = lys + ceil(ly.*rand(nf,1));
%     f = sort(f);
%     findex = [];
%     for j = 1:(length(f)-1)
%         if f(j) == f(j+1)
%             findex = [findex,j];
%         end
%     end
%     f(findex) = [];
%     train10 = alltrain(:,f);
%     yindex = [yindex,f'];
%     train = [train,train10];
%     lys = lys + ly;
% end
%
% alltrain(:,yindex) = [];
% test = alltrain;


function folds = create_folds(N, k_folds)

prev_rng = seed_rand();

perm = randperm(N);
fold_points = floor(linspace(1, N, k_folds+1));
fold_points(end) = N+1;
folds = cell(k_folds, 1);
for i = 1:k_folds
  folds{i} = perm(fold_points(i):fold_points(i+1)-1);
end
assert(isempty(setdiff(1:N, cat(2, folds{:}))));

rng(prev_rng);


function X = model_2_data(N, K, S)
    % Data as per model 2 in eldar2010average paper
    gen  = spx.data.synthetic.SparseSignalGenerator(N, K, S);
    X = gen.complex_gaussian;
end


function geometry_test03323 ( )

%*****************************************************************************80
%
%% TEST0323 tests I4COL_FIND_PAIR_WRAP.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  m = 5;
  n = 4;
  test_num = 5;

  item1_test = [ 22, 32, 22, 54, 54 ];
  item2_test = [ 32, 22, 23, 14, 11 ];

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0323\n' );
  fprintf ( 1, '  I4COL_FIND_PAIR_WRAP finds the first occurrence of\n' );
  fprintf ( 1, '  a pair of item in an I4COL.\n' );
  fprintf ( 1, '  Items in the array are ordered by column, and\n' );
  fprintf ( 1, '  wraparound is allowed.\n' );

  for i = 1 : m
    for j = 1 : n
      a(i,j) = 10 * i + j;
    end
  end

  i4mat_print ( m, n, a, '  The matrix of columns:' );

  for test = 1 : test_num

    item1 = item1_test(test);
    item2 = item2_test(test);

    [ row, col ] = i4col_find_pair_wrap ( m, n, a, item1, item2 );

    fprintf ( 1, ...
      '  Item %d followed by item %d occurs in row %d and column %d\n', ...
      item1, item2, row, col );

  end

  return
end


% M = MEAN2(MTX)
%
% Sample mean of a matrix.

function res = mean2(mtx)

res = mean(mean(mtx));


function [detvtx,pth] = SStat_mass_FDR2(pval,maskvtx,rate)
% [detvtx,pth] = SStat_mass_FDR2(pval,maskvtx,rate)
%
% Two-stage FDR approach to achieve tighter control of the FDR. This
% procedure is more powerful than the original FDR procedure implemented in
% SStat_mass_FDR.
%
% Input
% pval: P-values.
% maskvtx: Mask's vertices (1-based). Default [] (all vertices included).
% rate: Expected FDR (between 0 and 1).
%
% Output
% detvtx: Detected vertices (1-based).
% pth: FDR threshold.
%
% $Revision: 1.1 $  $Date: 2015/01/06 17:03:59 $
% Original Author: Jorge Luis Bernal Rusiel
% CVS Revision Info:
%    $Author: mreuter $
%    $Date: 2015/01/06 17:03:59 $
%    $Revision: 1.1 $
% References: Benjamini, Y., Krieger, A.M., Yekutieli, D. (2006). Adaptive
% linear step-up procedures that control the false discovery rate.
% Biometrika, 93, 491-507.
%
if nargin < 3
    rate = 0.05;
    if nargin < 2
        maskvtx = [];
    end
end;
nv0 = length(pval);
if isempty(maskvtx)
   maskvtx = 1:nv0;
end;
p = pval(maskvtx);
nv = length(p);
%% First stage (m0 estimation)
q0 = rate/(1+rate);
pth0 = SStat_mass_FDR(p,q0);
detv0 = maskvtx(p <= pth0);
ndetv0 = length(detv0);
m0 = nv-ndetv0;
%% Second stage
if (ndetv0 ~= 0) && (ndetv0 ~= nv)
    pth = SStat_mass_FDR(p,q0*nv/m0);
    detvtx = maskvtx(p <= pth);
else
    detvtx = detv0;
    pth = pth0;
end;


%% DEMO 18: Arbitrary axis of rotation
%
%
%
% Some modenr CT geometires are starting to be a bit more complex, one of
% the common things being arbitrary axis of rotation i.e. the detector and the
% source can move not in a circular path, but in a "spherical" path.
%
% In TIGRE this has been implemented by defining the rotation with 3
% angles, specifically the ZYZ configuration of Euler angles.
%
%  This demo shows how to use it.
%
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
% % Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri
%--------------------------------------------------------------------------
%% Initialize

clear;
close all;
%% Define Geometry
%
% VARIABLE                                   DESCRIPTION                    UNITS
%-------------------------------------------------------------------------------------
geo.DSD = 1536;                             % Distance Source Detector      (mm)
geo.DSO = 1000;                             % Distance Source Origin        (mm)
% Detector parameters
geo.nDetector=[512; 512];					% number of pixels              (px)
geo.dDetector=[0.8; 0.8]; 					% size of each pixel            (mm)
geo.sDetector=geo.nDetector.*geo.dDetector; % total size of the detector    (mm)
% Image parameters
geo.nVoxel=[128;128;128];                   % number of voxels              (vx)

% a bit smaller than usual because the demo includes a very big detector
% angle for showcase
geo.sVoxel=[256;256;256]/1.5;               % total size of the image       (mm)

geo.dVoxel=geo.sVoxel./geo.nVoxel;          % size of each voxel            (mm)
% Offsets
geo.offOrigin =[0;0;0];                     % Offset of image from origin   (mm)
geo.offDetector=[0; 0];                     % Offset of Detector            (mm)

% Auxiliary
geo.accuracy=0.5;                           % Accuracy of FWD proj          (vx/sample)

geo.mode='cone';

%% Define angles
numProjs = 100;

anglesY=linspace(0,2*pi,numProjs);
anglesZ2=anglesY;
anglesZ1=pi*sin(linspace(0,2*pi,numProjs));
angles=[anglesZ1;anglesY;anglesZ2];
%% Get Image

head=headPhantom(geo.nVoxel);

%% Project

projections=Ax(head,geo,angles);

plotProj(projections,(1:100)*pi/180); % angle information not right in the title
%% Reconstruct:

% Note, FDK will not work.

imgSIRT = SIRT(projections,geo, angles,50);
imgCGLS = CGLS(projections,geo, angles,10);

plotImg([head imgCGLS imgSIRT] ,'dim',3)


function gamma_inc_test ( )

%*****************************************************************************80
%
%% GAMMA_INC_TEST tests R4_GAMIC and R8_GAMIC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'GAMMA_INC_TEST:\n' );
  fprintf ( 1, '  Test GAMMA_INC_VALUES, R4_GAMIC, R8_GAMIC.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             A               X     GAMIC(A,X)\n' );
  fprintf ( 1, '                                R4_GAMIC(A,X)         Diff\n' );
  fprintf ( 1, '                                R8_GAMIC(A,X)         Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, a, x, fx1 ] = gamma_inc_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_gamic ( single ( a ), single ( x ) );
    fx3 = r8_gamic ( a, x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.4f  %14.6g\n', a, x, fx1 );
    fprintf ( 1, '                                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


function [elem,bdFlag] = sortelem3(elem,bdFlag)
%% SORTELEM3 sort elem in ascend ordering
%
% [elem,bdFlag] = sortelem3(elem,bdFlag) sorts the elem such that
% elem(t,1)< elem(t,2)< elem(t,3)<elem(t,4). A simple sort(elem,2) cannot
% sort bdFlag.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Step 1: elem(:,4) is the biggest one
[tempvar,idx] = max(elem,[],2);  %#ok<*ASGLU>
elem(idx==1,1:4) = elem(idx==1,[2 4 3 1]);
elem(idx==2,1:4) = elem(idx==2,[3 4 1 2]);
elem(idx==3,1:4) = elem(idx==3,[4 2 1 3]);
if exist('bdFlag','var')
    bdFlag(idx==1,1:4) = bdFlag(idx==1,[2 4 3 1]);
    bdFlag(idx==2,1:4) = bdFlag(idx==2,[3 4 1 2]);
    bdFlag(idx==3,1:4) = bdFlag(idx==3,[4 2 1 3]);
end
%% Step 2: elem(:,1) is the smallest one
[tempvar,idx] = min(elem(:,1:3),[],2);
% elem(idx==1,1:3) = elem(idx==1,[1 2 3]);
elem(idx==2,1:3) = elem(idx==2,[2 3 1]);
elem(idx==3,1:3) = elem(idx==3,[3 1 2]);
if exist('bdFlag','var')
    bdFlag(idx==2,1:3) = bdFlag(idx==2,[2 3 1]);
    bdFlag(idx==3,1:3) = bdFlag(idx==3,[3 1 2]);
end

%% Step 3: sort elem(:,2)<elem(:3)
idx = (elem(:,3) < elem(:,2));
elem(idx,[2 3]) = elem(idx,[3 2]);
if exist('bdFlag','var')
    bdFlag(idx,[2 3]) = bdFlag(idx,[3 2]);
end

%% Output
if ~exist('bdFlag','var')
    bdFlag = [];
end


% When you have a light and object described by a mesh, you want to know
% the shadow of the object.
% This function calculates a mesh named "shadowvolume" (see wikipedia) from
% a triangulated object mesh and a light.
%
% [SVvertices,SVfaces]=patchshadowvolume(OBJvertices,OBJfaces,L);
%
% Inputs,
%    OBJvertices, OBJfaces : The triangulated patch vertices and faces
%                       of the object causing a shadow
%    L: The light must be a 1x4 array with x,y,z,d,
%		 with d=0 for parallel light, then x,y,z is the light direction
%		 and d=1 for point light, then x,y,z is the light position
%
% Outputs,
%    SVvertices, SVfaces : The triangulated shadow volume
%
% Function is written by D.Kroon University of Twente (March 2010)


function [S,SE] = sample_edges(V,E,samples_per_edge)
  % SAMPLE_EDGES Compute samples_per_edge extra points along each edge in E
  % defined over vertices of V.
  %
  % S = sample_edges(V,E,samples_per_edge)
  %
  % Inputs:
  %   V  vertices over which edges are defined, # vertices by dim
  %   E  edge list, # edges by 2
  %   samples_per_edge  number of extra samples to be computed along edge not
  %     including start and end points
  % Outputs:
  %   S  sampled vertices, size less than # edges * (2+samples_per_edge) by dim,
  %   always begins with V so that E is also defined over S
  %
  %

  dim = size(V,2);

  % trivial case
  if(isempty(E))
    S = [];
    SE = [];
  elseif(samples_per_edge < 0)
    S = V;
    SE = [];
  else
    % fraction parameter
    t = linspace(0.0,1.0,samples_per_edge+2);
    % get rid of start and end points
    t = t(2:(end-1));
    % repeat for each coordinate
    t = reshape(repmat(t,dim,1),1,size(t,2)*dim);
    % repeat for each edge
    t = repmat(t,size(E,1),1);
    % repeat start coords

    sp = repmat(V(E(:,1),:),1,samples_per_edge);
    % repeat end coords
    ep = repmat(V(E(:,2),:),1,samples_per_edge);
    % lerp from start point to end point for each coordinate
    S = sp.*(1-t) + ep.*t;
    % reshape to list coordinates
    S = S';
    S = reshape(S,dim,prod(size(S))/dim)';
    S = [V;S];
    % Determine edges between samples
    E1 = repmat((1:(samples_per_edge-1))',size(E,1),1);
    E2 = repmat((2:(samples_per_edge))',size(E,1),1);
    off = size(V,1) + ...
      reshape( ...
        repmat( ...
          samples_per_edge*((1:size(E,1))-1), ...
          samples_per_edge-1, ...
          1), ...
        (samples_per_edge-1)*size(E,1),...
        1);
    SE = repmat(off,1,2) + [E1 E2];
    % end point edges
    SE = [ ...
      SE; ...
      E(:,1) size(V,1) + (1+samples_per_edge*((1:size(E,1))-1))'; ...
      E(:,2) size(V,1) + (samples_per_edge*((1:size(E,1))))'];
  end
end


function [elem2dof,elem2edge,edge,bdDof,freeDof] = dofP3(elem)
%% DOFP3 dof structure for P3 element.
%
%  [elem2dof,edge,bdDof] = DOFP3(elem) constructs the dof structure
%  for the quadratic element based on a triangle. elem2dof(t,i) is the
%  global index of the i-th dof of the t-th element.
%
%  The global indices of the dof is organized  according to the order of
%  nodes, edges and elements, namely, first give index number to the dofs
%  on nodes, then the dofs on edges, last the dofs on elements.
%
%  See also dofP2, dof3P3.
%
%  Created by Jie Zhou.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

N = max(max(elem)); NT = size(elem,1);

%% Data structure
totalEdge = uint32(sort([elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])],2));
matlabversion = version;
if str2double(matlabversion(end-5:end-2)) > 2012
    [edge, i2, j] = unique(totalEdge,'rows','legacy');
else
    [edge, i2, j] = unique(totalEdge,'rows');
end
NE = size(edge,1);
elem2edge = reshape(j,NT,3);

%% Nodal dof
elem2dof = uint32(zeros(NT,10));
elem2dof(:,1:3) = elem;

%% Two dof on each edge
% edge 1
idx0 = (elem(:,3) > elem(:,2));
elem2dof(idx0,4) = N + 2*(elem2edge(idx0,1))-1;
elem2dof(idx0,5) = N + 2*(elem2edge(idx0,1));
elem2dof(~idx0,4)= N + 2*(elem2edge(~idx0,1));
elem2dof(~idx0,5)= N + 2*(elem2edge(~idx0,1))-1;
% edge 2
idx0 = (elem(:,3) > elem(:,1));
elem2dof(idx0,6) = N + 2*(elem2edge(idx0,2));
elem2dof(idx0,7) = N + 2*(elem2edge(idx0,2))-1;
elem2dof(~idx0,6) = N + 2*(elem2edge(~idx0,2))-1;
elem2dof(~idx0,7) = N + 2*(elem2edge(~idx0,2));
% edge 3
idx0 = (elem(:,2) > elem(:,1));
elem2dof(idx0,8) = N + 2*(elem2edge(idx0,3))-1;
elem2dof(idx0,9) = N + 2*(elem2edge(idx0,3));
elem2dof(~idx0,8) = N + 2*(elem2edge(~idx0,3));
elem2dof(~idx0,9) = N + 2*(elem2edge(~idx0,3))-1;

%% Element dof
elem2dof(:,10) = (N+2*NE+1:N+2*NE+NT)';

%% Boundary dof
i1(j(3*NT:-1:1)) = 3*NT:-1:1;
i1 = i1';
bdEdgeIdx = (i1 == i2);
isBdDof = false(N+2*NE+NT,1);
isBdDof(edge(bdEdgeIdx,:)) = true;   % boundary node
idx = find(bdEdgeIdx);
isBdDof(N+2*idx) = true;      % two dof on boundary edges
isBdDof(N+2*idx-1) = true;
bdDof = find(isBdDof);
 freeDof = find(~isBdDof);


function [Y,h]=glyph(X,r,c)
%Syntax: [Y,h]=glyph(X,r,c)
%__________________________
%
% 3D glyph visualization. The glyph is a convex deltahedron where each
% node is connected to its 6 nearest nodes.
%
% Y is the N-by-3 matrix whith the cartesian coordinates of the points on
%   the glyph.
% h returns a vector of tetrahedra handles. Each element of h is a handle
%   to the set of patches forming one tetrahedron. Type "help tetramesh"
%   for more info.
% X is the N-by-3 matrix whith the cartesian coordinates of the points on
%   the sphere.
% r is the range parameter.
% c is the color parameter.
%
%
% References:
%
% Sangole A., Knopf G. K. (2002): Representing high-dimensional data sets
% as close surfaces. Journal of Information Visualization 1: 111-119
%
% Sangole A., Knopf G. K. (2003): Geometric representations for
% high-dimensional data using a spherical SOFM. International Journal of
% Smart Engineering System Design 5: 11-20
%
% Sangole A., Knopf G. K. (2003): Visualization of random ordered numeric
% data sets using self-organized feature maps. Computers and Graphics 27:
% 963-976
%
% Sangole A. P. (2003): Data-driven Modeling using Spherical
% Self-organizing Feature Maps. Doctor of Philosophy (Ph.D.) Thesis.
% Department of Mechanical and Materials Engineering. Faculty of
% Engineering. The University of Western Ontario, London, Ontario, Canada.
%
%
% Remark:
%
% If no output is desired, the function plots the glyph.
%
%
% Archana P. Sangole, PhD., P.E. (TX chapter)
% School of Physical & Occupational Therapy
% McGill University
% 3654 Promenade Sir-William-Osler
% Montreal, PQ, H3G 1Y5
% e-mail: archana.sangole@mail.mcgill.ca
%
% CRIR, Rehabilitation Institute of Montreal
% 6300 Ave Darlington
% Montreal, PQ, H3S 2J5
% Tel: 514.340.2111 x2188
% Fax: 514.340.2154
%
%
% Alexandros Leontitsis, PhD
% Department of Education
% University of Ioannina
% 45110- Dourouti
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% 23-Mar-2006

% Add a center to the sphere, which is [0 0 0]
X=[X;zeros(1,3)];

% 3-dimensional Delaunay tessellation
tri=delaunay3(X(:,1),X(:,2),X(:,3));

% Remove the center
X(end,:)=[];

if nargin<2 | isempty(r)==1
    r=ones(length(X),1);
else
    % r must be a vector
    if min(size(r))>1
        error('r must be a vector.');
    end
    % The length of r should be equal to the length of X.
    if length(r)~=length(X)
        error('The length of r should be equal to the length of X.');
    end
    r=r(:);
end

if nargin<3 | isempty(c)==1
    c=r;
else
    % c must be a vector
    if min(size(c))>1
        error('c must be a vector.');
    end
    % The length of c should be equal to the length of X.
    if length(c)~=length(X)
        error('The length of c should be equal to the length of X.');
    end
    c=c(:);
end

% Go to spherical coordinates, ...
[theta,phi]=cart2sph(X(:,1),X(:,2),X(:,3));
% ... and compute the cartesian coordinates with the given r
[Y(:,1),Y(:,2),Y(:,3)]=sph2cart(theta,phi,r);

% Add a center to the glyph, which is [0 0 0]
Y=[Y;zeros(1,3)];
% Sort the vertices of each tereahedron ...
tri=sort(tri')';
% ... in order to calculate the color
i=1:length(tri);
cnew(i)=mean(c(tri(i,1:end-1))')';

% If no output is desired, plot the glyph
if nargout==0
    % Plot the glyph
    tetramesh(tri,Y,cnew,'FaceAlpha',1,'LineStyle','-');
    % Define the x axis
    xlim([-1.2 1.2])
    % Define the y axis
    ylim([-1.2 1.2])
    % Define the z axis
    zlim([-1.2 1.2])
    % Define the color axis
    caxis([1 1.2]);
    % Remove the axes
    axis off
    % Freeze aspect ratio properties to faciliate 3D rotation
    axis vis3d
% Else if the handle is desired
elseif nargout==2
    % Retrieve the handle
    h=tetramesh(tri,Y,cnew,'FaceAlpha',1,'LineStyle','none');
end


function [output] = F_exp(input_layer)
input = input_layer.a;
output = exp(input);
end


% AssignmentToIndex Convert assignment to index.
%
%   I = AssignmentToIndex(A, D) converts an assignment, A, over variables
%   with cardinality D to an index into the .val vector for a factor.
%   If A is a matrix then the function converts each row of A to an index.
%
%   See also IndexToAssignment.m and SampleFactors.m

function I = AssignmentToIndex(A, D)

D = D(:)'; % ensure that D is a row vector
if (any(size(A) == 1)),
    I = cumprod([1, D(1:end - 1)]) * (A(:) - 1) + 1;
else
    I = sum(repmat(cumprod([1, D(1:end - 1)]), size(A, 1), 1) .* (A - 1), 2) + 1;
end;

end


%%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%%

check_active_images;

% Reproject the patterns on the images, and compute the pixel errors:

ex = []; % Global error vector
x = []; % Detected corners on the image plane
y = []; % Reprojected points

if ~exist('alpha_c'),
   alpha_c = 0;
end;

for kk = 1:n_ima,

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

   if active_images(kk) & (~isnan(omckk(1,1))),

      %Rkk = rodrigues(omckk);

      eval(['y_' num2str(kk) '  = project_points_fisheye(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']);

      eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' - y_' num2str(kk) ';']);

      eval(['x_kk = x_' num2str(kk) ';']);

      eval(['ex = [ex ex_' num2str(kk) '];']);
      eval(['x = [x x_' num2str(kk) '];']);
      eval(['y = [y y_' num2str(kk) '];']);

   else

      %	eval(['y_' num2str(kk) '  = NaN*ones(2,1);']);

      % If inactivated image, the error does not make sense:
      eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']);

   end;

end;

err_std = std(ex')';


function [t,tm,b] = get_max_t(Zpop,sterr,tt)
% :Usage:
% ::
%
%     [t,tm,b] = get_max_t(Zpop,sterr,tt)
%
% :Outputs:
%
%   **t:**
%        t-value timeseries
%
%   **tm:**
%        max t-value (abs)
%
%   **b:**
%        time (index) of max t-value
%

mu = mean(Zpop(1:tt));                              % population mean
t = (Zpop - mu) ./ sterr;
[tm, b] = max(abs(t(tt+1:end)));              % Calculate maximum absolute t-value
b = b+tt;                                     % max t time
tm = t(b);                                      % put sign back in

return


%{
load('dataset/trafficdb/traffic_patches.mat');
[M,m,n,p] = convert_video3d_to_2d(im2double(imgdb{100}));
out = run_algorithm('MC', 'OP-RPCA', M, [])
show_results(M.*out.Omega,out.L,out.S,out.O,p,m,n);
%}

lambda = 0.35; % [0,1] 0.5(+lowrank) 0.4 0.3 0.2(+sparse)
[L,S] = mr_pca_part(M,Omega,lambda);


function [out] = prep_envelope(dat,varargin)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% prep_envelope
%
% Synopsis:
%   [out] = prep_envelope(dat,<var>)
%
% Example :
%    [out] = prep_envelope(dat)
%
% Arguments:
%     dat    - Epoched signal
% Options:
%     Time[ms] - time window. scalar or nx1 vector for weighting (default: 100)
%     Method - 'centered' or 'causal' (default: causal)
%
% Returns:
%     out - Envelope of the signal
%
% Description:
%     This function smoothly outlines the extremes of an oscillating
%     signal, continuous or epoched.
%     continuous data should be [time * channels]
%     epoched data should be [time * channels * trials]
%
% See also 'https://github.com/PatternRecognition/OpenBMI'
%
% Min-ho Lee, 01-2018
% mh_lee@korea.ac.kr
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~isfield(dat,'x')
    warning('OpenBMI: Data structure must have a field named ''x''')
    return
end

s = size(dat.x);
dat.x= reshape(abs(hilbert(dat.x(:,:))),s);
out= prep_movingAverage(dat,varargin{:});


function [Data_PCA,F,angles] = locPCA_affnity_fast(data,index_mat,params)
n_neighbors = size(index_mat,1)%params.n_neighbors;
%n_neighbors = params.n_neighbors;
int_dim = params.int_dim;
N = size( data, 2 );
D = size( data, 1 );
Data_eig=cell(1,N);
% retain = params.m*params.n_neighbors;
% n_neighbors=retain
%n_neighbors=retain;
%n_neighbors = params.n_neighbors;
%[index_mat]=K_nearest_neighbors(data,n_neighbors);
%F=sparse(zeros(N,N));
F=spalloc(N,N,300*N);
%F=(zeros(N,N));
[m,N] = size(data);  % m is the dimensionality of the input sample points.

eig_dim1=zeros(m,N);
if length(n_neighbors)==1
    K = repmat(n_neighbors,[1,N]);
end;

parfor i=1:N

    % Compute the d largest right singular eigenvectors of the centered matrix
    Ii = index_mat(:,i); ki = K(i);
    Xi = data(:,Ii)-repmat(mean(data(:,Ii),2),[1,ki]);

    %construct the matrix Y
    Y = Xi' / sqrt(n_neighbors-1);
    %[U,Sing,V] = svd( Xi );
    [U,Sing,V] = svd( Y,'econ');
    %   Zi = U(:,1:d)*U(1:d,:)*Xi;
    %  [signals,PC,V] = pca2(Xi) ;
    % Data_PCA(1:D,i) = data(1:D,i);
    %Data_eig{i}=V(:,1:d);
    Data_eig{i}= V(:,1:int_dim);
    eig_dim1(:,i)=(Data_eig{i}(1:m));
    %Data_eig{i}=V(:,:1:d);

end

angles=spalloc(N,N,300*N);
%h = waitbar(0,'Local PCA...');
if params.int_dim>1
for i=1:N
    waitbar(i/N);
    normal_i=Data_eig{i}';
    temp_nei_i=index_mat(:,i);
    for j=1:length(temp_nei_i)
        idxNeig = temp_nei_i(j);
        normal_j=(Data_eig{idxNeig})';
        [theta] = max(subspaceangle(normal_i',normal_j'));
        angles(idxNeig,i) = theta;
        angles(i,idxNeig) = theta;
       % theta = subspace(normal_i',normal_j');
        F(idxNeig,i)=cos(theta).^params.powerCos;
        % F(j,i)=abs(acosd(theta));
        %F(j,i) = sind(F(j,i));
        %F(j,i)=exp((-F(j,i).^2)./1.0);
        F(i,idxNeig)=F(idxNeig,i);
    end
end
elseif params.int_dim==1
%  % s = sqrt( sum( ( data_noise(:,I)-data_noise(:,J) ).^2, 1) );
% for i=1:N
%     waitbar(i/N);
%     normal_i   =   eig_dim1(:,i)';
%     temp_nei_i =   index_mat(:,i);
%     normal_j=( eig_dim1(:, temp_nei_i));
%     F(i, temp_nei_i)=normal_i*normal_j;
%     F(temp_nei_i,i)=F(i,temp_nei_i);
% end
% [I]=find(F);
% s1 =  (acosd(F(I)));
% s2=  180- (acosd(F(I)));
% s=min(s1,s2);
% angles(I)=s;
% [IDX_vote,S]=knnsearch(data',data','k',retain+1,'distance','euclidean');
%  IDX_vote=IDX_vote(:,2:end);
%  S=S(:,2:end);
%  IDX_vote=IDX_vote';
%  S=S';
%  S = S(1:n_neighbors,:);
 IDX2 = index_mat;%(1:n_neighbors,:);
 a1=1:N;
 a2=repmat(a1,n_neighbors);
 a2=a2(1:n_neighbors,1:N);
 a3=a2(:);
 I=a3;
 J=IDX2(:);
 r1=eig_dim1(:,I);
 r2=eig_dim1(:,J);
 %A1=zeros(D,N); A2=zeros(D,N);
 a3=r1(1,:);
 a4=r2(1,:);
 a5=r1(2,:);
 a6=r2(2,:);
 A=r1.*r2;
 A=sum(A);
 %A=(a3.*a4)+(a5.*a6);
 s=A(:);

 w_temp=abs(s);
 waff = sparse(I,J,w_temp,N,N);
 W_k=waff;
 W_k= (W_k + W_k')./2;
 F=sparse(W_k);
% for i=1:N
%     waitbar(i/N);
%     normal_i   =   eig_dim1(:,i)';
%     temp_nei_i =   index_mat(:,i);
%     normal_j=( eig_dim1(:, temp_nei_i));
%     F(i, temp_nei_i)=normal_i*normal_j;
%     F(temp_nei_i,i)=F(i,temp_nei_i);
% end
% [I]=find(F);
% s1 =  (acosd(F(I)));
% s2=  180- (acosd(F(I)));
% s=min(s1,s2);
% angles(I)=s;
% end
end

%close(h);
Data_PCA = Data_eig;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = inversekinematic_Viper_s1300(robot, T)
%   Solves the inverse kinematic problem for the ADEPT Viper_S1300 robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=inversekinematic_Viper_s1300 returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('ADEPT', 'Viper_s1300');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry at the reference for this robot, distance from the wrist to
%the end effector
L6=d(6);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution, obtain theta1
% by geometric methods
q1 = atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%Arrange solutions, there are 8 possible solutions so far.
% if q1 is a solution, q1* = q1 + pi is also a solution.
% For each (q1, q1*) there are two possible solutions
% for q2 and q3 (namely, elbow up and elbow up solutions)
% So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
% there exist two more possible solutions for the last three joints, generally
% called wrist up and wrist down solutions. For this reason,
%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    % use solve_spherical_wrist2 for the particular orientation
    % of the systems in this ABB robot
    % use either the geometric or algebraic method.
    % the function solve_spherical_wrist2 is used due to the relative
    % orientation of the last three DH reference systems.

    %use either one algebraic method or the geometric
    %qtemp = solve_spherical_wrist(robot, q(:,i), T, 1, 'geometric'); %wrist up
    qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'algebraic'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    %qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist down
    qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'algebraic'); %wrist down
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%DH table parameters with which we calculate theta2, using
%geometric methods
L2=a(2);
L3=d(4);

%Offset distance between the centers of the reference systems of the links
%2 and 3
A1 = a(3);

%See geometry of the robot. Considering L4 calculate the offset between the
%centers of the reference systems of the links 2 and 3
L4 = sqrt(A1^2 + L3^2);

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

%Distance between the system 1 to the wrist
r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));   %Theorem of the cosine

if ~isreal(gamma)
    disp('WARNING:inversekinematic_Viper_s1300: the point is not reachable for this configuration, imaginary solutions');
    %gamma = real(gamma);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta2. We add pi/2 to offset lags in our reference systems
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%DH table parameters with which we calculate theta3, using
%geometric methods
L2=a(2);
L3=d(4);

A1 = a(3);

%See geometry of the robot, like in the function q2
L4 = sqrt(A1^2 + L3^2);

%The delta angle is fixed because they are the lines that make up the gap
%between the links 2, 3 and 4
delta = real(acos((A1^2+L4^2-L3^2)/(2*A1*L4)));   %Theorem of the cosine

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

%Same as the function q2
r = sqrt(p1(1)^2 + p1(2)^2);

%Real angle between the links 2 and 3
ro = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));   %Theorem of the cosine

if ~isreal(ro)
   disp('WARNING:inversekinematic_Viper_s1300: the point is not reachable for this configuration, imaginary solutions');
   %ro = real(ro);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3. We add pi to offset lags in our reference systems
q3(1) = pi - ro - delta; %elbow up
q3(2) = pi + ro - delta; %elbow down

%remove complex solutions for q for the q1+pi solutions
function  qreal = arrange_solutions(q)
qreal=q(:,1:4);

%sum along rows if any angle is complex, for any possible solutions, then v(i) is complex
v = sum(q, 1);

for i=5:8,
    if isreal(v(i))
        qreal=[qreal q(:,i)]; %store the real solutions
    end
end

qreal = real(qreal);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [test_kernel,train_kernel,optimal_alpha] = DSK_optimization(train_data,train_label,test_data,opt)
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input parameters:
% train_data: column cells containing the SPD matrices for training
% train_label: one column vector containing the labels for the training data
% test_data: test data with the same format as the training data
% opt:  a structure containing parameter settings
%       elements:
%       theta -- a kernel parameter
%       obj_method -- which criterion to be used
%       original_alpha  set to 1 to use original Stein kernel or 0 to use DSK
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Output parameters:
% test_kernel: the adjusted test_kernel
% train_kernel: the adjusted train_kernel
% optimal_alpha: the optimized adjustment parameters alpha
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Jianjia Zhang, jz163@uowmail.edu.au Dec, 2014, all rights reserved
% For implementation details, please refer to:
% "Learning Discriminative Stein Kernel for SPD Matrices and Its Applications."
% arXiv preprint arXiv:1407.1974 (2014).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [test_kernel,train_kernel,optimal_alpha] = DSK_optimization_new(TrainSet,TestSet,opt)
nmode = size(TrainSet.X_cov,1); % dimension of the SPD matrices
train_decomp = Decomposite_eig_new(TrainSet); % eigen decomposition of the training/test data
test_decomp = Decomposite_eig_new(TestSet);
%%%%%%%%%%%%%%%%%%%%%%%%%
if(~opt.original_alpha)
    initial_alpha = 1*ones(1,nmode); % the initial alpha corresponding to the original Stein kernel
    LB = 0.01*initial_alpha;

    options = optimset('Algorithm','interior-point'); % run interior-point algorithm
    options.Display = 'iter';
    options.Display = 'off';
    options.MaxIter = 100;
    options.TolFun = 1e-5;
    %tic
    optimal_alpha = fmincon(@(alpha) objfun_ff_new(alpha,TrainSet.y,train_decomp,opt.lambda,initial_alpha,opt.obj_method,opt.theta),initial_alpha,[],[],[],[],LB,[],[],options);
    %toc
else
    optimal_alpha = ones(1,nmode);
end
[S_test] = EigComp2SD_power_new(train_decomp,test_decomp,optimal_alpha); % compute the Stein divergence with the obtained adjustment parameter optimal_alpha
[S_train] = EigComp2SD_power_new(train_decomp,train_decomp,optimal_alpha);
test_kernel = exp(-1*opt.theta*S_test); % compute the kernel
train_kernel = exp(-1*opt.theta*S_train);
end


function BED = calc_BED(paramS,varargin)
%BED = calc_BED(paramS)
%Ref: Comparison Between Mechanistic Radiobiological Modeling Vs.
%Fowler BED Equation in Evaluating Lung Cancer Radiotherapy Outcome
%for a Broad Range of Fractionation, J Jeong et al., AAPM 2017
%-----------------------------------------------------------------------
% INPUTS
% paramS : Parameter dictionary with fields:
%          d  - Fraction size
%          n  - No. fractions
%          T  - No. treatment days
%          alpha
%          abRatio
% Note : For a 3D dose distibution, calculate 3D BED by setting input
%        paramS.frxSize.val = doseArray3M/numFrx
%-----------------------------------------------------------------------
% AI 12/4/17
% AI 07/30/18 Updated to handle 3D dose distibution

%Define constants
Tk = paramS.Tk.val;         %Kick-off time of repopulation (days)
Tp =  paramS.Tp.val;        %Potential tumor doubling time (days)

alpha = paramS.alpha.val;
abRatio = paramS.abRatio.val;  %alpha/beta for tumor
d = paramS.frxSize.val;
n = paramS.numFractions.val;
if isfield(paramS,'treatmentDays')
    txDaysV = paramS.treatmentDays.val;
    %Check for numeric input
    if ~isnumeric(txDaysV)
        txDaysV = str2num(txDaysV);
    end
    %Otherwise assume function specified
    if isempty(txDaysV)
        txDaysV = eval([txDaysV,'(',num2str(n),')']);
    end
    T = txDaysV(end);
else
    % Default: Compute length of treatment assuming one fraction every
    % weekday with weekend breaks.
    T = floor(n/5)*7 + mod(n,5);
end

%Compute BED
BED = n.* d .* (1 + d./abRatio);
if T > Tk
  BED = BED -  log(2) * (T-Tk)/(alpha*Tp);
end

end


% Application of Oleinik's entropy condition

% Copyright 2001 P. Wesseling
% This program and its subprograms may be freely used, modified and distributed
% under the GNU General Public License: http://www.gnu.org/copyleft/gpl.html

% Theory in Section 9.4 of:

% 	P. Wesseling: Principles of Computational Fluid Dynamics
% 	Springer, Heidelberg, 2000 ISBN 3-5453-0. XII, 642 pp.
% See http://ta.twi.tudelft.nl/nw/users/wesseling/cfdbook.html

% This program generates Fig. 9.11 in the book

		% ...............Input.........................................
a = 0.5;	% Parameter in flux function on page 371
		% ............End of input.....................................

x = 0.0:0.05:1.0; den = x.*x + 0.5*(1-x).^2;
f = (x.*x)./den;	% Buckley-Leverett flux function

x1 = 1/sqrt(3);
f1 = (x1*x1)/(x1*x1 + 0.5*(1-x1)^2);	%Point where tangent meets curve

figure(1), clf, hold on, plot(x,f,'-')
xx = [0  1];  xx = x1*xx; yy = [0  1]; yy = f1*yy; plot(xx,yy,'-')
zz = [0  f1]; xx = [x1 x1];			   plot(xx,zz,'--')


function [axis,angle]=rotation2axisangle(R)
cosq = 0.5*(R(1,1)+R(2,2)+R(3,3)-1);
if(cosq>1)
    cosq=1;
end
if(cosq<-1)
    cosq=-1;
end
angle = acos(cosq);
axis = zeros(3,1);
if(abs(angle)<1e-4)
elseif(abs(angle-pi)<1e-4)
    for i=1:3
        a = 0.5*(R(i,i)+1);
        if abs(a)<1e-4
            axis(i) = 0;
        else
            axis(i) = a^0.5;
        end
    end

    maxind = 1;
    if axis(2)>axis(1)
        maxind = 2;
    end
    if axis(3)>axis(2)
        maxind = 3;
    end
    if(maxind == 1)
        a1a2 = R(1,2) + R(2,1);
        a1a3 = R(1,3) + R(3,1);
        if(a1a2<0)
            axis(2) = -axis(2);
        end
        if(a1a3<0)
            axis(3) = -axis(3);
        end
    end
    if(maxind == 2)
        a1a2 = R(1,2) + R(2,1);
        a2a3 = R(2,3) + R(3,2);
        if(a1a2<0)
            axis(1) = -axis(1);
        end
        if(a2a3<0)
            axis(3) = -axis(3);
        end
    end
    if(maxind == 3)
        a1a3 = R(1,3) + R(3,1);
        a2a3 = R(2,3) + R(3,2);
        if(a1a3<0)
            axis(1) = -axis(1);
        end
        if(a2a3<0)
            axis(2) = -axis(2);
        end
    end
    axis = axis/norm(axis);
else
    axis(1) = R(3,2) - R(2,3);
    axis(2) = R(1,3) - R(3,1);
    axis(3) = R(2,1) - R(1,2);
    axis = axis/norm(axis);
end

end

github-repos/MATLAB/xuhuairuogu-V-REP-Simulation-Projects/V-REP-Simulation-Projects-841b944af4ea3a8fb250578d36434515f577f411/admittance control(adapted from an example in the book--A Systematic Approach to Learning Robot Programming with ROS)/iiwa14_kinematics/rotation2axisangle.m


function [ D ] = D06( f,h1,h2 )

    Df = 0.000389*f*h1*h2;
    Dh = 4.1*(sqrt(h1)+sqrt(h2));
    D = Df*Dh/(Df+Dh);
end


% Estimation of the standard deviation of an image assuming that the noise
% is Gaussian white additive. The file estimates the standard deviation
% using the median filter on the fine scale subband of the wavelet
% decomposition.
 function sd_estimate=sdest(x)
 [ca,ch,cv,cd] = dwt2(x,'sym4','mode','sym');
 sd_estimate = mad(cd(:),1)/0.6745


% compute SVD of data and save to file
function [ops, U, Sv] = get_svdcomps(ops)

% load(sprintf('%s/%s/%s/regops_%s_%s_plane%d.mat', ops.ResultsSavePath, ops.mouse_name, ops.date, ...
%     ops.mouse_name, ops.date, ops.iplane))

iplane = ops.iplane;

[Ly, Lx] = size(ops.mimg);

ntotframes          = ceil(sum(ops.Nframes));
% number of frames used to compute SVD
ops.NavgFramesSVD   = min(ops.NavgFramesSVD, ntotframes);
% size of binning (in time)
nt0 = ceil(ntotframes / ops.NavgFramesSVD);

if isfield(ops, 'chunk_align') && ~isempty(ops.chunk_align); chunk_align   = ops.chunk_align(iplane);
else chunk_align = 1; end

if chunk_align>9
    nt0 =  ops.chunk_align;
end
ops.NavgFramesSVD = floor(ntotframes/nt0);
nimgbatch = nt0 * floor(2000/nt0);

%% load the data

ix = 0;
fid = fopen(ops.RegFile, 'r');
mov = zeros(numel(ops.yrange), numel(ops.xrange), ops.NavgFramesSVD, 'single');

while 1
    data = fread(fid,  Ly*Lx*nimgbatch, '*int16');
    if isempty(data)
        break;
    end
    data = reshape(data, Ly, Lx, []);

    % subtract off the mean of this batch
%     data = data - repmat(ops.mimg1, 1, 1, size(data,3));

    nSlices = nt0*floor(size(data,3)/nt0);
    if nSlices ~= size(data,3)
        data = data(:,:, 1:nSlices);
    end

    % bin data
    data = reshape(data, Ly, Lx, nt0, []);
    data = single(data);
    davg = squeeze(mean(data,3));

    mov(:,:,ix + (1:size(davg,3))) = davg(ops.yrange, ops.xrange, :);

    ix = ix + size(davg,3);
end
fclose(fid);
mov = mov(:, :, 1:ix);

%% SVD options

% number of SVD components kept
ops.nSVD = min(ops.nSVD, size(mov,3));
%
mov             = reshape(mov, [], size(mov,3));
% mov             = mov./repmat(mean(mov.^2,2).^.5, 1, size(mov,2));

% compute covariance matrix of frames
if ops.useGPU
    COV             = gpuBlockXtX(mov)/size(mov, 1);
else
    COV             = mov' * mov/size(mov,1);
end

ops.nSVD = min(size(COV,1)-2, ops.nSVD);

% take SVD of covariance matrix and keep ops.nSVD components
if ops.nSVD<1000 || size(COV,1)>1e4
    [V, Sv]          = eigs(double(COV), ops.nSVD);
else
    if ops.useGPU
        gpuCOV = gpuArray(double(COV));
        [V, Sv]         = svd(gpuCOV);
        clear gpuCOV;
        V = gather(single(V));
        Sv = gather(single(Sv));
    else
         [V, Sv]         = svd(COV);
    end
    V               = V(:, 1:ops.nSVD);
    Sv              = Sv(1:ops.nSVD, 1:ops.nSVD);
end
%%

% compute U (normalized spatial masks... pixels x components)
if ops.useGPU
    U               = normc(gpuBlockXY(mov, V));
else
    U               = normc(mov*V);
end
U               = single(U);
Sv              = single(diag(Sv));

if ~exist(ops.ResultsSavePath, 'dir')
    mkdir(ops.ResultsSavePath)
end

% project spatial masks onto raw data
fid = fopen(ops.RegFile, 'r');
ix = 0;
Fs = zeros(ops.nSVD, sum(ops.Nframes), 'single');
while 1
    data = fread(fid,  Ly*Lx*nimgbatch, '*int16');
    if isempty(data)
        break;
    end
    data = reshape(data, Ly, Lx, []);

    % subtract off the mean of this batch
    %         data = data - repmat(mean(data,3), 1, 1, size(data,3));
%     data = data - repmat(ops.mimg1, 1, 1, size(data,3));
    data = data(ops.yrange, ops.xrange, :);
    data = single(data);
    if ops.useGPU
        Fs(:, ix + (1:size(data,3))) = gpuBlockXtY(U, reshape(data, [], size(data,3)));
    else
        Fs(:, ix + (1:size(data,3))) = U' * reshape(data, [], size(data,3));
    end

    ix = ix + size(data,3);
end
fclose(fid);

if ~exist(ops.ResultsSavePath, 'dir')
    mkdir(ops.ResultsSavePath);
end

totF = [0 cumsum(ops.Nframes)];
for iexp = 1:length(ops.expts)
    Vcell{iexp} = Fs(:, (1+ totF(iexp)):totF(iexp+1));
end

%%% save SVDs
U = reshape(U, numel(ops.yrange), numel(ops.xrange), []);
try % this is faster, but is limited to 2GB files
    save(sprintf('%s/SVD_%s_%s_plane%d.mat', ops.ResultsSavePath, ...
        ops.mouse_name, ops.date, iplane), 'U', 'Sv', 'Vcell', 'ops', '-v6');
catch % this takes a bit less space, but is significantly slower
    save(sprintf('%s/SVD_%s_%s_plane%d.mat', ops.ResultsSavePath, ...
        ops.mouse_name, ops.date, iplane), 'U', 'Sv', 'Vcell', 'ops');
end

% keyboard;


function vectors2d
%VECTORS2D Description of functions operating on plane vectors.
%
%   A vector is defined by its two cartesian coordinates, put into a row
%   vector of 2 elements:
%   V = [vx vy];
%
%   Several vectors are stored in a matrix with two columns, one for the
%   x-coordinate, one for the y-coordinate.
%   VS = [vx1 vy1 ; vx2 vy2 ; vx3 vy3];
%
%   See also
%   vectorNorm, vectorAngle, isPerpendicular, isParallel
%   normalizeVector, transformVector, rotateVector
%

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2008-10-13, using Matlab 7.4.0.287 (R2007a)
% Copyright 2008-2022 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas

help('vectors2d');


%  Figure 10.72      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% Fig. 10.72
% Data for RTP Demo 3-3-99
% Data provided by Dr. Gwen van der Linden
% Data is from System Identification Studies
InputFlux=[3.460064464376177e-1 1.177299050104922e-1 2.838023866104041e-2;
   3.880303397347619e-11 8.024902450324316e-2 1.807231516460469e-2;
   8.004191616976514e-9 2.721604310757543e-3 3.171348842079633e-2];
M_inv=diag([1.000040130716728 5.557442686788876 13.63821806414694]);
Radiation=[5.47621193859299e-2 -8.570695054070524e-3 -8.296135532988507e-4...
      -4.536181077856052e-2;
   -8.570695054070524e-3 8.570946319867835e-3 -1.621311365067015e-7...
      -8.913466080455817e-8;
   -8.296135532988507e-4 -1.621311365067015e-7 8.299854517643017e-4...
      -2.097673289443245e-7];
Conduction=[3.559939609150268e-7 -1.113667477845243e-7 -1.976161155515125e-7...
      -4.701109757899004e-8;
   -1.113667477845243e-7 1.160207476868843e-2 -2.502736022145532e-3...
      -9.099227379795117e-3;
   -1.976161155515125e-7 -2.502736022145532e-3 6.37364815665867e-3...
      -3.870714518397587e-3];
ScaleTemp=diag([0.01 0.01 0.01 0.01]);

clf;
%
sim('fig10_71')
%plot(tout,r,'-');
%hold on;
%plot(tout,y,'--');
%xlabel('Time (sec)');
%ylabel('Temperature (K)');
%hold off;
%pause;
ii=240:876;
plot(tout(ii),r(ii),'-',tout(ii),y(ii,2),'--');
legend('r','y');
grid on;
xlabel('Time (sec)');
ylabel('Temperature (K)');
title('Fig. 10.72(a) Temperature tracking response');
pause;
hold off;
ii=240:876;
plot(tout(ii),u(ii),'-');
xlabel('Time (sec)');
ylabel('Lamp voltage (V)');
grid on;
legend('u');
title('Fig. 10.72(b) Control effort');


function [u,p,edgeC,A,eqn,info] = StokesisoP2P1(node,elem,pde,bdFlag,option)
%% STOKESISOP2P1 Stokes equation: isoP2-P1 modified Taylor-Hood elements.
%
%  [u,p] = STOKESisoP2P1(node,elem,pde,bdFlag) use constinous P1 element
%  on grid h and continous P1 element on grid H = 2*h to approximate
%  velocity u and pressure p, repectively.
%
%       -div(mu*grad u) + grad p = f in \Omega,
%                        - div u = 0  in \Omega,
%   with
%       Dirichlet boundary condition        u = g_D  on \Gamma_D,
%       Neumann boundary condition du/dn - np = g_N  on \Gamma_N.
%
% Created by Ming Wang at Aug., 2012.
%
% See also StokesisoP2P0, StokesP2P1
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('option','var'), option = []; end

%% Refine grid for P1
nodeC = node; elemC = elem; bdFlagC = bdFlag;
[tempvar,edgeC] = dofP2(elemC);
[node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
NC = size(nodeC,1); NTC = size(elemC,1);
N = size(node,1);  NT = size(elem,1); Nu = N; Np = NC;

tic;
%% Compute geometric quantities and gradient of local basis
[Dlambda,area] = gradbasis(node,elem);
areaC = sum(reshape(area,NTC,4),2);

%% Assemble stiffness matrix for Laplace operator
A = sparse(Nu,Nu);
for i = 1:3
    for j = i:3
        Aij = (Dlambda(:,1,i).*Dlambda(:,1,j) + ...
            Dlambda(:,2,i).*Dlambda(:,2,j)).*area;
        if isfield(pde,'mu') && (pde.mu~=1)
            Aij = pde.mu*Aij;
        end
        if (j==i)
            A = A + sparse(elem(:,i),elem(:,j),Aij,N,N);
        else
            A = A + sparse([elem(:,i);elem(:,j)],[elem(:,j);elem(:,i)],...
                [Aij; Aij],N,N);
        end
    end
end
clear Aij
A = blkdiag(A,A);

%% Assemble the matrix for divergence operator
% idea: Basis on coarse grids can be expanded from fine grids, i.e.,
%       lambda_{i,c} = lambda_{i,f} + 1/2*sum_{j \ in V(i)} lambda_{j,f}
%        where V(i) is the index of points on edges surrounding point i.
Dx = sparse(N,N);
Dy = sparse(N,N);
for j = 1:3 % loop for u index
    Dx = Dx + sparse(elem(:),repmat(elem(:,j),3,1),...
                     repmat(1/3*Dlambda(:,1,j).*area,3,1),N,N);
    Dy = Dy + sparse(elem(:),repmat(elem(:,j),3,1),...
                     repmat(1/3*Dlambda(:,2,j).*area,3,1),N,N);
end
Bf = [-Dx -Dy];
Bv = Bf(1:NC,:);
Be = Bf(NC+1:end,:);
D = 1/2*icdmat(double(edgeC),[1,1]);
B = Bv + D'*Be;

%% Assemble right hand side by 4-points quadrature rule
f1 = zeros(Nu,1);
f2 = zeros(Nu,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    % quadrature points in the barycentric coordinate
    [lambda,weight] = quadpts(option.fquadorder);
    nQuad = size(lambda,1);
    ft1 = zeros(NT,3);
    ft2 = zeros(NT,3);
    for p = 1:nQuad
        % quadrature points in the x-y coordinate
        pxy = lambda(p,1)*node(elem(:,1),:) ...
            + lambda(p,2)*node(elem(:,2),:) ...
            + lambda(p,3)*node(elem(:,3),:);
        % function values at quadrature points
        fp = pde.f(pxy);
        % evaluate fp outside.
        for j = 1:3
            ft1(:,j) = ft1(:,j) + fp(:,1).*lambda(p,j)*weight(p);
            ft2(:,j) = ft2(:,j) + fp(:,2).*lambda(p,j)*weight(p);
        end
    end
    ft1 = ft1.*repmat(area,1,3);
    ft2 = ft2.*repmat(area,1,3);
    f1 = accumarray(elem(:),ft1(:),[Nu 1]);
    f2 = accumarray(elem(:),ft2(:),[Nu 1]);
end

[AD,BD,f,g,u,p,ufreeDof,pDof] = getbdStokesisoP2P1;

%% Record assembeling time
assembleTime = toc;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(ufreeDof), return; end
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if length(f)+length(g) <= 1e3  % Direct solver for small size systems
        option.solver = 'direct';
    else          % Multigrid-type  solver for large size systems
        option.solver = 'asmg';
    end
end
solver = option.solver;

%% Solver
switch solver
    case 'direct'
        tic;
        bigA = [AD, BD'; ...
                BD, sparse(Np,Np)];
        bigF = [f; g];
        bigu = [u; p];
        bigFreeDof = [ufreeDof; 2*Nu+pDof];
        bigu(bigFreeDof) = bigA(bigFreeDof,bigFreeDof)\bigF(bigFreeDof);
        u = bigu(1:2*Nu);
        p = bigu(2*Nu+1:end);
        residual = norm(bigF - bigA*bigu);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'mg'
        option.solver  = 'WCYCLE';
        [u(ufreeDof),p,info] = mgstokes(A(ufreeDof,ufreeDof),B(:,ufreeDof),f(ufreeDof),g,...
                                        u(ufreeDof),p,elemC,ufreeDof,option);
    case 'asmg'
        [u(ufreeDof),p,info] = asmgstokes(A(ufreeDof,ufreeDof),B(:,ufreeDof),f(ufreeDof),g,...
                                          u,p,nodeC,elemC,bdFlagC,ufreeDof,option);
end

%% Post-process
if length(pDof)~=Np % p is unique up to a constant
    % impose the condition int(p)=0
    c = sum(mean(p(elemC),2).*areaC)/sum(areaC);
    p = p - c;
end

%% Output information
eqn = struct('A',AD,'B',BD,'f',f,'g',g,'ufreeDof',ufreeDof,'pDof',pDof);
info.assembleTime = assembleTime;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbdStokesisoP2P1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,BD,f,g,u,p,ufreeDof,pDof] = getbdStokesisoP2P1
        %% Initial set up
        g = zeros(Np,1);
        u = zeros(2*Nu,1);
        p = zeros(Np,1);
        ufreeDof = (1:Nu)';
        pDof = (1:Np)';
        if ~exist('bdFlag','var'), bdFlag = []; end
        if ~isfield(pde,'g_D'), pde.g_D = []; end
        if ~isfield(pde,'g_N'), pde.g_N = []; end
        if ~isfield(pde,'g_R'), pde.g_R = []; end

        %% Part 1: Find Dirichlet dof and modify the matrix
        % Find Dirichlet boundary dof: fixedDof and pDof
        isFixedDof = false(Nu,1);
        if ~isempty(bdFlag) % case: bdFlag is not empty
            allEdge = [elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])];
            Dirichlet = allEdge((bdFlag(:) == 1),:);
            isFixedDof(Dirichlet(:)) = true;
            fixedDof = find(isFixedDof);
            ufreeDof = find(~isFixedDof);
        end
        if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N)
            fixedDof = findboundary(elem);
            isFixedDof(fixedDof) = true;
            ufreeDof = find(~isFixedDof);
        end
        if isempty(fixedDof) % pure Neumann boundary condition
            % pde.g_N could be empty which is homogenous Neumann boundary condition
            fixedDof = 1;
            ufreeDof = 2:Nu;    % eliminate the kernel by enforcing u(1) = 0;
        end

        % Modify the matrix
        % Build Dirichlet boundary condition into the matrix AD by enforcing
        % AD(fixedDof,fixedDof)=I, AD(fixedDof,ufreeDof)=0, AD(ufreeDof,fixedDof)=0.
        % BD(:,fixedDof) = 0 and thus BD'(fixedDof,:) = 0.
        bdidx = zeros(2*Nu,1);
        bdidx(fixedDof) = 1;
        bdidx(Nu+fixedDof) = 1;
        Tbd = spdiags(bdidx,0,2*Nu,2*Nu);
        T = spdiags(1-bdidx,0,2*Nu,2*Nu);
        AD = T*A*T + Tbd;
        BD = B*T;

        %% Part 2: Find boundary edges and modify the right hand side f and g
        % Find boundary edges: Neumann and Robin
        Neumann = []; Robin = []; %#ok<*NASGU>
        if ~isempty(bdFlag)
            allEdge = [elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])];
            Neumann = allEdge((bdFlag(:)==2)|(bdFlag(:) == 3),:);
            Robin = allEdge((bdFlag(:) == 3),:);
        end
        if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
            % no bdFlag, only pde.g_N or pde.g_R is given in the input
            [tempvar,Neumann] = findboundary(elem);
            if ~isempty(pde.g_R)
                Robin = Neumann;
            end
        end

        % Neumann boundary condition
        if ~isempty(pde.g_N) && ~isempty(Neumann) && ~(isnumeric(pde.g_N) && (pde.g_N == 0))
            [lambda,w] = quadpts1(3);
            nQuad = size(lambda,1);
            ve = node(Neumann(:,1),:) - node(Neumann(:,2),:); % length of edge
            edgeLength = sqrt(sum(ve.^2,2));
            % update RHS
            gex = zeros(size(Neumann,1),2);   % x-component
            gey = zeros(size(Neumann,1),2);   % y-component
            for pp = 1:nQuad
                pxy = lambda(pp,1)*node(Neumann(:,1),:)+lambda(pp,2)*node(Neumann(:,2),:);
                gp = pde.g_N(pxy);
                gex(:,1) = gex(:,1) + w(pp)*edgeLength.*gp(:,1)*lambda(pp,1);
                gex(:,2) = gex(:,2) + w(pp)*edgeLength.*gp(:,1)*lambda(pp,2);
                gey(:,1) = gey(:,1) + w(pp)*edgeLength.*gp(:,2)*lambda(pp,1);
                gey(:,2) = gey(:,2) + w(pp)*edgeLength.*gp(:,2)*lambda(pp,2);
            end
            f1(1:N) = f1(1:N) + accumarray(Neumann(:), gex(:),[N,1]);
            f2(1:N) = f2(1:N) + accumarray(Neumann(:), gey(:),[N,1]);
        end
        f = [f1; f2];
        % The case non-empty Neumann but g_N=[] corresponds to the zero flux
        % boundary condition on Neumann edges and no modification is needed.

        % Dirichlet boundary conditions
        if ~isempty(fixedDof) && ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && (pde.g_D == 0))
            u1 = zeros(Nu,1);
            u2 = zeros(Nu,1);
            uD = pde.g_D(node(fixedDof,:));
            u1(fixedDof) = uD(:,1);
            u2(fixedDof) = uD(:,2);
            u = [u1;u2];
            f = f - A*u;  % bring affect of nonhomgenous Dirichlet bd condition
            g = g - B*u;  % to the right hand side
            g = g - mean(g);
            f(fixedDof)    = u1(fixedDof);
            f(fixedDof+Nu) = u2(fixedDof);
        end
        % The case non-empty Dirichlet but g_D=[] corresponds to the zero Dirichlet
        % boundary condition and no modification is needed.

        % modfiy pressure dof for pure Dirichlet
        if isempty(Neumann)
            pDof = (1:Np-1)';
        end

        ufreeDof = [ufreeDof; Nu+ufreeDof];
    end % end of function getbdStokesisoP2P1
end


function RR = SqueezeAngularFT(R);
% Usage:
%   RR = SqueezeAngularFT(R);
% Inputs:
%   R    Squared array of Fourier samples. Samples inside an
%        interior square are a priori zero.
%        a-priori zero.
% Outputs:
%   RR   Same as R but with the entries corresponding to the
%        interior square deleted
% Description:
%    R is an area of coefficients obtained after scale and angular
%    separation. Values inside an interior square are identically
%    zero. SqueezeAngularFT essentially removes this interior
%    square.

  nn = size(R);
  n2 = 2*nn(3);
  n = 2*n2;
  boxcnt = nn(2);
  boxlen = nn(4)/2;

  [ix,w] = DetailMeyerWindow([n2/4 n2/2],3);
  alpha_max = max(ix)./n2;
  Lmax = ceil(alpha_max*boxlen);
  mid = (boxlen - Lmax + 1):(boxlen + Lmax);

  RR = zeros(nn(1),nn(2),nn(3),2*Lmax);

  RR = R(:,:,:,mid);


for num =1:50
clearvars -except num;
warning off;
addpath(genpath(cd));

path1 = ['../road/ir/',num2str(num),'.jpg'];
path2 = ['../road/vi/',num2str(num),'.jpg'];
fused_path = ['result/',num2str(num),'.bmp'];

I=double(imread(path1))/255;
V=double(imread(path2))/255;

% image_left = ['../road/ir/',num2str(i),'.jpg'];
% image_right = ['../road/vi/',num2str(i),'.jpg'];
% fused_path = ['result/',num2str(i),'.bmp'];
%
% x{1}=imread(image_left);
% x{2}=imread(image_right);

calc_metric = 1; % Calculate the metrices is time consuming, it is used for quantitative evaluation. Set it to 0 if you do not want to do it.

%%
%The proposed GTF
nmpdef;
pars_irn = irntvInputPars('l1tv');

pars_irn.adapt_epsR   = 1;
pars_irn.epsR_cutoff  = 0.01;   % This is the percentage cutoff
pars_irn.adapt_epsF   = 1;
pars_irn.epsF_cutoff  = 0.05;   % This is the percentage cutoff
pars_irn.pcgtol_ini = 1e-4;
pars_irn.loops      = 5;
pars_irn.U0         = I-V;
pars_irn.variant       = NMP_TV_SUBSTITUTION;
pars_irn.weight_scheme = NMP_WEIGHTS_THRESHOLD;
pars_irn.pcgtol_ini    = 1e-2;
pars_irn.adaptPCGtol   = 1;

tic;
U = irntv(I-V, {}, 4, pars_irn);
t0=toc;

X=U+V;
X=im2gray(X);
imwrite(X,fused_path);
% imwrite(X,['F/GTF/',num2str(num),'.png'],'png');
% if calc_metric, Result = Metric(uint8(abs(I)*255),uint8(abs(V)*255),uint8(abs(X*255))); end

%%
%The laplacian pyramid as a compact image code(1983)
% level=4;
% tic;
% X1 = lp_fuse(I, V, level, 3, 3);       %LP
% t1=toc;
% X1=im2gray(X1);
% % imwrite(X1,['F/1/',num2str(num),'.png'],'png');
% if calc_metric, Result1 = Metric(uint8(abs(I)*255),uint8(abs(V)*255),uint8(abs(X1*255))); end

%%
%Image fusion by a ratio of low pass pyramid(1989)
% tic;
% X2 = rp_fuse(I, V, level, 3, 3);      %RP
% t2=toc;
% X2=im2gray(X2);
% imwrite(X2,['F/2/',num2str(num),'.png'],'png');
% if calc_metric, Result2 = Metric(uint8(abs(I)*255),uint8(abs(V)*255),uint8(abs(X2*255))); end

%%
% Wavelet
% fusion by taking the mean for both approximations and details
% tic;
% X3 = wfusimg(I,V,'db2',5,'mean','mean');
% X3=im2gray(X3);
% t3=toc;
% imwrite(X3,['F/3/',num2str(num),'.png'],'png');
% imwrite(X3,fused_path);
% if calc_metric, Result3 = Metric(uint8(abs(I)*255),uint8(abs(V)*255),uint8(abs(X3*255))); end

%%
%Pixel-and region-based image fusion with complex wavelets(2007)
% [M,N]=size(I);
% I4=imresize(I,[M+mod(M,2) N+mod(N,2)]);
% V4=imresize(V,[M+mod(M,2) N+mod(N,2)]);
% tic;
% X4 = dtcwt_fuse(I4, V4,level);           %DTCWT
% t4=toc;
% X4=im2gray(X4);
% imwrite(X4,['F/4/',num2str(num),'.png'],'png');
% if calc_metric, Result4 = Metric(uint8(abs(I4)*255),uint8(abs(V4)*255),uint8(abs(X4*255))); end
%
% %%
% %Remote sensing image fusion using the curvelet transform(2007)
% tic;
% X5 = curvelet_fuse(I4, V4,level+1);      %CVT
% t5=toc;
% X5=im2gray(X5);
% imwrite(X5,['F/5/',num2str(num),'.png'],'png');
% if calc_metric, Result5 = Metric(uint8(abs(I4)*255),uint8(abs(V4)*255),uint8(abs(X5*255))); end

%%
%Image Fusion technique using Multi-resolution singular Value decomposition(2011)
%apply MSVD
% tic;
% [Y1, U1] = MSVD(I4);
% [Y2, U2] = MSVD(V4);
%
% %fusion starts
% X6.LL = 0.5*(Y1.LL+Y2.LL);
%
% D  = (abs(Y1.LH)-abs(Y2.LH)) >= 0;
% X6.LH = D.*Y1.LH + (~D).*Y2.LH;
% D  = (abs(Y1.HL)-abs(Y2.HL)) >= 0;
% X6.HL = D.*Y1.HL + (~D).*Y2.HL;
% D  = (abs(Y1.HH)-abs(Y2.HH)) >= 0;
% X6.HH = D.*Y1.HH + (~D).*Y2.HH;
%
% %XX = [X.LL, X.LH; X.HL, X.HH];
% U = 0.5*(U1+U2);
%
% %apply IMSVD
% X6 = IMSVD(X6,U);
% t6=toc;
% X6=im2gray(X6);
% imwrite(X6,fused_path);
% if calc_metric, Result6 = Metric(uint8(abs(I4)*255),uint8(abs(V4)*255),uint8(abs(X6*255))); end
%
% %%
% %Image Fusion with Guided Filtering(2013)
% %run('F:\Code\Lichang\16.Image fusion total variation\Image fusion with guided filtering\Demo.m');
% I7=load_images('.\img',1);% the folder of source image
% tic;
% X7 = double(GFF(I7,5,10^-6,5,10^-6));
% %X7=rgb2gray(X7);
% t7=toc;
% imwrite(X7,['F/7/',num2str(num),'.png'],'png');
% %if calc_metric, Result7 = Metric(uint8(abs(I)*255),uint8(abs(V)*255),uint8(X7)); end
%
% %%
% %A general framework for image fusion based on multi-scale transform and sparse representation(2014)
% overlap = 6;
% epsilon=0.1;
% level=4;
% load('D_100000_256_8.mat');
% tic;
% X8= lp_sr_fuse(I,V,level,3,3,D,overlap,epsilon);      %LP-SR
% t8=toc;
% X8=im2gray(X8);
% imwrite(X8,['F/8/',num2str(num),'.png'],'png');
% if calc_metric, Result8 = Metric(uint8(abs(I)*255),uint8(abs(V)*255),uint8(abs(X8*255))); end
end


% Copyright (C) 2016, 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 speed_demo_scara
global robot
% joint position
q = [pi/4 pi/4 0.2 0]';
% joint speed
qd = [1 1 1 1]';
%robot = load_robot

J = manipulator_jacobian(robot, q);

Ve=J*qd;

%plot speed
T = directkinematic(robot, q);
p0 = T(1:3,4);
drawrobot3d(robot, q)
draw_vector(Ve(1:3), p0, 'linear speed V', 2)
draw_vector(Ve(4:6), p0, 'angular speed W', 1)


%{
load('dataset/trafficdb/traffic_patches.mat');
[M,m,n,p] = convert_video3d_to_2d(im2double(imgdb{100}));
out = run_algorithm('MC', 'FPC', M, [])
show_results(M.*out.Omega,out.L,out.S,out.O,p,m,n);
%}

maxiter = 500;
mu_final = .01; tol = 1e-6;
MIdx = M(Idx);

fprintf('\nSolving by FPC...\n');
[U,S,V,numiter] = FPC(size(M),Idx,MIdx,mu_final,maxiter,tol);

L = U*S*V'; % low-rank
S = M - L; % sparse


function [duplicated_elems] = duplicateElems(elems, num_duplicates)
%DUPLICATEELEMS takes an elems vector of length N and num_duplicates vector
% of length N, and produces a duplicated_elems vector of length
% sum(num_duplicates). Each element in num_duplicates specifies how many
% times an element in elems needs to be replicated.
%
% E.g. on how to do it in a loopy way:
%
%   elems = [2,4,6,7,10,9,20,15];
%   num_duplicates = [0,2,4,0,3,2,0,0];
%   duplicated_elems = [];
%   % loopy way to do it
%   for idx = 1:length(elems)
%       duplicated_elems = [duplicated_elems, ...
%                           repmat(elems(idx), 1, num_duplicates(idx))];
%   end
%   disp(duplicated_elems)
%
%
% @authors:     Ahmad Humayun
% @contact:     ahumayun@cc.gatech.edu
% @affiliation: Georgia Institute of Technology
% @date:        Fall 2013 - Summer 2014

    assert(isvector(elems), 'elems should be a vector');
    assert(isvector(num_duplicates), 'num_duplicates should be a vector');
    assert(numel(elems) == numel(num_duplicates), ...
        'elems and num_duplicates should be of the same size');

    num_duplicates = num_duplicates(:);

    % remove elements which weren't replicated
    elems(num_duplicates <= 0) = [];
    num_duplicates(num_duplicates <= 0) = [];

    % if no duplication element is present in num_duplicates
    if isempty(num_duplicates)
        duplicated_elems = [];
        return;
    end

    duplicate_idxs = zeros(sum(num_duplicates), 1);
    % mark the starting locations
    duplicate_idxs([1; cumsum(num_duplicates(1:end-1))+1]) = 1;
    duplicate_idxs = cumsum(duplicate_idxs);
    duplicated_elems = elems(duplicate_idxs);
end