function varargout = double_pendulum2(varargin)

% the function simulate double pendulum, using 4th-order Runge-Kutta algorithm for the diffrential equations.
% the diffrential equation are very similar to eq. (12) and (13) here:
%                           http://home1.fvcc.edu/~dhicketh/DiffEqns/spring09projects/LauraStickel/Double%20Pendulum.pdf
% where omega=d(theta)/dt
% Moshe Lindner & Orit Peleg, August 2010 (C)

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @double_pendulum_OpeningFcn, ...
                   'gui_OutputFcn',  @double_pendulum_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before double_pendulum is made visible.
function double_pendulum_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to double_pendulum (see VARARGIN)

% Choose default command line output for double_pendulum
handles.output = hObject;
handles.len1=.5;
handles.len2=.5;
handles.mass1=.5;
handles.mass2=.5;
handles.t1=0;
handles.t2=0;
handles.w1=0;
handles.w2=0;
handles.G=9.8;
pend_plot(handles);

% Update handles structure
guidata(hObject, handles);

% UIWAIT makes double_pendulum wait for user response (see UIRESUME)
% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.
function varargout = double_pendulum_OutputFcn(hObject, eventdata, handles)
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;

% --- Executes on button press in start_stop.
function start_stop_Callback(hObject, eventdata, handles)
% hObject    handle to start_stop (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(hObject,'string','Stop');
set(handles.omega1,'enable','off');
set(handles.theta1,'enable','off');
set(handles.omega2,'enable','off');
set(handles.theta2,'enable','off');
set(handles.length_rat,'enable','off');
set(handles.mass_rat,'enable','off');
set(handles.reset,'enable','off');
set(handles.gravity,'enable','off');
i=1;
while get(hObject,'value')==1
    if (get(handles.traj,'value'))==1
        x_1=handles.len1*sin(handles.t1);
        y_1=-handles.len1*cos(handles.t1) ;
        x_2=handles.len2*sin(handles.t2) + x_1;
        y_2=-handles.len2*cos(handles.t2) + y_1;
        handles.trajectory(1,i)=x_2;
        handles.trajectory(2,i)=y_2;
        i=i+1;
        if i>400
            i=1;
        end
    elseif i~=1
        handles.trajectory=[];
        i=1;
    end
    [handles.t1,handles.t2,handles.w1,handles.w2]=db_pendulum(handles.t1,handles.t2,handles.w1,handles.w2,handles.mass1,handles.mass2,handles.len1,handles.len1,handles.G,.01);
    pend_plot(handles);
    set(handles.omega1,'string',num2str(handles.w1));
    set(handles.theta1,'string',num2str(handles.t1));
    set(handles.omega2,'string',num2str(handles.w2));
    set(handles.theta2,'string',num2str(handles.t2));
    guidata(hObject, handles);
end
set(hObject,'string','Start');
set(handles.omega1,'enable','on');
set(handles.theta1,'enable','on');
set(handles.omega2,'enable','on');
set(handles.theta2,'enable','on');
set(handles.length_rat,'enable','on');
set(handles.mass_rat,'enable','on');
set(handles.reset,'enable','on');
set(handles.gravity,'enable','on');
% --- Executes on slider movement.
function length_rat_Callback(hObject, eventdata, handles)
% hObject    handle to length_rat (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.len1=get(hObject,'value');
handles.len2=1-handles.len1;
pend_plot(handles);
guidata(hObject, handles);
% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

% --- Executes during object creation, after setting all properties.
function length_rat_CreateFcn(hObject, eventdata, handles)
% hObject    handle to length_rat (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end

% --- Executes on slider movement.
function mass_rat_Callback(hObject, eventdata, handles)
% hObject    handle to mass_rat (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.mass1=get(hObject,'value');
handles.mass2=1-handles.mass1;
pend_plot(handles);
guidata(hObject, handles);
% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

% --- Executes during object creation, after setting all properties.
function mass_rat_CreateFcn(hObject, eventdata, handles)
% hObject    handle to mass_rat (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end

function omega1_Callback(hObject, eventdata, handles)
% hObject    handle to omega1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.w1=str2num(get(hObject,'String'));
energy(handles)
guidata(hObject, handles);
% Hints: get(hObject,'String') returns contents of omega1 as text
%        str2double(get(hObject,'String')) returns contents of omega1 as a double

% --- Executes during object creation, after setting all properties.
function omega1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to omega1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function theta1_Callback(hObject, eventdata, handles)
% hObject    handle to theta1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.t1=str2num(get(hObject,'String'));
pend_plot(handles);
guidata(hObject, handles);
% Hints: get(hObject,'String') returns contents of theta1 as text
%        str2double(get(hObject,'String')) returns contents of theta1 as a double

% --- Executes during object creation, after setting all properties.
function theta1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to text_1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function omega2_Callback(hObject, eventdata, handles)
% hObject    handle to omega2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.w1=str2num(get(hObject,'String'));
energy(handles)
guidata(hObject, handles);
% Hints: get(hObject,'String') returns contents of omega2 as text
%        str2double(get(hObject,'String')) returns contents of omega2 as a
%        double

% --- Executes during object creation, after setting all properties.
function omega2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to omega2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

function theta2_Callback(hObject, eventdata, handles)
% hObject    handle to theta2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.t2=str2num(get(hObject,'String'));
pend_plot(handles);
guidata(hObject, handles);
% Hints: get(hObject,'String') returns contents of theta2 as text
%        str2double(get(hObject,'String')) returns contents of theta2 as a double

% --- Executes during object creation, after setting all properties.
function theta2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to theta2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

% --- Executes on button press in traj.
function traj_Callback(hObject, eventdata, handles)
% hObject    handle to traj (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.trajectory=[];
guidata(hObject, handles);
% Hint: get(hObject,'Value') returns toggle state of traj

% --- Executes on button press in reset.
function reset_Callback(hObject, eventdata, handles)
% hObject    handle to reset (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.len1=.5;
handles.len2=.5;
handles.mass1=.5;
handles.mass2=.5;
handles.t1=0;
handles.t2=0;
handles.w1=0;
handles.w2=0;
handles.G=9.8;
set(handles.gr,'string','9.8')
set(handles.gravity,'value',9.8)
set(handles.omega1,'string','0')
set(handles.omega2,'string','0')
set(handles.theta1,'string','0')
set(handles.theta2,'string','0')
set(handles.mass_rat,'value',0.5)
set(handles.length_rat,'value',0.5)
pend_plot(handles);
guidata(hObject, handles);

function pend_plot(data);
[xx yy zz]=cylinder(0.1,20);
    x_1=data.len1*sin(data.t1);
    y_1=-data.len1*cos(data.t1) ;
    x_2=data.len2*sin(data.t2) + x_1;
    y_2=-data.len2*cos(data.t2) + y_1;
hold off
plot([0 x_1 x_2],[0 y_1 y_2],'k','linewidth',2);
hold on
if get(data.traj,'value') ==1
    plot(data.trajectory(1,:),data.trajectory(2,:),'r.','markersize',3)
end
fill(x_1+xx(1,:).*(data.mass1)^.3,y_1+yy(1,:).*(data.mass1)^.3,'b')
fill(x_2+xx(1,:).*(data.mass2)^.3,y_2+yy(1,:).*(data.mass2)^.3,'b')
set(gca,'xtick',[],'ytick',[])
axis(1.2*[-1 1 -1 1]);
box on;
drawnow
energy(data)

function [theta_1 theta_2 omega_1 omega_2]=db_pendulum(theta1, theta2, w1, w2, mass1, mass2, d1, d2, G, dt)
l1 = dt * (l(w1));
m1 = dt * (m(w2));
f1 = dt * (f(theta1, theta2, w1, w2, mass1, mass2, d1, d2, G));
g1 = dt * (g(theta1, theta2, w1, w2, mass1, mass2, d1, d2, G));

l2 = dt * (l(w1+(f1/2.)));
m2 = dt * (m(w2+(g1/2.)));
f2 = dt * (f(theta1+(l1/2.),theta2+(m1/2.),w1+(f1/2.),w2+(g1/2.), mass1, mass2, d1, d2, G));
g2 = dt * (g(theta1+(l1/2.),theta2+(m1/2.),w1+(f1/2.),w2+(g1/2.), mass1, mass2, d1, d2, G));

l3 = dt * (l(w1+(f2/2.)));
m3 = dt * (m(w2+(g2/2.)));
f3 = dt * (f(theta1+(l2/2.),theta2+(m2/2.),w1+(f2/2.),w2+(g2/2.), mass1, mass2, d1, d2, G));
g3 = dt * (g(theta1+(l2/2.),theta2+(m2/2.),w1+(f2/2.),w2+(g2/2.), mass1, mass2, d1, d2, G));

l4 = dt * (l(w1+f3));
m4 = dt * (m(w2+g3));
f4 = dt * (f(theta1+(l3),theta2+(m3),w1+(f3),w2+(g3), mass1, mass2, d1, d2, G));
g4 = dt * (g(theta1+(l3),theta2+(m3),w1+(f3),w2+(g3), mass1, mass2, d1, d2, G));
theta_1 = theta1 + (l1+(2.*l2)+(2.*l3)+l4)/6.;
theta_2 = theta2 + (m1+(2.*m2)+(2.*m3)+m4)/6.;
omega_1 = w1 + (f1+(2.*f2)+(2.*f3)+f4)/6.;
omega_2 = w2 + (g1+(2.*g2)+(2.*g3)+g4)/6.;

function f1=f(theta1, theta2, w1, w2, mass1, mass2, d1, d2, G)
f1= (  (( -d2*mass2*w2*w2*sin(theta1- theta2)/(d1*(mass1+mass2)) )  - ...
    (  mass2*w1*w1*cos(theta1-theta2)*sin(theta1- theta2)/(mass1+mass2) )  +...
    (  mass2*G*cos(theta1- theta2)*sin(theta2)/(d1*(mass1+mass2))  )  -...
    (  G*sin(theta1)/d1)...
    ) /  ( 1. - (mass2*cos(theta1- theta2)*cos(theta1- theta2)/(mass1+mass2)))  );
function g1=g(theta1, theta2 ,w1, w2, mass1, mass2, d1, d2, G)
g1=(( (-G*sin(theta2)/d2) + (mass2*w2*w2*cos(theta1-theta2)*sin(theta1-theta2)/(mass1+mass2)) +...
    (G*cos(theta1-theta2)*sin(theta1)/d2) + (d1*w1*w1*sin(theta1-theta2)/d2)  )  /  (1. - (mass2*cos(theta1-theta2)*cos(theta1-theta2)/(mass1+mass2))) );
function L1=l(w1)
L1=w1;
function M1=m(w2)
M1= w2;

% --- Executes on slider movement.
function gravity_Callback(hObject, eventdata, handles)
% hObject    handle to gravity (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
handles.G=get(hObject,'value');
set(handles.gr,'string',num2str(handles.G));
guidata(hObject, handles);

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

% --- Executes during object creation, after setting all properties.
function gravity_CreateFcn(hObject, eventdata, handles)
% hObject    handle to gravity (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background.
if isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor',[.9 .9 .9]);
end

function energy(data)
  Epot1=-data.mass1*data.G*data.len1*(cos(data.t1)-1.000) ;
  Epot2=-data.mass2*data.G*((cos(data.t1)-1.000)*data.len1+(cos(data.t2)-1.000)*data.len2);
  Ekin1=0.5*data.mass1*(data.w1*data.len1).^2;
  Ekin2=0.5*data.mass2*(   (data.w2*data.len2*cos(data.t2) +data.w1*data.len1*cos(data.t1)).^2 + (data.w2*data.len2*sin(data.t2) +data.w1*data.len1*sin(data.t1)).^2 );
  Etot=Ekin1+Ekin2+Epot1+Epot2;

  set(data.U1,'string',num2str(Epot1));
  set(data.U2,'string',num2str(Epot2));
  set(data.T1,'string',num2str(Ekin1));
  set(data.T2,'string',num2str(Ekin2));
  set(data.totE,'string',num2str(Etot));


function STEPWISE = stepwise_tor(dat,y,varargin)
% :Usage:
% ::
%
%     STEPWISE = stepwise_tor(dat, y, [pred. names], [alpha])
%
% Stepwise regression using Matlab with a couple of extras:
%
% Print omnibus F-values for stepwise regression
%
% get adjusted R-squared
%
% save output structure
%
% ..
%    tor wager
% ..

alph = .05;
if length(varargin) > 1
    alph = varargin{2};
end

if length(varargin) > 0
    nms = varargin{1};
else
    for i = 1:size(dat,2), nms{i} = ['V ' num2str(i)]; end
end

    % Add logistic regression for categorical 1/0 DVs!

    [STEPWISE.b,STEPWISE.se,STEPWISE.pval, ...
    STEPWISE.inmodel,stats] = ...
    stepwisefit(dat,y,'penter',alph,'display','off');

    t = stats.TSTAT;
    STEPWISE.t = t;

    S = STEPWISE;
    tabled = [S.b S.se t S.inmodel' S.pval]; tabnames = {'Beta' 'Std. Err.' 't-value' 'In Model' 'p-value'};
    print_matrix(tabled, tabnames,nms);

    N = stats.dfe+stats.df0;
    r2 = (stats.SStotal-stats.SSresid) ./ stats.SStotal;
    adjr2 = 1 - (1-r2)*((N - 1) ./ (N - stats.df0 - 1));

    fprintf(1,'\nOmnibus F(%3.0f,%3.0f) = %3.2f, RMSE = %3.2f, p = %3.6f, Adj R^2 = %3.2f\n', ...
    stats.df0,stats.dfe,stats.fstat, ...
    stats.rmse,stats.pval,adjr2);
    stats.adjr2 = adjr2;
    STEPWISE.stats = stats;

 return


% Figure 5.40 Feedback Control of Dynamic Systems, 5e
%                   Franklin, Powell, Emami
%
% attitude hold auto pilot for Dakota
% root locus with integral control

clf
Wsp=6.3;  % short period
Zsp=.4;
Wph=.24;  % phugoid mode
Zph=.06;
Kg=160;    % gain so that 1" of control input produces 5 deg

NUMsp=[1 Zsp*Wsp];
NUMph=[1 3*Wph];
DENsp=[1 2*Zsp*Wsp Wsp^2];
DENph=[1 2*Zph*Wph Wph^2];

numG=Kg*conv(NUMsp,NUMph);
denG=conv(DENsp,DENph);
zeroG=roots(numG);

% lead compensation

numD=[1 3];
denD=[1 20];

num=conv(numG,numD);
den=conv(denG,denD);
zeroD=roots(numD);

K=1.5;
[r2]=rlocus(num,den,K);

% now add integral control

polesI=[r2';0];  % add integral control pole
denI=poly(polesI);

axis('square')
subplot(121)
axis([-30 0 -15 15])
plot(polesI,'x'),grid
hold on
plot(zeroD,0,'o')
plot(zeroG(1),0,'o',zeroG(2),0,'o')

ki0=.01:.05:.51;
ki1=1:.5:10;
ki2=12:5:102;
ki3=120:20:1000;
ki=[ki0 ki1 ki2 ki3 5000];
[R]=rlocus(K*num,denI,ki);
plot(R,'-')
axis([-25 2 -15 15]);
title ('Fig. 5.40 Root Locus vs. K_I for autopilot')
xlabel('Re(s)')
ylabel('Im(s)')

KI=.15;
[R1]=rlocus(K*num,denI,KI);
damp(R1')
plot(R1,'*')
hold off

% now zoom in to origin

subplot(122)
plot(polesI,'x'),grid
axis([-4 0 -2 2])
hold on
xlabel('Re(s)')
ylabel('Im(s)')
plot(zeroD,0,'o')
plot(zeroG(1),0,'o',zeroG(2),0,'o')
plot(R,'-')
plot(R1,'*')

hold off
axis('normal')

subplot(111)


function [slug] = kg2slug(kg)
% Convert mass from kilograms to slugs.
% Chad Greene 2012
slug = kg/14.5939;


function [err1, err2] = llGrad(p1,p2,type,tolGrad,tolEval)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% [err1,err2] = llGrad(p1, p2, type [, tolGrad ,tolEval])
%
%    Compute the gradient with respect to the means of p1,p2 of the
%       average log-likelihood of p1 evaluated at the locations of p2
%       (p1=p2 => gradient of a resubstitution estimate of negentropy)
%    type: what to take gradient with respect to.
%          0 = means, 1 = bw (variance), 2=weights
%    tolGrad is an acceptable error tolerance on the gradient values
%    tolEval is a *percent* tolerance on the evaluation done as a
%       subroutine in the calculation; it should be tight enough to allow
%       tolGrad to be achieved but beyond that, larger => faster.
%
% Specifically, using the resubstition estimate
%    \hat H = \sum_j v_j \log \sum_i w_i K(x_i - y_j)
% The outputs are
%    err1(:,i) = \sum_j v_j 1/p(y_j) dp(y_j)/dx_i = E_y[ 1/p dp/dx_i ]
%    err2(:,j) = v_j 1/p(y_j) p'(y_j)
%
% Note: [err1,err2] = llGrad(p1, type [, tolGrad ,tolEval]) computes the leave-
%   one-out resub. estimate, similar to llGrad(p1,p1,type...)
%
% Some useful estimates which can be made using this function:
%
% ENTROPY GRADIENT
%    [err1, err2] = llGrad(p,p,1e-3,1e-3);
%    p -= (err1 + err2)    moves p in the direction of increasing entropy
%
% KL-DIVERGENCE GRADIENT
%    [errXX1, errXX2] = llGrad(p1,p1,1e-3,1e-3);
%    [errXYY, errXYX] = llGrad(p2,p1,1e-3,1e-3);
%    err1 = (errXX1 + errXX2 - errXYX);
%    err2 = (-errXYY);
%    p1 += err1, p2 += err2  moves p1,p2 in the direction of increasing KLDiv
%
% See also:  klGrad, miGrad, entropyGrad, adjustPoints
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2003 Alexander Ihler; distributable under GPL -- see README.txt

%#mex
error('MEX-file kde/llGrad not found -- please recompile if necessary');

% Incorrect:  Mean shift corresponds to an estimate of 1/p(y) p'(y) where
%    p'(y) is computed using the Epan. kernel while p(y) is computed using
%    a uniform kernel with the same support.  This does *not* correspond to
%    the derivative of log-likelihood for any KDE (that I know of)
%
% MEAN-SHIFT
%    For equal weight uniform bandwidth Epan. kernel densities,
%      the mean shift algorithm (Comaniciu '99) is given by
%    [err1, err2] = llGrad(p,p,1e-3,1e-3);
%    p += .5*getNpts(p)*getBW(p,1)^2 * err2
%


%% Transient diffusion equation
%% PDE and boundary conditions
% The transient diffusion equation reads
%
% $$\alpha\frac{\partial c}{\partial t}+\nabla.\left(-D\nabla c\right)=0,$$
%
% where $c$ is the independent variable (concentration, temperature, etc)
% , $D$ is the diffusion coefficient, and $\alpha$ is a constant.
% Written by Ali A. Eftekhari
% Last checked: June 2021
clc

%% Define the domain and create a mesh structure
L = 50;  % domain length
Nx = 10; % number of cells
m = createMesh3D(Nx,Nx+3,Nx+6, L/2,2*pi,L);
%% Create the boundary condition structure
BC = createBC(m); % all Neumann boundary condition structure
% BC.left.a(:) = 0; BC.left.b(:)=1; BC.left.c(:)=0; % left boundary
BC.right.a(:) = 0; BC.right.b(:)=1; BC.right.c(:)=0; % right boundary
% BC.top.a(:) = 0; BC.top.b(:)=1; BC.top.c(:)=3; % top boundary
% BC.bottom.a(:) = 0; BC.bottom.b(:)=1; BC.bottom.c(:)=0; % bottom boundary
BC.top.periodic=1;
BC.back.a(:) = 0; BC.back.b(:)=1; BC.back.c(:)=3; % back boundary
BC.front.a(:) = 0; BC.front.b(:)=1; BC.front.c(:)=0; % front boundary
%% define the transfer coeffs
D_val = 1;
D = createCellVariable(m, D_val);
alfa = createCellVariable(m, 1);
u = createFaceVariable(m, [0,0,0.5]);
%% define initial values
c_init = 1;
c_old = createCellVariable(m, c_init,BC); % initial values
c = c_old; % assign the old value of the cells to the current values
%% loop
Dave = harmonicMean(D);
Mdiff = diffusionTerm(Dave);
[Mbc, RHSbc] = boundaryCondition(BC);
FL = fluxLimiter('Superbee');
Mconv = convectionTvdTerm(u, c, FL);
dt = 1; % time step
final_t = 50;
for t=dt:dt:final_t
    [M_trans, RHS_trans] = transientTerm(c_old, dt, alfa);
    M = M_trans-Mdiff+Mbc+Mconv;
    RHS = RHS_trans+RHSbc;
    c = solvePDE(m,M, RHS);
    c_old = c;
    figure(1);visualizeCells(c);drawnow;
end
%% visualization
 figure(1);visualizeCells(c);


function [t,x] = prz(bits, bitrate)
% PRZ Encode bit string using polar RZ code.
%   [T, X] = PRZ(BITS, BITRATE) encodes BITS array using polar RZ
%   code with given BITRATE. Outputs are time T and encoded signal
%   values X.

% Copyright (c) 2013 Yuriy Skalko <yuriy.skalko@gmail.com>

T = length(bits)/bitrate; % full time of bit sequence
n = 200;
N = n*length(bits);
dt = T/N;
t = 0:dt:T;
x = zeros(1,length(t)); % output signal

for i = 0:length(bits)-1
  if bits(i+1) == 1
    x(i*n+1:(i+0.5)*n) = 1;
    x((i+0.5)*n+1:(i+1)*n) = 0;
  else
    x(i*n+1:(i+0.5)*n) = -1;
    x((i+0.5)*n+1:(i+1)*n) = 0;
  end
end


function [v,d,z]=soundspeed(t,p,m,g)
%SOUNDSPEED gives the speed of sound, density of air and acostuc impedance as a function of temp & pressure [V,D,Z]=(T,P,M,G)
%
%  Inputs:  T        air temperature in Celsius  [20 deg C]
%           P        air pressure [1 atm]
%           M        average molecular weight of air [0.0289644 kg/mol]
%           G        adiabatic constant for air [1.4]
%
% Outputs:  V        is the speed of sound in m/s
%           D        density of air in kg/m^3
%           Z        characteristic impedance of air Pa.s/m

% Notes: (1) Sound pressure is often measured in dB (SPL) relative to 20uPa [20*log10(p/p0)]
%            Sound pressure is inversely proportional to distance.
%        (2) Sound intensity is often measured indB relative to pW/m^2,[10*log10(J*10^12)]
%            Intensity is inversely proportional to distance squared.
%        (3) Intensity * impedance = pressure^2, so with the default values, 1 pW/m^2 = 20.33 uPa
%            So: X dB (SPL) = X-93.98 dB (Pa) = X-0.14 dB (pW/m^2) =  X-120.14 dB (W/m^2)
%        (4) The default air pressure (which does not affect sound speed) in various units is:
%            1 atm = 101325 Pa = 1.01325 bar = 1.0332 at = 760 torr = 14.696 psi

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

if nargin<4
    g=1.4;
    if nargin<3
        m=0.0289644;      % gm/mol
        if nargin<2
            p=1;
            if nargin<1
                t=20;
            end
        end
    end
end
p=p*101325; % convert pressure: atm to pascal
k=t+273.15;  % absolute temperature
r=8.3144;  % J/(mol K) universal gas constant
d=p*m/(r*k);
v=sqrt(g*r*k/m);
z=v*d;


function [C_pos,C_neg,Ctot_pos,Ctot_neg] = clustering_coef_wu_sign(W,coef_type)
%CLUSTERING_COEF_WU_SIGN     Multiple generalizations of the clustering coefficient
%
%   [C_pos,C_neg,Ctot_pos,Ctot_neg] = clustering_coef_wu_sign(W,coef_type);
%
%   The weighted clustering coefficient is the average weight or intensity
%   of all triangles associated with each node.
%
%   Inputs:
%       W,
%           Weighted undirected connection matrix
%
%       corr_type,
%           Desired type of clustering coefficient.
%           Options:
%           1,  (default) Onnela et al. formula, used in original
%               clustering_coef_wu.m. Computed separately for positive &
%               negative weights.
%           2,  Zhang & Horvath formula, similar to Onnela formula except
%               denominator of Onnela formula relies on binarizing the
%               network whereas this denominator is based on weight value,
%               which reduces the sensitivity of this measure to the
%               weights directly connected to the node of interest.
%               Computed separately for positive & negative weights.
%           3,  Constantini & Perugini's generalization of the Zhang &
%               Horvath formula. This formula takes both positive &
%               negative weights into account simultaneously, & is
%               particularly sensitive to non-redundnacy in path
%               information based on sign (i.e., when two weights are
%               positive & one negative, or all three are negative, both of
%               which indicate that the weight of the third path is not
%               redundant information). Produces only one value.
%
%
%   Outputs:
%       C_pos/C_neg,
%           Clustering coefficient vector for positive/negative weights.
%           For the third option, only one vector is outputted (as C_pos).
%       Ctot_pos/Ctot_neg,
%           Mean clustering coefficient for positive and negative weights.
%
%   References:
%       Onnela et al. (2005) Phys Rev E 71:065103
%       Zhang & Horvath (2005) Stat Appl Genet Mol Biol 41:1544-6115
%       Costantini & Perugini (2014) PLOS ONE 9:e88669
%
%
%   Contributor: Jeff Spielberg, Boston University, 2014-2015
%                (script based on clustering_coef_wu.m)

%
%   Modification History:
%   May 2014: Added computation of pos & neg weights separately &
%             computation of mean coefficient (Jeff Spielberg)
%   May 2015: Added computation of Zhang & Horvath and Constantini &
%             Perugini formulas (Jeff Spielberg)

if ~exist('coef_type','var')
    coef_type = 1;
end

n            = length(W);                   %number of nodes
W(1:n+1:end) = 0;

switch coef_type
    case 1
        W_pos                = W.*(W>0);
        K_pos                = sum(W_pos~=0,2);
        cyc3_pos             = diag((W_pos.^(1/3))^3);
        K_pos(cyc3_pos == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_pos                = cyc3_pos./(K_pos.*(K_pos-1));         %clustering coefficient
        Ctot_pos             = mean(C_pos);

        W_neg                = -W.*(W<0);
        K_neg                = sum(W_neg~=0,2);
        cyc3_neg             = diag((W_neg.^(1/3))^3);
        K_neg(cyc3_neg == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_neg                = cyc3_neg./(K_neg.*(K_neg-1));         %clustering coefficient
        Ctot_neg             = mean(C_neg);
    case 2
        W_pos    = W.*(W>0);
        cyc3_pos = zeros(n,1);
        cyc2_pos = zeros(n,1);
        for i = 1:n
            for j = 1:n
                for q = 1:n
                    cyc3_pos(i) = cyc3_pos(i)+(W_pos(j,i)*W_pos(i,q)*W_pos(j,q));
                    if j~=q
                        cyc2_pos(i) = cyc2_pos(i)+(W_pos(j,i)*W_pos(i,q));
                    end
                end
            end
        end
        cyc2_pos(cyc3_pos == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_pos                   = cyc3_pos./cyc2_pos;         %clustering coefficient
        Ctot_pos                = mean(C_pos);

        W_neg    = -W.*(W<0);
        cyc3_neg = zeros(n,1);
        cyc2_neg = zeros(n,1);
        for i = 1:n
            for j = 1:n
                for q = 1:n
                    cyc3_neg(i) = cyc3_neg(i)+(W_neg(j,i)*W_neg(i,q)*W_neg(j,q));
                    if j~=q
                        cyc2_neg(i) = cyc2_neg(i)+(W_neg(j,i)*W_neg(i,q));
                    end
                end
            end
        end
        cyc2_neg(cyc3_neg == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_neg                   = cyc3_neg./cyc2_neg;         %clustering coefficient
        Ctot_neg                = mean(C_neg);
    case 3
        cyc3         = zeros(n,1);
        cyc2         = zeros(n,1);

        for i = 1:n
            for j = 1:n
                for q = 1:n
                    cyc3(i) = cyc3(i)+(W(j,i)*W(i,q)*W(j,q));
                    if j~=q
                        cyc2(i) = cyc2(i)+(W(j,i)*W(i,q));
                    end
                end
            end
        end

        cyc2(cyc3 == 0) = inf;             %if no 3-cycles exist, make C=0 (via K=inf)
        C_pos           = cyc3./cyc2;         %clustering coefficient
        Ctot_pos        = mean(C_pos);
        C_neg           = nan(size(C_pos));
        Ctot_neg        = nan(size(Ctot_pos));
end


function J = computeCost(traj_obj, gamma)

tau_vec = traj_obj.tau_vec;

A_yaw = augmentA_yaw(tau_vec);
% Permutation matrix to rearrange b
C_yaw = permutMat_yaw(tau_vec);
% Cost matrix
Q_yaw = augmentQ(tau_vec,2);
% C*A^(-T)*Q*A^(-1)*C^(T) = R
R_yaw = comp_R_appA(A_yaw,C_yaw,Q_yaw);
% Known derivatives including position.
bFyaw = bF_yaw(0, tau_vec);
b_srtdYaw = b_srtd_yaw(R_yaw, bFyaw);
% P_yaw = (C_yaw*A_yaw)\b_srtdYaw;
J_yaw = b_srtdYaw'*R_yaw*b_srtdYaw;

% Compute cost
J = traj_obj.J + J_yaw + gamma*sum(tau_vec);

end


function lambda = bis_eigenvalues ( alpha, beta, n )

%*****************************************************************************80
%
%% BIS_EIGENVALUES returns the eigenvalues of the BIS matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, BETA, the scalars which define the
%    diagonal and first superdiagonal of the matrix.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real LAMBDA(N,1), the eigenvalues of the matrix.
%
  lambda(1:n,1) = alpha;

  return
end


function varargout = PlotPaths(varargin)

% Begin initialization code
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @GUI_OpeningFcn, ...
                   'gui_OutputFcn',  @GUI_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code

% --- Executes just before GUI is made visible.
function GUI_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to GUI (see VARARGIN)

% Choose default command line output for GUI
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

% UIWAIT makes GUI wait for user response (see UIRESUME)
% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.
function varargout = GUI_OutputFcn(hObject, eventdata, handles)
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;

% --- Executes on button press in pushbutton1.
function pushbutton1_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
kappa=2;
theta=1;
sigma=.4;
X0=1;
n=1000;
m=7;
W=randn(n,m);
X=zeros(n,m);
X(1,:)=X0;

t=[0:1/n:1];

for j=1:m
    for i=1:n
        X(i+1,j)=X(i,j)+kappa*(theta-X(i,j))/n+sigma*W(i,j)/sqrt(n);
    end
end
plot(t,X)
xlabel('$t$','interpreter','latex','FontSize',14)
ylabel('$r_t$','interpreter','latex','FontSize',14)
title('$Some~Vasicek~Paths~:~~dr_{t}=\kappa(\theta-r_{t})dt+\sigma dW_{t}$','interpreter','latex','FontSize',14)
axis([0 1 0 2])

% --- Executes on button press in pushbutton2.
function pushbutton2_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
kappa=2;
theta=1;
sigma=.4;
X0=1;
n=1000;
m=7;
W=randn(n,m);
X=zeros(n,m);
X(1,:)=X0;
t=[0:1/n:1];
for j=1:m
    for i=1:n
        X(i+1,j)=X(i,j)+kappa*(theta-X(i,j))/n+sigma*sqrt(X(i,j))*W(i,j)/sqrt(n);
    end
end
plot(t,X)
xlabel('$t$','interpreter','latex','FontSize',14)
ylabel('$r_t$','interpreter','latex','FontSize',14)
title('$Some~Cox.Ingersoll.Ross~Paths~:~~dr_t=\kappa(\theta-r_t)dt+\zeta \sqrt{r_t} dW_t$','interpreter','latex','FontSize',14)
axis([0 1 0 2])

% --- Executes on button press in pushbutton3.
function pushbutton3_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

n=1000;
m=50;
X0=ones(1,m);
r=0.35;
sigma=.25;
X=[X0;1+r/n+sigma*sqrt(1/n)*randn(n,m)];
t=[0:1/n:1];
Xt=cumprod(X);
plot(t,Xt)
xlabel('$t$','interpreter','latex','FontSize',13)
ylabel('$X_t$','interpreter','latex','FontSize',13)
title('$Geometric ~Brownian ~Motion~:~~dX_t=r X_t dt+ \sigma X_t dW_t$','interpreter','latex','FontSize',13)
axis([0 1 0 3])

% --- Executes on button press in pushbutton4.
function pushbutton4_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

n=1000;
m=50;
X0=zeros(1,m);
r=0.05;
sigma=.2;
X=[X0;r/n+sigma*sqrt(1/n)*randn(n,m)];
Xt=cumsum(X);
t=[0:1/n:1];
plot(t,Xt)
xlabel('$t$','interpreter','latex','FontSize',13)
ylabel('$X_t$','interpreter','latex','FontSize',13)
title('$Arithmetic ~Brownian ~Motion~:~~dX_t=r dt+ \sigma dW_t$','interpreter','latex','FontSize',13)
axis([0 1 -1 1])

% --- Executes on button press in pushbutton5.
function pushbutton5_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

n=1000;
m=20;
X0=zeros(1,m);
sigma=.35;
X=[X0;sigma*sqrt(1/n)*randn(n,m)];
Xt=cumsum(X);
Y=[X0;repmat(Xt(n+1,:),n,1)/n];
Yt=cumsum(Y);
Z=Xt-Yt;
t=[0:1/n:1];
plot(t,Z)
xlabel('$t$','interpreter','latex','FontSize',13)
ylabel('$B_t$','interpreter','latex','FontSize',13)
title('$~Brownian ~Bridge~:~~B(t)=W(t)- \frac{t}{T} W(T)$','interpreter','latex','FontSize',13)
axis([0 1 -1 1])

% --- Executes on button press in pushbutton6.
function pushbutton6_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

n=1000;
m=5;
X0=zeros(1,m);
sigma=.3;
X=[X0;sigma*sqrt(1/n)*randn(n,m)];
Y=[X0;sigma*sqrt(1/n)*randn(n,m)];
Xt=cumsum(X);
Yt=cumsum(Y);
t=[0:1/n:1];
plot(Xt,Yt)
xlabel('$W_x(t)$','interpreter','latex','FontSize',13)
ylabel('$W_y(t)$','interpreter','latex','FontSize',13)
title('$2~Dimension ~Brownian ~Motion~:~~\vec{W}(t)=(W_x(t)~~W_y(t))$','interpreter','latex','FontSize',13)
axis([-1 1 -1 1])

% --- Executes on button press in pushbutton7.
function pushbutton7_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton7 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

n=1000;
m=5;
X0=zeros(1,m);
sigma=.4;
X=[X0;sigma*sqrt(1/n)*randn(n,m)];
Y=[X0;sigma*sqrt(1/n)*randn(n,m)];
Z=[X0;sigma*sqrt(1/n)*randn(n,m)];
Xt=cumsum(X);
Yt=cumsum(Y);
Zt=cumsum(Z);
t=[0:1/n:1];
plot3(Xt,Yt,Zt)
grid on
xlabel('$W_x(t)$','interpreter','latex','FontSize',13)
ylabel('$W_y(t)$','interpreter','latex','FontSize',13)
zlabel('$W_z(t)$','interpreter','latex','FontSize',13)
title('$3~Dimension ~Brownian ~Motion~:~~\vec{W}(t)=(W_x(t)~~W_y(t)~~W_z(t))$','interpreter','latex','FontSize',13)
axis([-1 1 -1 1 -1 1])

% Rodolphe Sitter


function Y = AtA_Toeplitz(X,Lambda);
% AtA_Toeplitz: Gram Operator of SeparateAngle
%  Usage:
%     Y = AtA(X,Lambda)
%  Inputs:
%    X      n*n matrix (x,y)
%    Lambda Fourier multipliers
%  Outputs:
%    Y      n*n matrix (x,y)
%  Description:
%    Performs  A'A where A is the angular windowing using the
%    Toeplitz structure of A'A. AtA_Toeplitz uses FFTs.
%
% By Emmanuel Candes, 2003-2004

  n  = size(X,1); n2 = n/2;

  X1 = zeros(n); X2 = zeros(n);

  [ix,w] = DetailMeyerWindow([n2/4 n2/2],3);
  lx = reverse(-ix);

  F = ifft_mid0(X).*sqrt(n); % Take FT

  for r = 1:(n2/2),
    k = lx(r); t = n2 + k + 1;
    X1(:,t) = GUSFT_Toeplitz(F(:,t),Lambda(:,r,1));

    rsym = n2/2+1-r; t = n2 - k + 1;
    X1(:,t) = GUSFT_Toeplitz(F(:,t),Lambda(:,rsym,2));
  end

  X1 = fft_mid0(X1)/sqrt(n); % Invert FT

  F = ifft_mid0(X.').*sqrt(n); % Take FT

  for r = 1:(n2/2),
    k = lx(r); t = n2 + k + 1;
    X2(:,t) = GUSFT_Toeplitz(F(:,t),Lambda(:,r,1));

    rsym = n2/2+1-r;  t = n2 - k + 1;
    X2(:,t) = GUSFT_Toeplitz(F(:,t),Lambda(:,rsym,2));
  end

  X2 = fft_mid0(X2)/sqrt(n); % Invert FT

  Y = X1 + X2.';


function err = relaxT1FitFunc(x, m, alpha, tr, te)
%
% function err = relaxT1FitFunc(x, m, alpha, tr, te);
% inputs:
%       x = [r1, pd] where r1 is the inverse of the longitudinal relaxation
%       time (1/T1) and pd is a compsite term representing
%       M_0*G*exp(-TE/T_2star)
%       m: spgr data
%
%       alpha: flip angle (in degrees)
%       tr, te = tr and te
%
%   Output: the error for each measurement
%
%

r1 = x(1);
pd = x(2);

%pd = M_0*G*exp(-te/T_2star);
S_alpha = pd .* sin(alpha) .* (1-exp(-r1.*tr)) ./ (1-cos(alpha) .* exp(-r1.*tr));
err = S_alpha - m;

return;


function d = dot(g1,g2,varargin)
% inner product of quaternions g1 and g2
%
% Input
%  q1, q2 - @quaternion
%
% Output
%  d - double
%

d = g1.a .* g2.a + g1.b .* g2.b + g1.c .* g2.c + g1.d .* g2.d;


% =========================
%  Copula-Marginal Algorithm (CMA) to generate and manipulate flexible copulas, as described in
%  Meucci A., "New Breed of Copulas for Risk and Portfolio Management", Risk, September 2011
%
%  Most recent version of article and code available at http://www.symmys.com/node/335
%  =========================

clear; clc; close all;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generate panic distribution
N=20;
J=50000;

p=ones(J,1)/J;

r_c=.3;
c2_c=(1-r_c)*eye(N)+r_c*ones(N,N);

b=0.02;
r=.99;
c2_p=(1-r)*eye(N)+r*ones(N,N);

s2=blkdiag(c2_c,c2_p);
Z = MvnRnd(zeros(2*N,1),s2,J);

X_c = Z(:,1:N);
X_p = Z(:,N+1:end);
D = (normcdf(X_p)<b);

X=(1-D).*X_c+D.*X_p;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% perturb probabilities via Fully Flexible Views
Aeq = ones(1,J);  % constrain probabilities to sum to one...
beq=1;
Aeq = [Aeq   % ...constrain the first moments...
    X'];
beq=[beq
    zeros(N,1)];
p_ = EntropyProg(p,[],[],Aeq ,beq); % ...compute posterior probabilities

[xdd,udd,U]=CMAseparation(X,p_);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% merge panic copula with normal marginals
y=[];
u=[];
for n=1:N
    sig=.20;
    yn = linspace(-4*sig,4*sig,100)';
    un=normcdf(yn,0,sig);

    y=[y yn];
    u=[u un];
end
Y=CMAcombination(y,u,U);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute portfolio risk
w=ones(N,1)/N; % portfolio weights
R_w=Y*w;

figure
[n,D]=pHist(R_w,p_,round(10*log(J))  );
h=bar(D,n,1);
[x_lim]=get(gca,'xlim');
set(gca,'ytick',[])
set(h,'FaceColor',.5*[1 1 1],'EdgeColor',.5*[1 1 1]);
grid on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% rotate panic copula (only 2Dim)
if N==2
    th=pi/2;
    R=[cos(th) sin(th)
        -sin(th) cos(th)];
    X_=X*R';
    [xdd,udd,U_]=CMAseparation(X_,p_);
end


function [ x, seed ] = r8_normal_ab ( a, b, seed )

%*****************************************************************************80
%
%% R8_NORMAL_AB returns a scaled pseudonormal R8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, the mean of the normal PDF.
%
%    Input, real B, the standard deviation of the normal PDF.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real X, a sample of the standard normal PDF.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  [ x, seed ] = r8_normal_01 ( seed );
  x = a + b * x;

  return
end


% LTFAT - Frames
%
%  Peter L. Søndergaard, 2012 - 2018.
%
%  Creation of a frame object
%    FRAME             - Construct a new frame
%    FRAMEPAIR         - Construct a pair of frames
%    FRAMEDUAL         - The canonical dual frame
%    FRAMETIGHT        - The canonical tight frame
%    FRAMEACCEL        - Precompute arrays for faster application
%
%  Linear operators
%    FRANA             - Frame analysis
%    FRSYN             - Frame synthesis
%    FRSYNMATRIX       - Frame synthesis operator matrix
%    FRGRAMIAN	       - Frame Gramian operator
%    FRAMEOPERATOR     - Frame operator
%    FRAMEDIAG         - Diagonal of frame operator
%    FRANAITER         - Iterative perfect reconstruction analysis
%    FRSYNITER         - Iterative perfect reconstruction synthesis
%
%  Visualization
%    PLOTFRAME         - Plot frame coefficients
%    FRAMEGRAM         - Plot energy of signal in frame space
%
%  Information about a frame
%    FRAMEBOUNDS       - Frame bounds
%    FRAMERED          - Redundancy of frame
%    FRAMELENGTH       - Length of frame to expand signal
%    FRAMELENGTHCOEF   - Length of frame given a set of coefficients
%    FRAMECLENGTH      - Number of coefficients given input signal length
%    FRAMEVECTORNORMS  - Norms of the frame vectors
%
%  Coefficients conversions
%    FRAMECOEF2NATIVE  - Convert to native transform format
%    FRAMENATIVE2COEF  - Convert native to column format
%    FRAMECOEF2TF      - Convert to time-frequency plane layout
%    FRAMETF2COEF      - Convert TF-plane layout to native
%    FRAMECOEF2TFPLOT  - Convert to time-frequency plane layout for plotting
%
%  Non-linear analysis and synthesis
%    FRANABP           - Basis pursuit using the SALSA algorithm.
%    FRANAMP           - Orthogonal matching pursuit
%    FRANALASSO        - LASSO thresholding using Landweber iterations.
%    FRANAGROUPLASSO   - Group LASSO thresholding.
%    FRSYNABS          - Frame synthesis from magnitude of coefficients
%
%  For help, bug reports, suggestions etc. please visit
%  http://github.com/ltfat/ltfat/issues


function digit = ch_to_digit ( c )

%*****************************************************************************80
%
%% CH_TO_DIGIT returns the integer value of a base 10 digit.
%
%  Example:
%
%     C   DIGIT
%    ---  -----
%    '0'    0
%    '1'    1
%    ...  ...
%    '9'    9
%    ' '    0
%    'X'   -1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 November 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, character C, the decimal digit, '0' through '9' or blank
%    are legal.
%
%    Output, integer DIGIT, the corresponding integer value.  If C was
%    'illegal', then DIGIT is -1.
%
  if ( '0' <= c & c <= '9' )

    digit = c - '0';

  elseif ( c == ' ' )

    digit = 0;

  else

    digit = -1;

  end

  return
end


function [filt] = highpassfilter(dat,Fs,Fhp,N,type,dir)

% HIGHPASSFILTER removes low frequency components from EEG/MEG data
%
% Use as
%   [filt] = highpassfilter(dat, Fsample, Fhp, N, type, dir)
% where
%   dat        data matrix (Nchans X Ntime)
%   Fsample    sampling frequency in Hz
%   Fhp        filter frequency
%   N          optional filter order, default is 6 (but) or 25 (fir)
%   type       optional filter type, can be
%                'but' Butterworth IIR filter (default)
%                'fir' FIR filter using MATLAB fir1 function
%   dir        optional filter direction, can be
%                'onepass'         forward filter only
%                'onepass-reverse' reverse filter only, i.e. backward in time
%                'twopass'         zero-phase forward and reverse filter (default)
%
% Note that a one- or two-pass filter has consequences for the
% strength of the filter, i.e. a two-pass filter with the same filter
% order will attenuate the signal twice as strong.
%
% See also LOWPASSFILTER, BANDPASSFILTER

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

% set the default filter order later
if nargin<4
    N = [];
end

% set the default filter type
if nargin<5
  type = 'but';
end

% set the default filter direction
if nargin<6
  dir = 'twopass';
end

% Nyquist frequency
Fn = Fs/2;

% compute filter coefficients
switch type
  case 'but'
    if isempty(N)
      N = 6;
    end
    [B, A] = butter(N, max(Fhp)/Fn, 'high');
  case 'fir'
    if isempty(N)
      N = 25;
    end
    B = fir1(N, max(Fhp)/Fn, 'high');
    A = 1;
end

% apply filter to the data
switch dir
  case 'onepass'
    filt = filter(B, A, dat')';
  case 'onepass-reverse'
    dat  = fliplr(dat);
    filt = filter(B, A, dat')';
    filt = fliplr(filt);
  case 'twopass'
    filt = filtfilt(B, A, dat')';
end


function element_num = grid_ql_element_num ( nelemx, nelemy )

%*****************************************************************************80
%
%% GRID_QL_ELEMENT_NUM counts the elements in a grid of QL quadrilaterals.
%
%  Example:
%
%    Input:
%
%      NELEMX = 3, NELEMY = 2
%
%    Output:
%
%      ELEMENT_NUM = NELEMX * NELEMY = 6
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NELEMX, NELEMY, the number of elements along the
%    X and Y directions.  The number of elements generated will be
%    NELEMX * NELEMY.
%
%    Output, integer ELEMENT_NUM, the number of elements in the grid.
%
  element_num = nelemx * nelemy;

  return
end


function [fMinLimit, fMaxLimit, nTickStep] = ex_CalcFigureLimits(fMin, fMax)

fDiff = fMax - fMin;
nDigits = ceil(log10(fDiff));
nTickStep = 10^(nDigits-1);

bMinPositive = (fMin >= 0);
if ~bMinPositive
  fMin = (-1) * fMin;
end
bMaxPositive = (fMax >= 0);
if ~bMaxPositive
  fMax = (-1) * fMax;
end

if (fMin == 0)
  fMinLimit = 0;
else
  if bMinPositive
    fMinLimit = (fMin - 10^(nDigits-1));
    fMinLimit = ceil(fMinLimit * 10^(-nDigits+1))/10^(-nDigits+1);
  else
    fMinLimit = (fMin + 10^(nDigits-1));
    fMinLimit = floor(fMinLimit * 10^(-nDigits+1))/10^(-nDigits+1);
  end
  if ~bMinPositive
    fMinLimit = (-1) * fMinLimit;
  end
end
if (fMax == 0)
  fMaxLimit = 0;
else
  fMaxLimit = (fMax + 10^(nDigits-1));
  fMaxLimit = floor(fMaxLimit * 10^(-nDigits+1))/10^(-nDigits+1);
  if ~bMaxPositive
    fMaxLimit = (-1) * fMaxLimit;
  end
end


function obj = subobjective(weight, ind, idealpoint, method)
%SUBOBJECTIVE function evaluate a point's objective with a given method of
%decomposition.

%   Two method are implemented by far is Weighted-Sum and Tchebesheff.
%   weight: is the decomposition weight.(column wise vector).
%   ind: is the individual point(column wise vector).
%   idealpoint: the idealpoint for Tchebesheff decomposition.
%   method: is the decomposition method, the default is 'te' when is
%   omitted.
%
%   weight and ind can also be matrix. in which have two scenairos:
%   When weight is a matrix, then it's treated as a column wise set of
%   weights. in that case, if ind is a size 1 column vector, then the
%   subobjective is computed with every weight and the ind; if ind is also
%   a matrix of the same size as weight, then the subobjective is computed
%   in a column-to-column, with each column of weight computed against the
%   corresponding column of ind.
%   A row vector of subobjective is return in both case.

    if (nargin==2)
        obj = ws(weight, ind);
    elseif (nargin==3)
        obj = te(weight, ind, idealpoint);
    else
        if strcmp(method, 'ws')
            obj=ws(weight, ind);
        elseif strcmp(method, 'te')
            obj=te(weight, ind, idealpoint);
        else
            obj= te(weight, ind, idealpoint);
        end
    end
end

function obj = ws(weight, ind)
    if size(ind, 2) == 1
       obj = (weight'*ind)';
    else
       obj = sum(weight.*ind);
    end
end

function obj = te(weight, ind, idealpoint)
    s = size(weight, 2);
    indsize = size(ind,2);

    weight((weight == 0))=0.00001;

    if indsize==s
        part2 = abs(ind-idealpoint(:,ones(1, indsize)));
        obj = max(weight.*part2);
    elseif indsize ==1
        part2 = abs(ind-idealpoint);
        obj = max(weight.*part2(:,ones(1, s)));
    else
        error('individual size must be same as weight size, or equals 1');
    end
end


% A simple tutorial file to interface with RF
% Options copied from http://cran.r-project.org/web/packages/randomForest/randomForest.pdf

%run plethora of tests
clc
close all

%compile everything
if strcmpi(computer,'PCWIN') |strcmpi(computer,'PCWIN64')
   compile_windows
else
   compile_linux
end

total_train_time=0;
total_test_time=0;

%diabetes
load data/diabetes

%modify so that training data is NxD and labels are Nx1, where N=#of
%examples, D=# of features

X = diabetes.x;
Y = diabetes.y;

[N D] =size(X);
%randomly split into 400 examples for training and 42 for testing
randvector = randperm(N);

X_trn = X(randvector(1:400),:);
Y_trn = Y(randvector(1:400));
X_tst = X(randvector(401:end),:);
Y_tst = Y(randvector(401:end));

% example 1:  simply use with the defaults
    model = regRF_train(X_trn,Y_trn);
    Y_hat = regRF_predict(X_tst,model);
    fprintf('\nexample 1: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));

% % example 2:  set to 100 trees
%     model = regRF_train(X_trn,Y_trn, 100);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 2: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
% % example 3:  set to 100 trees, mtry = 2
%     model = regRF_train(X_trn,Y_trn, 100,2);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 3: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
% % example 4:  set to defaults trees and mtry by specifying values as 0
%     model = regRF_train(X_trn,Y_trn, 0, 0);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 4: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
% % % example 5: set sampling without replacement (default is with replacement)
%     extra_options.replace = 0 ;
%     model = regRF_train(X_trn,Y_trn, 100, 4, extra_options);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 5: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
% % example 6: sampsize example
%     %  extra_options.sampsize =  Size(s) of sample to draw. For classification,
%     %                   if sampsize is a vector of the length the number of strata, then sampling is stratified by strata,
%     %                   and the elements of sampsize indicate the numbers to be drawn from the strata.
%     clear extra_options
%     extra_options.sampsize = size(X_trn,1)*2/3;
%
%     model = regRF_train(X_trn,Y_trn, 100, 4, extra_options);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 6: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
% % example 7: nodesize
%     %  extra_options.nodesize = Minimum size of terminal nodes. Setting this number larger causes smaller trees
%     %                   to be grown (and thus take less time). Note that the default values are different
%     %                   for classification (1) and regression (5).
%     clear extra_options
%     extra_options.nodesize = 7;
%
%     model = regRF_train(X_trn,Y_trn, 100, 4, extra_options);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 7: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
%
% % example 8: calculating importance
%     clear extra_options
%     extra_options.importance = 1; %(0 = (Default) Don't, 1=calculate)
%
%     model = regRF_train(X_trn,Y_trn, 100, 4, extra_options);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 8: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
%     %model will have 3 variables for importance importanceSD and localImp
%     %importance = a matrix with nclass + 2 (for classification) or two (for regression) columns.
%     %           For classification, the first nclass columns are the class-specific measures
%     %           computed as mean decrease in accuracy. The nclass + 1st column is the
%     %           mean decrease in accuracy over all classes. The last column is the mean decrease
%     %           in Gini index. For Regression, the first column is the mean decrease in
%     %           accuracy and the second the mean decrease in MSE. If importance=FALSE,
%     %           the last measure is still returned as a vector.
%     figure('Name','Importance Plots')
%     subplot(3,1,1);
%     bar(model.importance(:,end-1));xlabel('feature');ylabel('magnitude');
%     title('Mean decrease in Accuracy');
%
%     subplot(3,1,2);
%     bar(model.importance(:,end));xlabel('feature');ylabel('magnitude');
%     title('Mean decrease in Gini index');
%
%
%     %importanceSD = The ?standard errors? of the permutation-based importance measure. For classification,
%     %           a D by nclass + 1 matrix corresponding to the first nclass + 1
%     %           columns of the importance matrix. For regression, a length p vector.
%     model.importanceSD
%     subplot(3,1,3);
%     bar(model.importanceSD);xlabel('feature');ylabel('magnitude');
%     title('Std. errors of importance measure');
%
% % example 9: calculating local importance
%     %  extra_options.localImp = Should casewise importance measure be computed? (Setting this to TRUE will
%     %                   override importance.)
%     %localImp  = a D by N matrix containing the casewise importance measures, the [i,j] element
%     %           of which is the importance of i-th variable on the j-th case. NULL if
%     %          localImp=FALSE.
%     clear extra_options
%     extra_options.localImp = 1; %(0 = (Default) Don't, 1=calculate)
%
%     model = regRF_train(X_trn,Y_trn, 100, 4, extra_options);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 9: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
%     model.localImp
%
% % example 10: calculating proximity
%     %  extra_options.proximity = Should proximity measure among the rows be calculated?
%     clear extra_options
%     extra_options.proximity = 1; %(0 = (Default) Don't, 1=calculate)
%
%     model = regRF_train(X_trn,Y_trn, 100, 4, extra_options);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 10: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
%     model.proximity
%
%
% % example 11: use only OOB for proximity
%     %  extra_options.oob_prox = Should proximity be calculated only on 'out-of-bag' data?
%     clear extra_options
%     extra_options.proximity = 1; %(0 = (Default) Don't, 1=calculate)
%     extra_options.oob_prox = 0; %(Default = 1 if proximity is enabled,  Don't 0)
%
%     model = regRF_train(X_trn,Y_trn, 100, 4, extra_options);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 11: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
%
% % example 12: to see what is going on behind the scenes
% %  extra_options.do_trace = If set to TRUE, give a more verbose output as randomForest is run. If set to
% %                   some integer, then running output is printed for every
% %                   do_trace trees.
%     clear extra_options
%     extra_options.do_trace = 1; %(Default = 0)
%
%     model = regRF_train(X_trn,Y_trn, 100, 4, extra_options);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 12: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
% % example 13: to see what is going on behind the scenes
% %  extra_options.keep_inbag Should an n by ntree matrix be returned that keeps track of which samples are
% %                   'in-bag' in which trees (but not how many times, if sampling with replacement)
%
%     clear extra_options
%     extra_options.keep_inbag = 1; %(Default = 0)
%
%     model = regRF_train(X_trn,Y_trn, 100, 4, extra_options);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 13: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
%     model.inbag
%
%
% % example 14: getting the OOB MSE rate. model will have mse field
%     model = regRF_train(X_trn,Y_trn);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 14: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
%     figure('Name','OOB error rate');
%     plot(model.mse); title('OOB MSE error rate');  xlabel('iteration (# trees)'); ylabel('OOB error rate');
%
% %
% % example 15: nPerm
% %               Number of times the OOB data are permuted per tree for assessing variable
% %               importance. Number larger than 1 gives slightly more stable estimate, but not
% %               very effective. Currently only implemented for regression.
%     clear extra_options
%     extra_options.importance=1;
%     extra_options.nPerm = 1; %(Default = 0)
%     model = regRF_train(X_trn,Y_trn,100,2,extra_options);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 15: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
%     figure('Name','Importance Plots nPerm=1')
%     subplot(2,1,1);
%     bar(model.importance(:,end-1));xlabel('feature');ylabel('magnitude');
%     title('Mean decrease in Accuracy');
%
%     subplot(2,1,2);
%     bar(model.importance(:,end));xlabel('feature');ylabel('magnitude');
%     title('Mean decrease in Gini index');
%
%     %let's now run with nPerm=3
%     clear extra_options
%     extra_options.importance=1;
%     extra_options.nPerm = 3; %(Default = 0)
%     model = regRF_train(X_trn,Y_trn,100,2,extra_options);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 15: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));
%
%     figure('Name','Importance Plots nPerm=3')
%     subplot(2,1,1);
%     bar(model.importance(:,end-1));xlabel('feature');ylabel('magnitude');
%     title('Mean decrease in Accuracy');
%
%     subplot(2,1,2);
%     bar(model.importance(:,end));xlabel('feature');ylabel('magnitude');
%     title('Mean decrease in Gini index');
%
% % example 16: corr_bias (not recommended to use)
%     clear extra_options
%     extra_options.corr_bias=1;
%     model = regRF_train(X_trn,Y_trn,100,2,extra_options);
%     Y_hat = regRF_predict(X_tst,model);
%     fprintf('\nexample 16: MSE rate %f\n',   sum((Y_hat-Y_tst).^2));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inverse dynamics for the 2dof planar robot
%
%   tau = inversedynamics_2dofplanar(robot, q, qd, qdd, gc, fext)
%
%   Where robot stores the kinematic and dynamic parameters for this robot.
%   q: joint positions.
%   qd: joint velocities.
%   qdd: joint accelerations.
%   fext: vector of external forces. Defined in the BASE reference system.
%   let gc=9.81 m/s^2 if the force is acting on the -Y0 direction, or gc =
%   -9.81 m/s^2 if acting on the opposite direction.
%
%   This function just executes the inverse dynamic model for this robot.
%   The equations to compute this dynamic model can be found in:
%   "ROBOT ANALYSIS. The mechanics of Serial and Parallel
%        manipulators". Lung Weng Tsai. John Wiley and Sons, inc. ISBN:
%        0-471-32593-7. page 405.
%
%
%   Author: Arturo Gil Aparicio arturo.gil@umh.es
%   Date: 08/03/2014
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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 qdd = forwarddynamics_2dofplanar(robot, q, qd, tau, gc, fext)
a = eval(robot.DH.a);
a1=a(1);
a2=a(2);

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

%Express it as a general dynamic function:
%M*qdd + V + G = Q, where Q is a vector of generalized forces or moments
%M is a 2x2 manipulator inertia matrix
M = [(1/3)*m1*a1^2 + m2*(a1^2 + a1*a2*cos(q(2))+(1/3)*a2^2)  m2*((1/2)*a1*a2*cos(q(2))+(1/3)*a2^2);
     m2*((1/2)*a1*a2*cos(q(2))+(1/3)*a2^2)                      (1/3)*m2*a2^2];

V = [-m2*a1*a2*sin(q(2))*(qd(1)*qd(2)+(1/2)*qd(2)^2);
            (1/2)*m2*a1*a2*sin(q(2))*qd(1)^2        ];

G = [(1/2)*m1*gc*a1*cos(q(1))+ m2*gc*a1*cos(q(1)) + (1/2)*m2*gc*a2*cos(q(1)+q(2));
         (1/2)*m2*gc*a2*cos(q(1)+q(2))              ];

%assure fext is a column vector
fext=fext(:);

%External forces are propagated to every joint by using the manipulators
%Jacobian
J = manipulator_jacobian(robot, q);

% %Account for friction by summing in tau.
if robot.dynamics.friction
    for j=1:robot.DOF,
        tau(j) = tau(j) - friction(robot, qd, j);
    end
end

inv(M)
tau
V
G
J'*fext
%Finally use the general equation to compute the torques, considering the
%external forces.
qdd =  inv(M)*(tau - V - G - J'*fext);


function [logp, yhat, res] = tapas_softmax_2beta(r, infStates, ptrans)
% Calculates the log-probability of responses under the softmax model with different betas for
% rewards and punishments
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013-2016 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Predictions or posteriors?
predorpost = r.c_obs.predorpost;

% Transform betas to their native space
% be(1): rewards, be(2): punishments
be = exp(ptrans(1:2));

% Initialize returned log-probabilities, predictions,
% and residuals as NaNs so that NaN is returned for all
% irregualar trials
n = size(infStates,1);
logp = NaN(n,1);
yhat = NaN(n,1);
res  = NaN(n,1);

% Weed irregular trials out from inferred states, inputs, and responses
states = squeeze(infStates(:,1,:,1,predorpost));
states(r.irr,:) = [];
u = r.u(:,1);
u(r.irr) = [];
y = r.y(:,1);
y(r.irr) = [];

% Number of choices
nc = size(infStates,3);

% Partition functions
Z1 = sum(exp(be(1)*states),2);
Z1 = repmat(Z1,1,nc);

Z2 = sum(exp(be(2)*states),2);
Z2 = repmat(Z2,1,nc);

% Softmax probabilities
prob1 = exp(be(1)*states)./Z1;
prob2 = exp(be(2)*states)./Z2;

% Extract probabilities of chosen options
probc1 = prob1(sub2ind(size(prob1), 1:length(y), y'));
probc2 = prob2(sub2ind(size(prob2), 1:length(y), y'));

% Choose the correct column
probc = probc1'.*(u==1) +probc2'.*(u==0);

% Calculate log-probabilities for non-irregular trials
reg = ~ismember(1:n,r.irr);
logp(reg) = log(probc);
yhat(reg) = probc;
res(reg) = -log(probc);

return;


% MAIN  --  minimum acceleration trajectory
%
% For a simple pendulum:
%
% x = position
% v = velocity
% u = torque
%
% ddx = f(x,dx,u);     <-- dynamics
%
% cost = integral(  ddx^2  );     <-- cost function
%
% subject to:
%   x(0) = 0;
%   x(1) = pi;
%   dx(0) = 0;
%   dx(1) = pi;
%
% How to pose as a standard trajectory optimization problem?
%
% dx = v1;
% dv1 = f(x,v1,u1)
%
% v2 == v1;   % <-- Key line.
% dv2 = u2;
% cost = integral(  u2^2  );
%
%
% NOTES:
%
%   z = [x;v1;v2];
%   u = [u1;u2];
%
clc; clear;
addpath ../../..

%%%% Specify boundary conditions
t0 = 0;
tF = 5;

maxTorque = 1.0;
z0 = [0;0;0];
zF = [pi;0;0];

param.k = 1.0;  % gravity torque constant for pendulum model
param.b = 0.1;  % viscous damping constant

%%%% Pack up boundary conditions
problem.bounds.initialTime.low = t0;
problem.bounds.initialTime.upp = t0;

problem.bounds.finalTime.low = tF;
problem.bounds.finalTime.upp = tF;

problem.bounds.initialState.low = z0;
problem.bounds.initialState.upp = z0;

problem.bounds.finalState.low = zF;
problem.bounds.finalState.upp = zF;

problem.bounds.control.low = [-maxTorque; -inf];
problem.bounds.control.upp = [maxTorque; inf];

%%%% Initialize trajectory with a straight line
problem.guess.time = [t0,tF];
problem.guess.state = [z0, zF];
problem.guess.control = [zeros(2,1), zeros(2,1)];

%%%% Pack up function handles
problem.func.dynamics = @(t,z,u)(  dynamics(z,u,param)  );
problem.func.pathObj = @(t,z,u)(  pathObjective(u)  );
problem.func.pathCst = @(t,z,u)(  pathConstraint(z)  );

%%%% Choice of solver:
method = 'trapezoid';

switch method
    case 'chebyshev'
        problem.options.method = method;
        problem.options.chebyshev.nColPts = 25;
    case 'trapezoid'
        problem.options.method = method;
    case 'hermiteSimpson'
        problem.options.method = method;
        problem.options.hermiteSimpson.nSegment = 15;
        problem.options.nlpOpt.MaxFunEvals = 5e4;
    case 'gpops'
        problem.options.method = 'gpops';
    otherwise
        error('invalid method')
end

%%%% Solve
soln = optimTraj(problem);

%%%% Unpack the solution

tGrid = soln.grid.time;
xGrid = soln.grid.state(1, :);
v1Grid = soln.grid.state(2, :);
v2Grid = soln.grid.state(3, :);
u1Grid = soln.grid.control(1, :);
dv2Grid = soln.grid.control(2, :);

t = linspace(tGrid(1), tGrid(end), 100);
z = soln.interp.state(t);
u = soln.interp.control(t);
x = z(1,:);
v1 = z(2,:);
v2 = z(3,:);
u1 = u(1,:);
dv2 = u(2,:);

%%%% Plot the trajectory against time
figure(1); clf;

subplot(2,2,1); hold on;
plot(t,x)
plot(tGrid,xGrid,'ko','MarkerSize',8,'LineWidth',2);
title('angle')

subplot(2,2,2); hold on;
plot(t,v1)
plot(t,v2)
plot(tGrid,v1Grid,'ko','MarkerSize',8,'LineWidth',2);
plot(tGrid,v2Grid,'ko','MarkerSize',8,'LineWidth',2);
title('angular rate')
legend('v1','v2')

subplot(2,2,3); hold on;
plot(t([1,end]),[1,1]*maxTorque,'k--','LineWidth',1);
plot(t([1,end]),-[1,1]*maxTorque,'k--','LineWidth',1);
plot(t,u1)
plot(tGrid,u1Grid,'ko','MarkerSize',8,'LineWidth',2);
title('torque')

subplot(2,2,4); hold on;
plot(t,dv2)
plot(tGrid,dv2Grid,'ko','MarkerSize',8,'LineWidth',2);
title('angular acceleration')


%[2018]-"Butterfly optimization algorithm: a novel approach for global
%optimization"

% (9/12/2020)

function BOA = jButterflyOptimizationAlgorithm(feat,label,opts)
% Parameters
lb    = 0;
ub    = 1;
thres = 0.5;
c     = 0.01;   % modular modality
p     = 0.8;    % switch probability

if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'c'), c = opts.c; end
if isfield(opts,'p'), p = opts.p; end
if isfield(opts,'thres'), thres = opts.thres; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = zeros(N,dim);
for i = 1:N
	for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
	end
end
% Pre
Xnew = zeros(N,dim);
fitG = inf;
fit  = zeros(1,N);

curve = inf;
t = 1;
% Iterations
while t <= max_Iter
  % Fitness
  for i = 1:N
    fit(i) = fun(feat,label,(X(i,:) > thres),opts);
    % Global update
    if fit(i) < fitG
      fitG = fit(i);
      Xgb  = X(i,:);
    end
  end
  % Power component, increase from 0.1 to 0.3
  a = 0.1 + 0.2 * (t / max_Iter);
  for i = 1:N
    % Compute fragrance (1)
    f = c * (fit(i) ^ a);
    % Random number in [0,1]
    r = rand();
    if r < p
      r1 = rand();
      for d = 1:dim
        % Move toward best butterfly (2)
        Xnew(i,d) = X(i,d) + ((r1 ^ 2) * Xgb(d) - X(i,d)) * f;
      end
    else
      % Random select two butterfly
      R  = randperm(N);
      J  = R(1);
      K  = R(2);
      r2 = rand();
      for d = 1:dim
        % Move randomly (3)
        Xnew(i,d) = X(i,d) + ((r2 ^ 2) * X(J,d) - X(K,d)) * f;
      end
    end
    % Boundary
    XB = Xnew(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    Xnew(i,:) = XB;
  end
  % Replace
  X = Xnew;
  % Save
  curve(t) = fitG;
  fprintf('\nIteration %d Best (BOA)= %f',t,curve(t))
  t = t + 1;
end
% Select features
Pos   = 1:dim;
Sf    = Pos((Xgb > thres) == 1);
sFeat = feat(:,Sf);
% Store results
BOA.sf = Sf;
BOA.ff = sFeat;
BOA.nf = length(Sf);
BOA.c  = curve;
BOA.f  = feat;
BOA.l  = label;
end


%%*********************************************************************
%% This is an example of using the interface
%% to solve the following theta problem of a graph
%% with adjacency matrix G:
%% max <E,X>
%% s.t. trace(X)==1 ; X positive semidefinite
%%      X(i,j) == 0 for (i,j) in Edge_set
%% SDPNAL+:
%% Copyright (c) 2017 by
%% Yancheng Yuan, Kim-Chuan Toh, Defeng Sun and Xinyuan Zhao
%%*********************************************************************

 load theta6.mat
 [IE,JE] = find(triu(G,1));
 n = length(G);
 %%
 model = ccp_model('Example_theta');
     X = var_sdp(n, n);
     model.add_variable(X);
     model.maximize(sum(X));
     model.add_affine_constraint(trace(X) == 1);
     options=2;
     if (options==1)
        model.add_affine_constraint(X(IE,JE) == 0);
        model.add_affine_constraint(X >= 0);
     else %% another way to specify the constraints
        U = 1e20*(1-spones(G+G'));
        model.add_affine_constraint(0 <= X <= U);
     end
 model.solve;
%%*********************************************************************


%[2015]-"A human learning optimization algorithm and its application
%to multi-dimensional knapsack problems"

% (9/12/2020)

function HLO = jHumanLearningOptimization(feat,label,opts)
% Parameters
pi = 0.85;   % probability of individual learning
pr = 0.1;    % probability of exploration learning

if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'pi'), pi = opts.pi; end
if isfield(opts,'pr'), pr = opts.pr; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial
X   = jInitialPopulation(N,dim);
% Fitness
fit    = zeros(1,N);
fitSKD = inf;
for i = 1:N
  fit(i) = fun(feat,label,X(i,:),opts);
  % Update SKD/gbest
  if fit(i) < fitSKD
    fitSKD = fit(i);
    SKD    = X(i,:);
  end
end
% Get IKD/pbest
fitIKD = fit;
IKD    = X;
% Pre
curve = zeros(1,max_Iter);
curve(1) = fitSKD;
t = 2;
% Generations
while t <= max_Iter
  for i = 1:N
    % Update solution (8)
    for d = 1:dim
      % Radom probability in [0,1]
      r = rand();
      if r >= 0 && r < pr
        % Random exploration learning operator (7)
        if rand() < 0.5
          X(i,d) = 0;
        else
          X(i,d) = 1;
        end
      elseif r >= pr && r < pi
        X(i,d) = IKD(i,d);
      else
        X(i,d) = SKD(d);
      end
    end
  end
  % Fitness
  for i = 1:N
    % Fitness
    fit(i) = fun(feat,label,X(i,:),opts);
    % Update IKD/pbest
    if fit(i) < fitIKD(i)
      fitIKD(i) = fit(i);
      IKD(i,:)  = X(i,:);
    end
    % Update SKD/gbest
    if fitIKD(i) < fitSKD
      fitSKD = fitIKD(i);
      SKD    = IKD(i,:);
    end
  end
  curve(t) = fitSKD;
  fprintf('\nGeneration %d Best (HLO)= %f',t,curve(t))
  t = t + 1;
end
% Select features based on selected index
Pos   = 1:dim;
Sf    = Pos(SKD == 1);
sFeat = feat(:,Sf);
% Store results
HLO.sf = Sf;
HLO.ff = sFeat;
HLO.nf = length(Sf);
HLO.c  = curve;
HLO.f  = feat;
HLO.l  = label;
end

% Binary initialization strategy
function X = jInitialPopulation(N,dim)
X = zeros(N,dim);
for i = 1:N
  for d = 1:dim
    if rand() > 0.5
      X(i,d) = 1;
    end
  end
end
end


%tstoolbox/mex/baker
%   Generate time-series from the iterated Baker map .
%
%   Syntax:
%
%     * x = baker(length, [eta l1 l2 x0 y0])
%
%   Input arguments:
%
%     * length - number of samples to generate
%     * [eta l1 l2 x0 y0] - vector of parameters and initial conditions
%
%   Output arguments:
%
%     * x - time series
%
%   Example:
%
%x = baker(2000, [0.6 0.25 0.4 rand(1,1) rand(1,1)]);
%plot(x(1:end-1,2), x(2:end,2), '.')
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt


function [node,elem,T] = squarequadmesh(square,h)
%% SQUAREQUADMESH uniform rectangular mesh of a square
%
% [node,elem] = squarequadmesh([x0,x1,y0,y1],h) generates a uniform mesh of the
% rectangle [x0,x1]*[y0,y1] with mesh size h.
%
% [node,elem,T] = squarequadmesh([x0,x1,y0,y1],h) with additional output T
% contains auxiliary structure: edge, elem2edge, edge2elem, bdEdge
%
% Example
%
%   [node,elem] = squarequadmesh([0,1,0,2],0.5);
%   showmesh(node,elem);
%   findnode(node);
%   findquadelem(node,elem);
%
% See also: squaremesh, squaregradmeshquad, cubehexmesh
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Generate nodes
x0 = square(1); x1 = square(2);
y0 = square(3); y1 = square(4);
% [x,y] = meshgrid(x0:h:x1,y1:-h:y0);
[x,y] = ndgrid(x0:h:x1,y0:h:y1);
node = [x(:),y(:)];

%% Generate elements
ni = size(x,1); % number of rows
nj = size(x,2);
N = size(node,1);
nodeidx = reshape(1:N,ni,nj);
t2nidxMap = nodeidx(1:ni-1,1:nj-1);
k = t2nidxMap(:);
elem = [k k+ni k+ni+1 k+1];
NT = size(elem,1);
% 4 k+1 --- k+ni+1 3
%    |        |
% 1  k  ---  k+ni  2

%% Check if further structure is needed
if nargout<=2
    return;
end

%% Generate edges
Ne1 = (ni-1)*nj;
Ne2 = ni*(nj-1);
edge = zeros(Ne1+Ne2,2,'int32');
% vertical edges
e2nidxMap = nodeidx(1:ni-1,1:nj);
k = e2nidxMap(:);
edge(1:Ne1,:) = [k k+1];
% horizontal edges
e2nidxMap = nodeidx(1:ni,1:nj-1);
k = e2nidxMap(:);
edge(Ne1+(1:Ne2),:) = [k k+ni];

%% Generate index map between element and edges
elem2edge = zeros(NT,4,'int32');
edge2elem = zeros(Ne1+Ne2,2,'int32');
k = 1:NT;
veidx = k; % veidx = sub2ind([ni-1,nj],i,j);
heidx = Ne1 + (1:Ne2); % linear indexing
heidx = reshape(heidx,ni,nj-1); % matrix indexing
heidx = heidx(1:ni-1,:); % without top edges
elem2edge(k,1) = heidx(:);
elem2edge(k,3) = heidx(:)+1;
elem2edge(k,2) = veidx + (ni-1);
elem2edge(k,4) = veidx;
% vertical edges
edge2elem(veidx,2) = k;   % right element
edge2elem(veidx+ni-1,1) = k; % left element
% horizontal edges
edge2elem(heidx,1) = k;
edge2elem(heidx+1,2) = k;

%% Generate boundary edges
% for boundary edges, set the another element idx to zero
idx1 = edge2elem(:,1) == 0; % left and top
idx2 = edge2elem(:,2) == 0;
bdEdge = [edge(idx1,[2 1]); edge(idx2,:)];
% switch edge index on the left and top to have a consistent orientation

%% Auxstructure
T = struct('edge',edge,'elem2edge',elem2edge,'edge2elem',edge2elem,'bdEdge',bdEdge);
% neighbor and bdElem can be easily get from 2-D index of all elements


function [ a, irow, icol ] = i4mat_red ( m, n, a )

%*****************************************************************************80
%
%% I4MAT_RED divides out common factors in a row or column of an I4MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in the matrix.
%
%    Input, integer N, the number of columns in the matrix.
%
%    Input, integer A(M,N), the matrix to be reduced.
%
%    Output, integer A(M,N), the reduced matrix.  The greatest common factor
%    in any row or column is 1.
%
%    Output, integer IROW(M), the row factors that were divided out.
%
%    Output, integer ICOL(N), the column factors that were divided out.
%
  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_RED - Warning!\n' );
    fprintf ( 1, '  M must be greater than 0.\n' );
    fprintf ( 1, '  Input M = %d\n', m );
    error ( 'I4MAT_RED - Fatal error!' );
  end

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_RED - Warning!\n' );
    fprintf ( 1, '  N must be greater than 0.\n' );
    fprintf ( 1, '  Input N = %d\n', n );
    error ( 'I4MAT_RED - Fatal error!' );
  end
%
%  Remove factors common to a column.
%
  incx = 1;
  for j = 1 : n
    [ a(1:m,j), icol(j) ] = i4vec_red ( m, a(1:m,j), incx );
  end
%
%  Remove factors common to a row.
%
  incx = 1;
  for i = 1 : m
    [ a(i,1:n), irow(i) ] = i4vec_red ( n, a(i,1:n), incx );
  end

  return
end


function hermite_cubic_test02 ( )

%*****************************************************************************80
%
%% HERMITE_CUBIC_TEST02 tests HERMITE_CUBIC_VALUE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HERMITE_CUBIC_TEST02:\n' );
  fprintf ( 1, '  HERMITE_CUBIC_VALUE evaluates a Hermite cubic polynomial.\n' );
  fprintf ( 1, '  Try out data from a cubic function:\n' );
  fprintf ( 1, '  on [0,10] and [-1.0,1.0] and [0.5,0.75]\n' );

  for x_interval = 1 : 3

    if ( x_interval == 1 )
      x1 = 0.0;
      x2 = 10.0;
    elseif ( x_interval == 2 )
      x1 = -1.0;
      x2 = +1.0;
    elseif ( x_interval == 3 )
      x1 = 0.5;
      x2 = 0.75;
    end

    [ f1, d1, s1, t1 ] = cubic_value ( x1 );
    [ f2, d2, s2, t2 ] = cubic_value ( x2 );

    fprintf ( 1, '\n' );
    fprintf ( 1, '    J      X           F           D           S           T\n' );

    for j = - 3 : 12
      x = ( ( 10 - j ) * x1   ...
          +        j   * x2 ) ...
            / 10.0;

      [ f, d, s, t ] = hermite_cubic_value ( x1, f1, d1, x2, f2, d2, 1, x );
      [ fc, dc, sc, tc ] = cubic_value ( x );

      fprintf ( 1, '\n' );
      if ( j == 0 )
        fprintf ( 1, '*Data  %10f  %10f  %10f\n', x1, f1, d1 );
      end
      fprintf ( 1, 'Exact  %10f  %10f  %10f  %10f  %10f\n',    x,  fc, dc, sc, tc );
      fprintf ( 1, '  %3d  %10f  %10f  %10f  %10f  %10f\n', j, x,  f,  d,  s,  t );
      if ( j == 10 )
        fprintf ( 1, '*Data  %10f  %10f  %10f\n', x2, f2, d2 );
      end

    end

  end

  return
end


function transforms2d(varargin)
%TRANSFORMS2D Description of functions operating on transforms.
%
%   By 'transform' we mean an affine transform. A planar affine transform
%   can be represented by a 3x3 matrix.
%
%   Example
%     % create a translation by the vector [10 20]:
%     T = createTranslation([10 20])
%     T =
%          1     0    10
%          0     1    20
%          0     0     1
%
%     % apply a rotation on a polygon
%     poly = [0 0; 30 0;30 10;10 10;10 20;0 20];
%     trans = createRotation([10 20], pi/6);
%     polyT = transformPoint(poly, trans);
%     % display the original and the rotated polygons
%     figure; hold on; axis equal; axis([-10 40 -10 40]);
%     drawPolygon(poly, 'k');
%     drawPolygon(polyT, 'b');
%
%
%   See also
%   createTranslation, createRotation, createRotation90, createScaling
%   createHomothecy, createLineReflection, createBasisTransform
%   transformPoint, transformVector, transformLine, transformEdge
%   rotateVector, principalAxesTransform, fitAffineTransform2d
%   polynomialTransform2d, fitPolynomialTransform2d

% ------
% 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('transforms2d');


function [results] = hyperGlrt(M, t)
% HYPERGLRT Performs the generalized liklihood test ratio algorithm
%   Performs the generalized liklihood test ratio algorithm for target
% detection.
%
% Usage
%   [results] = hyperGlrt(M, U, target)
% Inputs
%   M - 2d matrix of HSI data (p x N)
%   t - target of interest (p x 1)
% Outputs
%   results - vector of detector output (N x 1)
%
% References
%   T F AyouB, "Modified GLRT Signal Detection Algorithm," IEEE
% Transactions on Aerospace and Electronic Systems, Vol 36, No 3, July
% 2000.

[p, N] = size(M);

% Remove mean from data
u = mean(M.').';
M = M - repmat(u, 1, N);
t = t - u;

R = inv(hyperCov(M));

results = zeros(1, N);
for k=1:N
    x = M(:,k);
    results(k) = ((t'*R*x)^2) / ((t'*R*t)*(1 + x'*R*x));
end


% Transforms a covariance matrix into a correlation matrix

function C = cov2corr(C)

S = diag(diag(C).^(-0.5));
C = S*C*S;


%INVSIGM Inverse sigmoid map
%
% 	W = W*INVSIGM
% 	B = INVSIGM(ARG)
%
% INPUT
%	ARG   Mapping/Dataset
%
% OUTPUT
%	W     Mapping transforming posterior probabilities into distances.
%
% DESCRIPTION
% The inverse sigmoidal transformation to transform a classifier to a
% mapping, transforming posterior probabilities into distances.
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, CLASSC, SIGM

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Sciences, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

function w = invsigm(arg)
		if nargin == 0

	% Create an empty mapping:
	w = prmapping('invsigm','combiner');
	w = setname(w,'Inverse Sigmoidal Mapping');

elseif isa(arg,'prmapping')

	% If the mapping requested a SIGM transformation (out_conv=1), it
	% is now removed (out_conv=0):
	if arg.out_conv == 1
		w = set(arg,'out_conv',0);
		if arg.size_out == 2
			w.size_out = 1;
			w.labels = w.labels(1,:);
			data.rot = w.data.rot(:,1);
			data.offset = w.data.offset(1);
			data.lablist_in = w.data.lablist_in;
			w.data = data;
		end
	else
		w_s = setname(prmapping('invsigm','fixed'),'Inverse Sigmoidal Mapping');
		w = arg*w_s;
	end

else
	% The data is really transformed:
	if isdatafile(arg)
		w = addpostproc(arg,invsigm);
	else  % datasets and doubles
		w = log(arg+realmin) - log(1-arg+realmin);
	end
end

return


function [b,t,p,hh] = ancova(groups,x,y,varargin)
% :Usage:
% ::
%
%     b,t,p,pthandles] = ancova(groups,x,y,[plot],[covs of no interest])
%
% :Outputs:
%
%   **Elements of b, t, p:**
%        1st = intercept, 2nd = group effect, 3rd = slope, 4th = grp x slope
%        interaction
%
% recursive -- call ancova repeately if y is a matrix
% get pairwise standardized slopes (corrs)
% and group diffs and slope interaction for all
% pairs of y vectors
%

hh = [];
doplot = 0; if length(varargin) > 0, doplot = varargin{1};,end

% covariates of no interest
if length(varargin) > 1,
    covs = varargin{2};,

    [x,y,r,p,rrob,prob] = partialcor([x covs],y,1);
    % doesn't adjust groups now...

end

if size(y,2) > 1 & isempty(x)

    for i = 1:size(y,2)-1
        for j = i+1:size(y,2)

            [b,t,p] = ancova(groups,y(:,i),y(:,j));
            bb{1}(i,j) = b(2); tt{1}(i,j) = t(2); pp{1}(i,j) = p(2);  % group effect = cell 1
            bb{2}(i,j) = b(3); tt{2}(i,j) = t(3); pp{2}(i,j) = p(3);  % slope = cell 2
            bb{3}(i,j) = b(4); tt{3}(i,j) = t(4); pp{3}(i,j) = p(4);  % grp x slope = cell 3

        end
    end

    % clean up last row
    for i = 1:length(bb)
        bb{i}(end+1,:) = 0;  bb{i} = bb{i} + bb{i}';
        tt{i}(end+1,:) = 0;  tt{i} = tt{i} + tt{i}';
        pp{i}(end+1,:) = 0;  pp{i} = pp{i} + pp{i}';
    end

    b = bb;
    t = tt;
    p = pp;
    return

end

% -----------------------------------------
% basic ancova program
% -----------------------------------------

% center and format
x = [scale(groups,1) scale(x,1)];
%y = scale(y);

% add interaction in slopes (diff. slopes model)

x(:,3) = scale(x(:,1).*x(:,2));

[b,dev,stats]=glmfit(x,y);
bi = b(end); ti = stats.t(end); ppi = stats.p(end);

if ppi > .05

    % no sig. interaction, drop interaction and use parallel slopes

    [b,dev,stats]=glmfit(x(:,1:2),y);
    b(end+1) = bi; stats.t(end+1) = ti; stats.p(end+1) = ppi;

end

t = stats.t;
p = stats.p;

% -----------------------------------------
%  ancova plot
% -----------------------------------------

if doplot
    hh = [];
    colors = {'yo' 'm^'};
    [uni,b,grps] = unique(groups);
    legstr = {['Group 1: ' num2str(uni(1))] ['Group 2: ' num2str(uni(2))]};
        uni = 1:max(grps);
    % median split, if continuous, high then low
    if length(uni) > 2,
        uni = [1 2]; grps = grps.*0;
        grps(groups>median(groups)) = 1;
        grps(groups<median(groups)) = 2;
        legstr = {'High' 'Low'};
    end
    %tor_fig;
    for i = 1:length(uni)   % for each group, make a plot

        [tmp,tmp,tmp,hh(i)] = plot_correlation_samefig(x(find(grps==uni(i)),2),y(find(grps==uni(i))),[],colors{i},0,1);
    end
    xlabel('x'); ylabel('y');
    legend(hh,legstr)
end

return


function [params,X,means,wX,wM,dwM] = pre_sphere_symm(params, X, dim)
% Spheres the data with covarianve matrix eigenvalue decomposition.
%   [params,X,means,wX,wM,dwM] = pre_sphere_symm(params, X, dim)
%     X    Data to be sphered
%     dim  Requested result data dimension
%     wX   Sphered data
%     wM   Sphering matrix
%     dwM  De-sphering matrix

% Copyright (C) 2004, 2005 DSS MATLAB package team (dss@cis.hut.fi).
% Distributed by Laboratory of Computer and Information Science,
% Helsinki University of Technology. http://www.cis.hut.fi/projects/dss/.
% $Id$

if nargin<2
    params.name = 'Symmetric sphering';
    params.description = 'Spheres the data with covarianve matrix eigenvalue decomposition.';
    return;
end

if nargin<3; dim=0; end

[xdim,tdim] = size(X);
% removing the mean
means = mean(X,2);
X = X - repmat(means,1,tdim);

[wM, dwM] = dss_sphere(X, dim, 1);

wX = wM * X;


function [energy, jumpPenalty, dataError] = energyL2Potts( u, f, gamma, A, isotropic )
%energyL2Potts Computes the energy of the L2 Potts functional

if exist('A', 'var') && not(isempty(A))
    Au = A * u;
    dataError = sum((Au(:) - f(:)).^2);
else
    dataError = sum((u(:) - f(:)).^2);
end

if isvector(u)
    % vectors
    jumpPenalty = sum(diff(u(:)) ~= 0);
else
    % matrices
    nJumpComp = 0; % jumps in compass directions
    nJumpDiag = 0; % jumps in diagonal directions

    % count jumps
    for i = 1:size(u, 1)
        for j = 1:size(u, 2)-1
            if ~all((u(i,j,:) == u(i,j+1,:)))
                nJumpComp = nJumpComp + 1;
            end
        end
    end
    for i = 1:size(u, 1)-1
        for j = 1:size(u, 2)
            if ~all((u(i,j,:) == u(i+1,j,:)))
                nJumpComp = nJumpComp + 1;
            end
        end
    end
    for i = 1:size(u, 1)-1
        for j = 1:size(u, 2)-1
            if ~all((u(i,j,:) == u(i+1,j+1,:)))
                nJumpDiag = nJumpDiag + 1;
            end
        end
    end
    for i = 1:size(u, 1)-1
        for j = 2:size(u, 2)
            if ~all((u(i,j,:) == u(i+1,j-1,:)))
                nJumpDiag = nJumpDiag + 1;
            end
        end
    end

    % set weights (isotropic by default)
    if ~exist('isotropic', 'var') || isotropic
        omega1 = sqrt(2) - 1;
        omega2 = 1 - sqrt(2)/2;
    else
        omega1 = 1;
        omega2 = 0;
    end

    % compute energy
    jumpPenalty = (omega1 * nJumpComp + omega2* nJumpDiag);

end

energy = gamma * jumpPenalty + dataError;

end


function [theta] = trainLinearReg(X, y, lambda)
%TRAINLINEARREG Trains linear regression given a dataset (X, y) and a
%regularization parameter lambda
%   [theta] = TRAINLINEARREG (X, y, lambda) trains linear regression using
%   the dataset (X, y) and regularization parameter lambda. Returns the
%   trained parameters theta.
%

% Initialize Theta
initial_theta = zeros(size(X, 2), 1);
% initial_theta = ones(size(X, 2), 1);

% Create "short hand" for the cost function to be minimized
costFunction = @(t) linearRegCostFunction(X, y, t, lambda);

% Now, costFunction is a function that takes in only one argument
options = optimset('MaxIter', 200, 'GradObj', 'on');

% Minimize using fmincg
theta = fmincg(costFunction, initial_theta, options);

end


function M = euclideansubspacefactory(E, proj, dim)
% Returns a manifold struct to optimize over a subspace of a linear manifold
%
% The factory produces a manifold description M for a linear subspace S of
% a linear space E.
%
% Points and tangent vectors on M are represented in memory in exactly the
% same way as they are for E. Furthermore, E is considered the embedding
% space of S. In particular, the Euclidean gradient of a function is
% understood as the gradient of that function in E, whereas the gradient of
% that function in S is here called the Riemannian gradient. Similar
% considerations hold for the Hessian.
%
% The linear space E is described by a structure obtained from a factory.
% For example, E can be the space of vectors, matrices or nD-arrays of real
% or complex numbers, or a subspace of those. E is equipped with a (real)
% inner product, making it a (real) Euclidean space.
%
% The subspace S is described by an orthogonal projector proj from E to E,
% whose image (i.e., span, range, ...) is S. Orthogonality is judged with
% respect to the inner product on E, and S inherits that inner product,
% making it a Euclidean space itself, and a Riemannian submanifold of E.
%
% The dimension of the subspace S must ideally also be specified.
%
% Inputs:
%   E is the factory for a linear space; for example:
%       E = euclideanfactory(n)
%       E = euclideanfactory(n, m)
%       E = euclideanfactory([n1, n2, n3])
%       E = euclideancomplexfactory(n) % and other dimensions too
%       ...
%
%   proj is a function handle that takes as input a point u of E and
%       returns another point of E: the orthogonal projection of u to S.
%       Orthogonality is understood with respect to the inner product on E,
%       that is, E.inner(x, u, v) = <u, v>. Like any orthogonal projector,
%       it must be linear, and it must satisfy:
%           <u, Pv> = <Pu, v>    and    PPw = Pw
%       for all points u, v, w in E.
%       To check these properties on random vectors, call M.checkproj();
%
%   dim is the dimension of the subspace S (an integer). As always in
%       Manopt, we consider linear spaces over the real numbers, hence,
%       this is the dimension of S as a real linear space. For example, the
%       dimension of R^n is n, and the dimension of C^n is 2n.
%
% Output:
%   M is a factory for optimization over the subspace S.
%
%
% Example:
%
% Among complex vector of length n, consider the subspace S of such vectors
% that can be the discrete Fourier transform of a real vector of length n
% (that is, the subspace generated by feeding all real vectors of length n
% to fft). The factory M below describes that subspace.
%
%   n = 17;
%   E = euclideancomplexfactory(n);
%   proj = @(u) (u + conj(u([1 ; (n:-1:2)'])))/2;
%   M = euclideansubspacefactory(E, proj, n);
%   M.checkproj(); % for debugging
%
%
% Note 1: this factory is designed to work with linear subspaces only: it
% does not work for affine subspaces. Explicitly: S must contain the origin
% of E. If you need support for affine subspaces, let us know on the Manopt
% forum and we can help (or share your improved code :)).
%
% Note 2: the linear space E can itself be a linear subspace of another.
% For example, we can have:
%       E = symmetricfactory(n, k)
%       E = skewfactory(n, k)
%       E = euclideansubspacefactory(...) % recursive nesting
% For these use-cases, bear in mind that the embedding space of M is E,
% hence, the Euclidean gradient and Hessian are expected to be given in E,
% not in the 'bigger' linear space E possibly lives in.
%
% See also: euclideanfactory euclideancomplexfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, April 25, 2019.
% Contributors:
% Change log:

    M = E;

    M.name = @() ['Subspace of ', E.name()];

    M.proj = @(x, u) proj(u);

    M.egrad2rgrad = @(x, eg) proj(eg);

    M.ehess2rhess = @(x, eg, eh, d) proj(eh);

    M.tangent = @(x, u) proj(u);

    M.rand = @() proj(E.rand());

    M.randvec = @randvec;
    function v = randvec(x)
        v = proj(E.randvec(x));
        v = v / E.norm(x, v);
    end

    if exist('dim', 'var')
        M.dim = @() dim;
        M.typicaldist = @() sqrt(dim);
    else
        M = rmfield(M, 'dim');
        warning('manopt:subspacedim', ...
               ['Since the dimension of the subspace was not specified' ...
                ', M.dim() is not available in the returned factory.']);
    end

    M.checkproj = @() checkproj(E, proj);

end

% Tool to check that proj is indeed an orthogonal projector to a subspace
% of E, with respect to the inner product on the linear space E.
function checkproj(E, proj)
    x = E.rand();
    u = E.randvec(x);
    v = E.randvec(x);
    a = randn();
    b = randn();
    % Check that P is linear.
    Paubv = proj(E.lincomb(x, a, u, b, v));
    aPvbPv = E.lincomb(x, a, proj(u), b, proj(v));
    fprintf(['Is proj linear? This number should be zero up to ' ...
             'machine precision:\n\t%.16e\n'], ...
                E.norm(x, E.lincomb(x, 1, Paubv, -1, aPvbPv)));
    % Check that PPw = Pw.
    fprintf(['Is it a projector? This number should be zero up to ' ...
             'machine precision:\n\t%.16e\n'], ...
                E.norm(x, E.lincomb(x, 1, proj(u), -1, proj(proj(u)))));
    % Check that <u, Pv> = <Pu, v>.
    fprintf(['Is it self-adjoint? These two numbers should be equal ' ...
             'up to machine precision:\n\t%.16e\n\t%.16e\n'], ...
                E.inner(x, u, proj(v)), ...
                E.inner(x, proj(u), v));
end