function anal = er_wtaClassifier(amps1,amps2,subtractDC,plotFlag,names);
%
% anal = er_wtaClassifier(amps1,amps2,[plotFlag],[names]);
%
% Apply a winner-take-all classifier to event-related
% data.
%
% [more description forthcoming]
%
% Input args: amps1 and amps2 should be of shape voxels x conditions:
% each column represents the pattern of response across voxels to a
% given condition. (See mv_reliability for an example of how
% to compute these.)
%
% plotFlag: 1 x 3 vector to flag each of the following
% possible output plots:
%   1) image the amplitudes for each subset
%   2) summarize the reliability analysis
%   3) 'omnibus' contrast: regress mean response, across conditions,
%   from each subset
%
% subtractDC: option to subtract the baseline response
% across conditions for
%
% ras 05/05. Based on exp5_haxby, by ras 03/05.
if ieNotDefined('subtractDc')
    subtractDC = 0;
else
    subtractDC
end

if ieNotDefined('plotFlag')
    plotFlag = [0 0 0];
else
    plotFlag
end

if ieNotDefined('names')
    for i = 1:size(amps1,2)
        names{i} = num2str(i);
    end
end

if size(amps1) ~= size(amps2)
    error('amps1 and amps2 must be same size.')
end

nVoxels = size(amps1,1);
nConds = size(amps1,2);
font = 'Arial';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% omnibus test                    %
% (always do this before zeroing, %
% or you remove the effect)       %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mu1 = mean(amps1,2);
mu2 = mean(amps2,2);
% ignore zeroed voxels
notzeroed = find(mu1~=0 & mu2~=0);
mu1 = mu1(notzeroed);
mu2 = mu2(notzeroed);
[R P] = corrcoef(mu1(:),mu2(:));
anal.omnibusR = R(1,2);
anal.omnibusP = P(1,2);
anal.mu1 = mu1;
anal.mu2 = mu2;

if subtractDC==1
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	% Normalize responses, in a manner similar   %
	% to Haxby et al 2001 -- subtract mean       %
	% response across conditions from each voxel %
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	bsl1 = repmat(mean(amps1,2),1,size(amps1,2));
	usedAmps1 = amps1 - bsl1;
	bsl2 = repmat(mean(amps2,2),1,size(amps2,2));
	usedAmps2 = amps2 - bsl2;
else
    % use the non-corrected amps
    usedAmps1 = amps1;
    usedAmps2 = amps2;
end

% assign amplitude fields to anal struct
anal.amps1 = amps1;
anal.amps2 = amps2;
anal.usedAmps1 = usedAmps1;
anal.usedAmps2 = usedAmps2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% perform pairwise correlations between maps %
% in the first and second data sets          %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = 1:nConds
    for j = 1:nConds
        training = usedAmps1(:,i);
        test = usedAmps2(:,j);

        % remove NaNs
        ok = ~isnan(training) & ~isnan(test);
        if sum(ok)==0
            error('Too many NaNs in amplitudes.');
        end
        training = training(ok); test = test(ok);

        [R p] = corrcoef(training,test);
        anal.corrRvals(i,j) = R(1,2);
        anal.corrPvals(i,j) = p(1,2);
    end
end

% % 02/22/05: kalanit suggested thresholding
% % the R vals, based on the p -- let's try it:
% anal.corrRvals(anal.corrPvals>0.05) = 0;

% as an ideal observer of activity patterns,
% 'guess' the image shown in the 2nd data set
% by looking at what provoked the most similar
% (by correlation) response from the 1st data set
for i = 1:nConds
    anal.guess(i,:) = zeros(1,nConds);
    guess = find(anal.corrRvals(i,:)==max(anal.corrRvals(i,:)));
    anal.guess(i,guess) = 1;

    % guesses along the diagonal are correct -- the
    % highest-correlated pattern comes from the same image.
    % other guesses are mistakes.
    if isempty(guess), guess = 0; end
    anal.correct(i) = (guess(1)==i);
end
anal.pctCorrect = 100 * sum(anal.correct)/length(anal.correct);

% let's also compute the 'voxel reliability',the
% correlation value for each voxel of the set of nConds
% response amplitudes between the two subsets:
for v = 1:nVoxels
    [R p] = corrcoef(anal.amps1(v,:),anal.amps2(v,:));
    voxR(v) = R(1,2);
end
anal.voxR = voxR;

% calculate mean correlations for same image,
% diff image but same cat, diff cat:
nImgs = nConds; %length(1:nConds);
group = 3*ones(nImgs,nImgs); % diff cat
for i = 1:nImgs
    for j = 1:nImgs
        if ceil(j/4)==ceil(i/4)
            group(i,j) = 2; % same cat
        end
    end
    group(i,i) = 1;  % same img
end

anal.group = group;
anal.meanR = [];
anal.semR = [];
for j = 1:3
    anal.meanR(j) = mean(anal.corrRvals(group==j));
    nObs = sum(group(:)==j);
    anal.semR(j) = std(anal.corrRvals(group==j))/sqrt(nObs-1);
    try
        [H p] = ttest2(anal.corrRvals(group==j),anal.corrRvals(group==3));
    catch
        H = 0; p = 1;
    end
    anal.sigDiffR(j) = H;
    anal.pvalR(j) = p;
end

%%%%%%%%%%%%%%%%%%%%%
% plot the results  %
%%%%%%%%%%%%%%%%%%%%%
% Amplitude plots
if plotFlag(1)==1
	anal.h1 = figure('Name','Amplitudes From Each Half of Data',...
                    'Units','Normalized','Position',[.7 .8 .3 .18],...
                    'MenuBar','none',...
                    'Color','w'); % [0 .4 .7 .4]
	colormap jet

    minVal = min([min(anal.amps1(:)) min(anal.amps2(:))]);
	maxVal = max([max(anal.amps1(:)) max(anal.amps2(:))]);

    subplot('Position',[.1 .15 .35 .75])
	imagesc(anal.amps1,[minVal maxVal]);
	set(gca,'XTick',1:nConds,'XTickLabel',names);
	ylabel('Voxels');
    title('Subset 1 (Training)')
    set(gca,'FontName',font,'FontSize',10);

    subplot('Position',[.5 .15 .35 .75])
	imagesc(anal.amps2,[minVal maxVal]);
	set(gca,'XTick',1:nConds,'XTickLabel',names,'YTick',[]);
	ylabel('Voxels');
    title('Subset 2 (Test)');
    set(gca,'FontName',font,'FontSize',10);

    hcb = subplot('Position',[.9 .2 .03 .6]);
    colorbar(hcb);
    ylabel('Response Amplitude, % Signal');
    set(gca,'FontName',font,'FontSize',10);
end

% reliability summary
if plotFlag(2)==1
	anal.h2 = figure('Name','Reliability Amps Results',...
                'Color','w','MenuBar','none','Units','Normalized',...
                'Position',[.7 .6 .3 .18]); % [.1 .2 .7 .7]
	subplot(2,2,1);
	imagesc(anal.corrRvals); % colorbar;
	set(gca,'XTick',1:nConds,'XTickLabel',names,...
            'YTick',1:nConds,'YTickLabel',names);
	set(gca,'FontName',font,'FontSize',10);
	xlabel('First Half');
	ylabel('Second Half');
	title('Correlation Coefficient R');

    subplot(2,2,2);
	imagesc(anal.guess); % colorbar;
	set(gca,'XTick',1:nConds,'XTickLabel',names,...
            'YTick',1:nConds,'YTickLabel',names);
	set(gca,'FontName',font,'FontSize',10);
	xlabel('First Half');
	ylabel('Second Half');
	title('Ideal Observer Best Guess');

    subplot(2,2,3);
	starbar(anal.meanR,anal.semR,anal.sigDiffR);
	grps = {'Same Image' 'Same Category' 'Different Category'};
	set(gca,'XTickLabel',grps);
	set(gca,'FontName',font,'FontSize',10);
	ylabel('Mean R Value');

    subplot(2,2,4);
	table = {'Percent Correct:'; num2str(anal.pctCorrect)};
	plotTable(table);
end

% 'omnibus' test
if plotFlag(3)==1
	anal.h3 = figure('Color','w','MenuBar','none','Units','Normalized',...
                     'Position',[.7 .4 .2 .18]);
	regressPlot(mu1,mu2);
	set(gca,'FontName',font,'FontSize',10);
	axis equal
	axis square
	xlabel('Amplitude, Odd Runs','FontName','Helvetica','FontSize',nConds);
	ylabel('Amplitude, Even Runs','FontName','Helvetica','FontSize',nConds);
	ttltxt = sprintf('Amplitudes Across All Images');
	title(ttltxt,'FontName','Helvetica','FontSize',14);
end

return


function x = rsc_encode(G,m,termination)
% Copyright 1998, Yufei Wu, MPRG lab, Virginia Tech. for academic use
% encodes a binary data block m (0/1) with a RSC (recursive systematic
% convolutional) code defined by generator matrix G, returns the output
% in x (0/1), terminates the trellis with all-0 state if termination>0
if nargin<3, termination = 0; end
[N,L] = size(G); % Number of output bits, Constraint length
M = L-1; % Dimension of the state
lu = length(m)+(termination>0)*M; % Length of the input
lm = lu-M; % Length of the message
state = zeros(1,M); % initialize the state vector
% To generate the codeword
x = [];
for i = 1:lu
   if termination<=0 | (termination>0 & i<=length(m))
     d_k = m(i);
    elseif termination>0 & i>lm
     d_k = rem(G(1,2:L)*state.',2);
   end
   a_k = rem(G(1,:)*[d_k state].',2);
   xp = rem(G(2,:)*[a_k state].',2); % 2nd output (parity) bits
   state = [a_k state(1:M-1)]; % Next sttate
   x = [x [d_k; xp]]; % since systematic, first output is input bit
end


% test_mecv2_reconstruction.m -
% image reconstruction using mirror-extended curvelets

if(1)
  N = 512;
  H = N/2;
  [a,b] = ndgrid(0:N-1);
  x = a+b-N;
  x = (x-min(min(x)))/max(max(x-min(min(x))));
  ns = ceil(log2(N) - 3);
  nag = 16;
  ci = 128;

  c = mefcv2(x,N,N,ns,nag);

  cfs = [];
  for s=1:length(c);    for w=1:length(c{s});      cfs = [cfs; abs(c{s}{w}(:))];    end;  end
  cfs = sort(abs(cfs)); cfs = cfs(end:-1:1);
  val = cfs(ci);
  d = c;
  cnt = 0;
  for s=1:length(c)
    for w=1:length(c{s})
      cnt = cnt + sum(sum(abs(c{s}{w})>=val+eps));
      d{s}{w} = c{s}{w}.*(abs(c{s}{w})>=val+eps);
    end
  end
  fprintf(1, 'number of coefficients used = %d\n', cnt);
  y = meicv2(d,N,N,ns,nag);
  figure;  imagesc(real(y)); axis equal; axis tight; colormap gray; colorbar;
  title('Partial reconstruction with mirror-extended curvelets');
end


function err = sb_test_ori(pnt,elem)
err = 1;
if(size(elem,2) == 4)
    det = sum(cross(pnt(elem(:,2),:)-pnt(elem(:,1),:),pnt(elem(:,4),:)-pnt(elem(:,1),:),2).*(pnt(elem(:,3),:)-pnt(elem(:,1),:)),2);
    if sum(det <= 0) > 0
        err = 0;
    end
elseif(size(elem,2) == 8)
    det1 = sum(cross(pnt(elem(:,6),:)-pnt(elem(:,1),:),pnt(elem(:,8),:)-pnt(elem(:,1),:),2).*(pnt(elem(:,5),:)-pnt(elem(:,1),:)),2);
    det2 = sum(cross(pnt(elem(:,3),:)-pnt(elem(:,1),:),pnt(elem(:,6),:)-pnt(elem(:,1),:),2).*(pnt(elem(:,2),:)-pnt(elem(:,1),:)),2);
    det3 = sum(cross(pnt(elem(:,8),:)-pnt(elem(:,1),:),pnt(elem(:,3),:)-pnt(elem(:,1),:),2).*(pnt(elem(:,4),:)-pnt(elem(:,1),:)),2);
    det4 = sum(cross(pnt(elem(:,8),:)-pnt(elem(:,3),:),pnt(elem(:,6),:)-pnt(elem(:,3),:),2).*(pnt(elem(:,7),:)-pnt(elem(:,3),:)),2);
    if sum(det1 <= 0) || sum(det2 <= 0) || sum(det3 <= 0) || sum(det4 <= 0)
        err = 0;
    end
else
    error('Invalid number of nodes per element!');
end
end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtStkRadiation.m                             |
%|    #    |   VERSION    : 0.50                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 31.10.2018                                    |
%|  / 0 \  |   LAST MODIF : 25.11.2018                                    |
%| ( === ) |   SYNOPSIS   : Stoke radiation of a unit sphere using BEM    |
%|  `---'  |                with stokeslet G and stresslet T              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
N  = 300;
n  = 100;
x0 = [0.1 0.2 0.3];

% Mesh unit sphere
mesh = mshSphere(N,1);

% Quadrature
gamma = dom(mesh,3);

% Finite element and unknowns
phi = fem(mesh,'P1');
unk = phi.unk;

% Radiation particles
mesh2 = mshSphere(n,2);
X     = mesh2.vtx;

% Graphical rep
figure
plot(mesh)
hold on
plotNrm(mesh)
plot(gamma)
plot(phi,'*r')
plot(msh(X))
alpha(0.5)
axis equal

% Initalization of block matrix for each coordinate (x,y,z)
G      = cell(3,3);
T      = cell(3,3);
mu     = cell(3,1);
lambda = cell(3,1);

% Loop for each coordinate
for i = 1:3
    for j = 1:3
        % Using r = x - y
        % Single layer : G = \int_gamma 1/(8pi) (\delta_ij/r + r_i*r_j/|r|^3)
        name   = ['[ij/r+rirj/r^3]',num2str(i),num2str(j)];
        green  = @(X,Y) 1/(8*pi) .* femGreenKernel(X,Y,name,[]);
        G{i,j} = integral(X,gamma,green,phi);

        % Double layer : T = \int_gamma -6/(8pi) (r_i*r_j*(r.n)/|r|^5)
        T{i,j} = 0;
        for k = 1:3
            name   = ['[rirjrk/r^5]',num2str(i),num2str(j),num2str(k)];
            green  = @(X,Y) -6/(8*pi) .* femGreenKernel(X,Y,name,[]);
            T{i,j} = T{i,j} + integral(X,gamma,green,ntimes(phi,k));
        end
    end

    % mu = [u] = u_int - u_ext = - Gi1 = - Gi1(x0,y)
    name  = ['[ij/r+rirj/r^3]',num2str(i),num2str(1)];
    mu{i} = -1/(8*pi) * femGreenKernel(x0,phi.unk,name,[]);

    % lambda = [sigma] = - Ti1, with nk = xk (because boundary is unit sphere)
    lambda{i} = 0;
    for k = 1:3
        name      = ['[rirjrk/r^5]',num2str(i),num2str(1),num2str(k)];
        lambda{i} = lambda{i} - (-6/(8*pi)) * femGreenKernel(x0,phi.unk,name,[]) .* unk(:,k);
    end
end

% Convert cells to full matrix
G      = cell2mat(G);
T      = cell2mat(T);
mu     = cell2mat(mu);
lambda = cell2mat(lambda);

% Stokes radiation in domain : ui(x) = - sum_j \int_gamma Gij(x,y) lambda_j dy ...
%              + sum_j \int_gamma Tij(x,y).n(y) mu_j dy
sol = -G*lambda + T*mu ;

% Analytic solution : Gi1
ref = cell(3,1);
for i = 1:3
    name   = ['[ij/r+rirj/r^3]',num2str(i),num2str(1)];
    ref{i} = 1/(8*pi) * femGreenKernel(X,x0,name,[]);
end
ref = cell2mat(ref);

% Relative error L2 and inf
norm(ref-sol)/norm(ref)
norm(ref-sol,'inf')/norm(ref,'inf')

% Graphical representation of solution
figure(2)
for i = 1:3
    subplot(1,3,i)
    ind = (i-1)*n + (1:n);
    plot(mesh2,sol(ind))
    axis equal
    colorbar
    title(['Component ',num2str(i)])
end

disp('~~> Michto gypsilab !')


clear all;
randn('seed',0);
% Data are generated
X=randn(20000,100);
X=X./repmat(sqrt(sum(X.^2)),[size(X,1) 1]);

% parameter of the optimization procedure are chosen
param.numThreads=-1; % number of processors/cores to use; the default choice is -1
                    % and uses all the cores of the machine

param.pos=0;
param.mode=1;       % projection on the l1 ball
param.thrs=2;
tic
X1=mexSparseProject(X,param);
t=toc;
toc
fprintf('%f signals of size %d projected per second\n',size(X,2)/t,size(X,1));
fprintf('Checking constraint: %f, %f\n',min(sum(abs(X1))),max(sum(abs(X1))));

param.mode=2;       % projection on the Elastic-Net
param.lambda1=0.15;

tic
X1=mexSparseProject(X,param);
t=toc;
toc
fprintf('%f signals of size %d projected per second\n',size(X,2)/t,size(X,1));
constraints=sum((X1.^2))+param.lambda1*sum(abs(X1));
fprintf('Checking constraint: %f, %f\n',min(constraints),max(constraints));

param.mode=6;       % projection on the FLSA
param.lambda1=0.7;
param.lambda2=0.7;
param.lambda3=1.0;

X=rand(2000,100);
X=X./repmat(sqrt(sum(X.^2)),[size(X,1) 1]);

tic
X1=mexSparseProject(X,param);
t=toc;
toc
fprintf('%f signals of size %d projected per second\n',size(X,2)/t,size(X,1));
constraints=0.5*param.lambda3*sum(X1.^2)+param.lambda1*sum(abs(X1))+param.lambda2*sum(abs(X1(2:end,:)-X1(1:end-1,:)));
fprintf('Checking constraint: %f, %f\n',mean(constraints),max(constraints));
fprintf('Projection is approximate (stops at a kink)\n',mean(constraints),max(constraints));

param.mode=6;       % projection on the FLSA
param.lambda1=0.7;
param.lambda2=0.7;
param.lambda3=1.0;

X=rand(2000,100);
X=X./repmat(sqrt(sum(X.^2)),[size(X,1) 1]);

tic
X1=mexSparseProject(X,param);
t=toc;
toc
fprintf('%f signals of size %d projected per second\n',size(X,2)/t,size(X,1));
constraints=0.5*param.lambda3*sum(X1.^2)+param.lambda1*sum(abs(X1))+param.lambda2*sum(abs(X1(2:end,:)-X1(1:end-1,:)));
fprintf('Checking constraint: %f, %f\n',mean(constraints),max(constraints));
fprintf('Projection is approximate (stops at a kink)\n',mean(constraints),max(constraints));


function Derive_Equations()
%%%% Derive Equations - Five Link Biped Model %%%%
%
% This function derives the equations of motion, as well as some other useful
% equations (kinematics, contact forces, ...) for the five-link biped
% model.
%
% This version of the code includes a few more complicated features for
% dealing with difficult cost functions. In particular, it adds 10 slack
% variables to compute the abs(power) term in the cost function, and the
% primary control is the derivative of torque, rather than torque itself.
% This allows for regularization by the derivative of the input.
%
%
% Nomenclature:
%
% - There are five links, which will be numbered starting with "1" for the
% stance leg tibia, increasing as the links are father from the base joint,
% and ending with "5" for the swing leg tibia.
%   1 - stance leg tibia (lower leg)
%   2 - stance leg femur  (upper leg)
%   3 - torso
%   4 - swing leg femur
%   5 - swing leg tibia
%
% - This script uses absolute angles, which are represented with "q". All
% angles use positive convention, with the zero angle corresponding to a
% vertically aligned link configuration. [q] = [0] has the torso balanced
% upright, with both legs fully extended straight below it.
%
% - Derivatives with respect to time are notated by prepending a "d". For
% example the rate of change in an absolute angle is "dq" and angular
% acceleration would be "ddq"
%
% - Joint positions are given with "P", center of mass positions are "G"
%

clc; clear;
disp('Creating variables and derivatives...')

%%%% Absolute orientation (angle) of each link
q1 = sym('q1', 'real');
q2 = sym('q2','real');
q3 = sym('q3','real');
q4 = sym('q4','real');
q5 = sym('q5','real');

%%%% Absolute angular rate of each link
dq1 = sym('dq1','real');
dq2 = sym('dq2','real');
dq3 = sym('dq3','real');
dq4 = sym('dq4','real');
dq5 = sym('dq5','real');

%%%% Absolute angular acceleration of each linke
ddq1 = sym('ddq1','real');
ddq2 = sym('ddq2','real');
ddq3 = sym('ddq3','real');
ddq4 = sym('ddq4','real');
ddq5 = sym('ddq5','real');

%%%% Torques at each joint
u1 = sym('u1','real');  %Stance foot
u2 = sym('u2','real');   %Stance knee
u3 = sym('u3','real');   %Stance hip
u4 = sym('u4','real');   %Swing hip
u5 = sym('u5','real');   %Swing knee

%%%% Torques rate at each joint
du1 = sym('du1','real');  %Stance foot
du2 = sym('du2','real');   %Stance knee
du3 = sym('du3','real');   %Stance hip
du4 = sym('du4','real');   %Swing hip
du5 = sym('du5','real');   %Swing knee

%%%% Slack variables -- negative component of power
sn1 = sym('sn1','real');  %Stance foot
sn2 = sym('sn2','real');   %Stance knee
sn3 = sym('sn3','real');   %Stance hip
sn4 = sym('sn4','real');   %Swing hip
sn5 = sym('sn5','real');   %Swing knee

%%%% Slack variables -- positive component of power
sp1 = sym('sp1','real');  %Stance foot
sp2 = sym('sp2','real');   %Stance knee
sp3 = sym('sp3','real');   %Stance hip
sp4 = sym('sp4','real');   %Swing hip
sp5 = sym('sp5','real');   %Swing knee

%%%% Mass of each link
m1 = sym('m1','real');
m2 = sym('m2','real');
m3 = sym('m3','real');
m4 = sym('m4','real');
m5 = sym('m5','real');

%%%% Distance between parent joint and link center of mass
c1 = sym('c1','real');
c2 = sym('c2','real');
c3 = sym('c3','real');
c4 = sym('c4','real');
c5 = sym('c5','real');

%%%% Length of each link
l1 = sym('l1','real');
l2 = sym('l2','real');
l3 = sym('l3','real');
l4 = sym('l4','real');
l5 = sym('l5','real');

%%%% Moment of inertia of each link about its own center of mass
I1 = sym('I1','real');
I2 = sym('I2','real');
I3 = sym('I3','real');
I4 = sym('I4','real');
I5 = sym('I5','real');

g = sym('g','real'); % Gravity
Fx = sym('Fx','real');   %Horizontal contact force at stance foot
Fy = sym('Fy','real');   %Vertical contact force at stance foot
empty = sym('empty','real');   %Used for vectorization, user should pass a vector of zeros
t = sym('t','real');  %dummy continuous time

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                Set up coordinate system and unit vectors                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

i = sym([1;0]);   %Horizontal axis
j = sym([0;1]);   %Vertical axis

e1 = cos(q1)*(j) + sin(q1)*(-i);  %unit vector from P0 -> P1, (contact point to stance knee)
e2 = cos(q2)*(j) + sin(q2)*(-i);  %unit vector from P1 -> P2, (stance knee to hip)
e3 = cos(q3)*(j) + sin(q3)*(-i);  %unit vector from P2 -> P3, (hip to shoulders);
e4 = -cos(q4)*(j) - sin(q4)*(-i);  %unit vector from P2 -> P4, (hip to swing knee);
e5 = -cos(q5)*(j) - sin(q5)*(-i);  %unit vector from P4 -> P5, (swing knee to swing foot);

P0 = 0*i + 0*j;   %stance foot = Contact point = origin
P1 = P0 + l1*e1;  %stance knee
P2 = P1 + l2*e2;  %hip
P3 = P2 + l3*e3;  %shoulders
P4 = P2 + l4*e4;  %swing knee
P5 = P4 + l5*e5;  %swing foot

G1 = P1 - c1*e1;  % CoM stance leg tibia
G2 = P2 - c2*e2;  % CoM stance leg febur
G3 = P3 - c3*e3;  % CoM torso
G4 = P2 + c4*e4;  % CoM swing leg femur
G5 = P4 + c5*e5;  % CoM swing leg tibia
G = (m1*G1 + m2*G2 + m3*G3 + m4*G4 + m5*G5)/(m1+m2+m3+m4+m5);  %Center of mass for entire robot

%%%% Define a function for doing '2d' cross product: dot(a x b, k)
cross2d = @(a,b)(a(1)*b(2) - a(2)*b(1));

%%%% Weight of each link:
w1 = -m1*g*j;
w2 = -m2*g*j;
w3 = -m3*g*j;
w4 = -m4*g*j;
w5 = -m5*g*j;

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                             Derivatives                                 %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

q = [q1;q2;q3;q4;q5];
dq = [dq1;dq2;dq3;dq4;dq5];
ddq = [ddq1;ddq2;ddq3;ddq4;ddq5];
u = [u1;u2;u3;u4;u5];
du = [du1;du2;du3;du4;du5];
sn = [sn1;sn2;sn3;sn4;sn5];
sp = [sp1;sp2;sp3;sp4;sp5];
z = [t;q;dq;u;du;sn;sp];   % time-varying vector of inputs

% Neat trick to compute derivatives using the chain rule
derivative = @(in)( jacobian(in,[q;dq;u])*[dq;ddq;du] );

% Velocity of the swing foot (used for step constraints)
dP5 = derivative(P5);

% Compute derivatives for the CoM of each link:
dG1 = derivative(G1);  ddG1 = derivative(dG1);
dG2 = derivative(G2);  ddG2 = derivative(dG2);
dG3 = derivative(G3);  ddG3 = derivative(dG3);
dG4 = derivative(G4);  ddG4 = derivative(dG4);
dG5 = derivative(G5);  ddG5 = derivative(dG5);
dG = derivative(G);  ddG = derivative(dG);

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                             Calculations:                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

singleStanceDynamics();
objectiveFunctions();
heelStrikeDynamics();

mechanicalEnergy();
contactForces();
kinematics();

disp('Done!');

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

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                      Single-Stance Dynamics                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

% I solve the dynamics here by carefully selecting angular momentum balance
% equations about each joint, working my way out the kinematic tree from
% the root.

    function singleStanceDynamics()
        disp('Deriving single stance dynamics...')

        %%%% AMB - entire system @ P0
        eqnTorque0 = ...
            cross2d(G1-P0,w1) + ...
            cross2d(G2-P0,w2) + ...
            cross2d(G3-P0,w3) + ...
            cross2d(G4-P0,w4) + ...
            cross2d(G5-P0,w5) + ...
            u1;

        eqnInertia0 = ...
            cross2d(G1-P0,m1*ddG1) + ddq1*I1 + ...
            cross2d(G2-P0,m2*ddG2) + ddq2*I2 + ...
            cross2d(G3-P0,m3*ddG3) + ddq3*I3 + ...
            cross2d(G4-P0,m4*ddG4) + ddq4*I4 + ...
            cross2d(G5-P0,m5*ddG5) + ddq5*I5;

        %%%% AMB - swing leg, torso, stance femer  @ stance knee
        eqnTorque1 = ...
            cross2d(G2-P1,w2) + ...
            cross2d(G3-P1,w3) + ...
            cross2d(G4-P1,w4) + ...
            cross2d(G5-P1,w5) + ...
            u2;

        eqnInertia1 = ...
            cross2d(G2-P1,m2*ddG2) + ddq2*I2  + ...
            cross2d(G3-P1,m3*ddG3) + ddq3*I3  + ...
            cross2d(G4-P1,m4*ddG4) + ddq4*I4  + ...
            cross2d(G5-P1,m5*ddG5) + ddq5*I5 ;

        %%%% AMB - swing leg, torso @ hip
        eqnTorque2 = ...
            cross2d(G3-P2,w3) + ...
            cross2d(G4-P2,w4) + ...
            cross2d(G5-P2,w5) + ...
            u3;

        eqnInertia2 = ...
            cross2d(G3-P2,m3*ddG3) + ddq3*I3  + ...
            cross2d(G4-P2,m4*ddG4) + ddq4*I4  + ...
            cross2d(G5-P2,m5*ddG5) + ddq5*I5 ;

        %%%% AMB - swing leg @ hip
        eqnTorque3 = ...
            cross2d(G4-P2,w4) + ...
            cross2d(G5-P2,w5) + ...
            u4;

        eqnInertia3 = ...
            cross2d(G4-P2,m4*ddG4) + ddq4*I4  + ...
            cross2d(G5-P2,m5*ddG5) + ddq5*I5 ;

        %%%% AMB - swing tibia % swing knee
        eqnTorque4 = ...
            cross2d(G5-P4,w5) + ...
            u5;

        eqnInertia4 = ...
            cross2d(G5-P4,m5*ddG5) + ddq5*I5 ;

        %%%% Collect and solve equations:
        eqns = [...
            eqnTorque0 - eqnInertia0;
            eqnTorque1 - eqnInertia1;
            eqnTorque2 - eqnInertia2;
            eqnTorque3 - eqnInertia3;
            eqnTorque4 - eqnInertia4];

        [MM, FF] = equationsToMatrix(eqns,ddq);  % ddq = MM\ff;

        %%%% Compute gradients:
        [m, mi, mz, mzi, mzd] = computeGradients(MM,z,empty);
        [f, fi, fz, fzi, fzd] = computeGradients(FF,z,empty);

        % Write function file:
        matlabFunction(m, mi, f, fi,...   %dynamics
            mz, mzi, mzd, fz, fzi, fzd,...  %gradients
            'file','autoGen_dynSs.m',...
            'vars',{...
            'q1','q2','q3','q4','q5',...
            'dq1','dq2','dq3','dq4','dq5',...
            'u1','u2','u3','u4','u5',...
            'm1','m2','m3','m4','m5',...
            'I1','I2','I3','I4','I5',...
            'l1','l2','l3','l4',...
            'c1','c2','c3','c4','c5',...
            'g','empty'});

    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Objective Functions                              %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function objectiveFunctions()

        % Joint rates:
        v1 = dq1;   % joint rate 1
        v2 = dq2-dq1;   % joint rate 2
        v3 = dq3-dq2; % joint rate 3
        v4 = dq4-dq3;  % joint rate 4
        v5 = dq5-dq4;  % joint rate 5

        % Compute the power used by each joint
        pow1 = v1*u1;  %Power used by joint 1
        pow2 = v2*u2;  %Power used by joint 2
        pow3 = v3*u3;  %Power used by joint 3
        pow4 = v4*u4;  %Power used by joint 4
        pow5 = v5*u5;  %Power used by joint 5

        % Constraint on the slack variables:
        slackCst = [...
            pow1 - (sp1 - sn1);
            pow2 - (sp2 - sn2);
            pow3 - (sp3 - sn3);
            pow4 - (sp4 - sn4);
            pow5 - (sp5 - sn5)];

        % Gradients of the constraint on slack variables:
        [c, ~, cz, czi, ~] = computeGradients(slackCst,z,empty);

        matlabFunction(c,cz,czi,...
            'file','autoGen_cst_costOfTransport.m',...
            'vars',{...
            'dq1','dq2','dq3','dq4','dq5',...
            'u1','u2','u3','u4','u5',...
            'sn1','sn2','sn3','sn4','sn5',...
            'sp1','sp2','sp3','sp4','sp5','empty'});

        % abs(power) using slack variables:
        gammaNeg = sym('gammaNeg','real');  %Torque-squared smoothing parameter
        gammaPos = sym('gammaPos','real');  %Torque-squared smoothing parameter
        absPower = gammaNeg*(sn1 + sn2 + sn3 + sn4 + sn5) + ...
            gammaPos*(sp1 + sp2 + sp3 + sp4 + sp5);

        % Cost of Transport:
        weight = (m1+m2+m3+m4+m5)*g;
        stepLength = sym('stepLength','real');
        alpha = sym('alpha','real');  %Torque-squared smoothing parameter
        beta = sym('beta','real');  %Torque-rate squared smoothing
        F = absPower/(weight*stepLength) + ...
            alpha*(u1^2 + u2^2 + u3^2 + u4^2 + u5^2) + ...
            beta*(du1^2 + du2^2 + du3^2 + du4^2 + du5^2);
        [f, ~, fz, fzi, ~]  = computeGradients(F,z,empty);

        matlabFunction(f,fz,fzi,...
            'file','autoGen_obj_costOfTransport.m',...
            'vars',{...
            'm1','m2','m3','m4','m5',...
            'u1','u2','u3','u4','u5',...
            'du1','du2','du3','du4','du5',...
            'sn1','sn2','sn3','sn4','sn5', ...
            'sp1','sp2','sp3','sp4','sp5',...
            'g','stepLength','gammaNeg','gammaPos','alpha','beta','empty'});

        % Swing foot height:
        stepHeight = sym('stepHeight','real');
        yFoot = P5(2);
        xFoot = P5(1);
        yMin = stepHeight*(1 - (xFoot/stepLength)^2);
        yCst = yMin - yFoot;  %Must be negative
        [y, ~, yz, yzi, ~]  = computeGradients(yCst,z,empty);
        matlabFunction(y,yz,yzi,...
                    'file','autoGen_cst_swingFootHeight.m',...
            'vars',{...
            'q1','q2','q4','q5',...
            'l1','l2','l4','l5'...
            'stepLength','stepHeight'});
    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Heel-Strike Dynamics                             %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function heelStrikeDynamics()
        disp('Deriving heel-strike dynamics...')

        %%%% Notes:
        % xF - heelStrike(xI) --> constraint --> 0
        % xF - collision(footSwap(xI));
        %

        % Angles before heel-strike:
        q1m = sym('q1m','real');
        q2m = sym('q2m','real');
        q3m = sym('q3m','real');
        q4m = sym('q4m','real');
        q5m = sym('q5m','real');
        qm = [q1m;q2m;q3m;q4m;q5m];

        % Angles after heel-strike
        q1p = sym('q1p','real');
        q2p = sym('q2p','real');
        q3p = sym('q3p','real');
        q4p = sym('q4p','real');
        q5p = sym('q5p','real');
        qp = [q1p;q2p;q3p;q4p;q5p];

        % Angular rates before heel-strike:
        dq1m = sym('dq1m','real');
        dq2m = sym('dq2m','real');
        dq3m = sym('dq3m','real');
        dq4m = sym('dq4m','real');
        dq5m = sym('dq5m','real');
        dqm = [dq1m;dq2m;dq3m;dq4m;dq5m];

        % Angular rates after heel-strike
        dq1p = sym('dq1p','real');
        dq2p = sym('dq2p','real');
        dq3p = sym('dq3p','real');
        dq4p = sym('dq4p','real');
        dq5p = sym('dq5p','real');
        dqp = [dq1p;dq2p;dq3p;dq4p;dq5p];

        % torque before heel-strike:
        u1m = sym('u1m','real');
        u2m = sym('u2m','real');
        u3m = sym('u3m','real');
        u4m = sym('u4m','real');
        u5m = sym('u5m','real');
        um = [u1m;u2m;u3m;u4m;u5m];

        % torque after heel-strike
        u1p = sym('u1p','real');
        u2p = sym('u2p','real');
        u3p = sym('u3p','real');
        u4p = sym('u4p','real');
        u5p = sym('u5p','real');
        up = [u1p;u2p;u3p;u4p;u5p];

        % Compute kinematics before heel-strike:
        inVars = {'q1','q2','q3','q4','q5','dq1','dq2','dq3','dq4','dq5'};
        outVarsM = {'q1m','q2m','q3m','q4m','q5m','dq1m','dq2m','dq3m','dq4m','dq5m'};
        %         P0m = subs(P0,inVars,outVarsM);
        P1m = subs(P1,inVars,outVarsM);
        P2m = subs(P2,inVars,outVarsM);
        %         P3m = subs(P3,inVars,outVarsM);
        P4m = subs(P4,inVars,outVarsM);
        P5m = subs(P5,inVars,outVarsM);
        dP5m = subs(dP5,inVars,outVarsM);
        G1m = subs(G1,inVars,outVarsM);
        G2m = subs(G2,inVars,outVarsM);
        G3m = subs(G3,inVars,outVarsM);
        G4m = subs(G4,inVars,outVarsM);
        G5m = subs(G5,inVars,outVarsM);
        dG1m = subs(dG1,inVars,outVarsM);
        dG2m = subs(dG2,inVars,outVarsM);
        dG3m = subs(dG3,inVars,outVarsM);
        dG4m = subs(dG4,inVars,outVarsM);
        dG5m = subs(dG5,inVars,outVarsM);

        % Compute kinematics after heel-strike:
        outVarsP = {'q1p','q2p','q3p','q4p','q5p','dq1p','dq2p','dq3p','dq4p','dq5p'};
        P0p = subs(P0,inVars,outVarsP);
        P1p = subs(P1,inVars,outVarsP);
        P2p = subs(P2,inVars,outVarsP);
        %         P3p = subs(P3,inVars,outVarsP);
        P4p = subs(P4,inVars,outVarsP);
        %         P5p = subs(P5,inVars,outVarsP);
        dP5p = subs(dP5,inVars,outVarsP);
        G1p = subs(G1,inVars,outVarsP);
        G2p = subs(G2,inVars,outVarsP);
        G3p = subs(G3,inVars,outVarsP);
        G4p = subs(G4,inVars,outVarsP);
        G5p = subs(G5,inVars,outVarsP);
        dG1p = subs(dG1,inVars,outVarsP);
        dG2p = subs(dG2,inVars,outVarsP);
        dG3p = subs(dG3,inVars,outVarsP);
        dG4p = subs(dG4,inVars,outVarsP);
        dG5p = subs(dG5,inVars,outVarsP);

        %%%% AMB - entire system @ New stance foot
        eqnHs0m = ...   %Before collision
            cross2d(G1m-P5m,m1*dG1m) + dq1m*I1 + ...
            cross2d(G2m-P5m,m2*dG2m) + dq2m*I2 + ...
            cross2d(G3m-P5m,m3*dG3m) + dq3m*I3 + ...
            cross2d(G4m-P5m,m4*dG4m) + dq4m*I4 + ...
            cross2d(G5m-P5m,m5*dG5m) + dq5m*I5;
        eqnHs0 = ...   %After collision
            cross2d(G1p-P0p,m1*dG1p) + dq1p*I1 + ...
            cross2d(G2p-P0p,m2*dG2p) + dq2p*I2 + ...
            cross2d(G3p-P0p,m3*dG3p) + dq3p*I3 + ...
            cross2d(G4p-P0p,m4*dG4p) + dq4p*I4 + ...
            cross2d(G5p-P0p,m5*dG5p) + dq5p*I5;

        %%%% AMB - new swing leg, torso, stance femer  @  stance knee
        eqnHs1m = ...   %Before collision
            cross2d(G1m-P4m,m1*dG1m) + dq1m*I1 + ...
            cross2d(G2m-P4m,m2*dG2m) + dq2m*I2 + ...
            cross2d(G3m-P4m,m3*dG3m) + dq3m*I3 + ...
            cross2d(G4m-P4m,m4*dG4m) + dq4m*I4;
        eqnHs1 = ...   %After collision
            cross2d(G2p-P1p,m2*dG2p) + dq2p*I2 + ...
            cross2d(G3p-P1p,m3*dG3p) + dq3p*I3 + ...
            cross2d(G4p-P1p,m4*dG4p) + dq4p*I4 + ...
            cross2d(G5p-P1p,m5*dG5p) + dq5p*I5;

        %%%% AMB - swing leg, torso  @ new hip
        eqnHs2m = ...   %Before collision
            cross2d(G3m-P2m,m3*dG3m) + dq3m*I3 + ...
            cross2d(G2m-P2m,m2*dG2m) + dq2m*I2 + ...
            cross2d(G1m-P2m,m1*dG1m) + dq1m*I1;
        eqnHs2 = ...   %After collision
            cross2d(G3p-P2p,m3*dG3p) + dq3p*I3 + ...
            cross2d(G4p-P2p,m4*dG4p) + dq4p*I4 + ...
            cross2d(G5p-P2p,m5*dG5p) + dq5p*I5;

        %%%% AMB - swing leg @ new hip
        eqnHs3m = ...   %Before collision
            cross2d(G1m-P2m,m1*dG1m) + dq1m*I1 + ...
            cross2d(G2m-P2m,m2*dG2m) + dq2m*I2;
        eqnHs3 = ...   %After collision
            cross2d(G4p-P2p,m4*dG4p) + dq4p*I4 + ...
            cross2d(G5p-P2p,m5*dG5p) + dq5p*I5;

        %%%% AMB - swing tibia @ new swing knee
        eqnHs4m = ...   %Before collision
            cross2d(G1m-P1m,m1*dG1m) + dq1m*I1;
        eqnHs4 = ...   %After collision
            cross2d(G5p-P4p,m5*dG5p) + dq5p*I5;

        %%%% Collect and solve equations:
        eqnHs = [...
            eqnHs0m - eqnHs0;
            eqnHs1m - eqnHs1;
            eqnHs2m - eqnHs2;
            eqnHs3m - eqnHs3;
            eqnHs4m - eqnHs4];
        [MM, FF] = equationsToMatrix(eqnHs,dqp);

        %%%% Compute gradients:
        tp = sym('tp','real');   %Initial trajectory time
        tm = sym('tm','real');   %Final trajectory time
        zBnd = [tp;qp;dqp;up;tm;qm;dqm;um];
        [m, mi, mz, mzi, mzd] = computeGradients(MM,zBnd,empty);
        [f, fi, fz, fzi, fzd] = computeGradients(FF,zBnd,empty);

        % Heel-strike
        matlabFunction(m, mi, f, fi,...   %dynamics
            mz, mzi, mzd, fz, fzi, fzd,...  %gradients
            'file','autoGen_cst_heelStrike.m',...
            'vars',{...
            'q1p','q2p','q3p','q4p','q5p',...
            'q1m','q2m','q3m','q4m','q5m',...
            'dq1m','dq2m','dq3m','dq4m','dq5m',...
            'm1','m2','m3','m4','m5',...
            'I1','I2','I3','I4','I5',...
            'l1','l2','l3','l4','l5',...
            'c1','c2','c3','c4','c5','empty'});

        % Collision velocity of the swing foot:
        cst = [-dP5p(2); dP5m(2)];  %Swing foot velocity before and after collision (negative sign is intentional, since output is constrained to be negative);
        cstJac = jacobian(cst,zBnd);  %Gradient
        matlabFunction(cst, cstJac,...
            'file','autoGen_cst_footVel.m',...
            'vars',{...
            'q1p','q2p','q4p','q5p',...
            'q1m','q2m','q4m','q5m',...
            'dq1p','dq2p','dq4p','dq5p',...
            'dq1m','dq2m','dq4m','dq5m',...
            'l1','l2','l4','l5'});

        % Step length and height constraint:
        stepLength = sym('stepLength','real');
        ceq = [P5m(1)-stepLength; P5m(2)];
        ceqJac = jacobian(ceq,zBnd);  %Gradient
        matlabFunction(ceq, ceqJac,...
            'file','autoGen_cst_steplength.m',...
            'vars',{...
            'q1m','q2m','q4m','q5m',...
            'l1','l2','l4','l5','stepLength'});
    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                         Mechanical Energy                               %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function mechanicalEnergy()
        disp('Deriving mechanical energy...')

        %%%% Energy:
        KineticEnergy = ...
            0.5*m1*dot(dG1,dG1) + 0.5*I1*dq1^2 + ...
            0.5*m2*dot(dG2,dG2) + 0.5*I2*dq2^2 + ...
            0.5*m3*dot(dG3,dG3) + 0.5*I3*dq3^2 + ...
            0.5*m4*dot(dG4,dG4) + 0.5*I4*dq4^2 + ...
            0.5*m5*dot(dG5,dG5) + 0.5*I5*dq5^2;
        PotentialEnergy = ...
            m1*g*G1(2) + ...
            m2*g*G2(2) + ...
            m3*g*G3(2) + ...
            m4*g*G4(2) + ...
            m5*g*G5(2);

        matlabFunction(KineticEnergy, PotentialEnergy,...
            'file','autoGen_energy.m',...
            'vars',{...
            'q1','q2','q3','q4','q5',...
            'dq1','dq2','dq3','dq4','dq5',...
            'm1','m2','m3','m4','m5',...
            'I1','I2','I3','I4','I5',...
            'l1','l2','l3','l4',...
            'c1','c2','c3','c4','c5',...
            'g'},...
            'outputs',{'KE','PE'});

    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                          Contact Forces                                 %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function contactForces()

        %%%% Contact Forces:
        eqnForce5 = w1 + w2 + w3 + w4 + w5 + Fx*i + Fy*j;
        eqnInertia5 = (m1+m2+m3+m4+m5)*ddG;
        [AA,bb] = equationsToMatrix(eqnForce5-eqnInertia5,[Fx;Fy]);
        ContactForces = AA\bb;

        matlabFunction(ContactForces(1),ContactForces(2),...
            'file','autoGen_contactForce.m',...
            'vars',{...
            'q1','q2','q3','q4','q5',...
            'dq1','dq2','dq3','dq4','dq5',...
            'ddq1','ddq2','ddq3','ddq4','ddq5',...
            'm1','m2','m3','m4','m5',...
            'l1','l2','l3','l4',...
            'c1','c2','c3','c4','c5',...
            'g'},...
            'outputs',{'Fx','Fy'});

    end

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                        Write Kinematics Files                           %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%

    function kinematics()
        disp('Writing kinematics files...')

        P = [P1; P2; P3; P4; P5];
        Gvec = [G1; G2; G3; G4; G5];

        % Used for plotting and animation
        matlabFunction(P,Gvec,'file','autoGen_getPoints.m',...
            'vars',{...
            'q1','q2','q3','q4','q5',...
            'l1','l2','l3','l4','l5',...
            'c1','c2','c3','c4','c5'},...
            'outputs',{'P','Gvec'});

    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                       Helper Functions                                  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [m, mi, mz, mzi, dim] = computeGradients(M,z,empty)
%
% This function computes the gradients of a matrix M with respect the the
% variables in z, and then returns both the matrix and its gradient as
% column vectors of their non-zero elements, along with the linear indicies
% to unpack them. It also simplifies m and mz.
%
% INPUTS:
%   M = [na, nb] = symbolic matrix
%   z = [nc, 1] = symbolic vector
%
% OUTPUTS:
%   m = [nd, 1] = symbolic vector of non-zero elements in M
%   mi = [nd, 1] = linear indicies to map m --> [na,nb] matrix
%   mz = [ne, 1] = symbolic vector of non-zero elements in Mz
%   mzi = [ne, 1] = linear indicies to map mz --> [na,nb,nc] array
%   dim = [3,1] = [na,nb,nc] = dimensions of 3d version of mz
%

[na, nb] = size(M);
nc = size(z,1);
M = simplify(M);

mz2 = jacobian(M(:),z);  %Compute jacobian of M, by first reshaping M to be a column vector
mz3 = reshape(mz2,na,nb,nc); %Expand back out to a three-dimensional array
mz3 = simplify(mz3);

% Extract non-zero elements to a column vector:
mi = find(M);
m = M(mi);
mzi = find(mz3);
mz = mz3(mzi); mz = mz(:);  %Collapse to a column vector
dim = [na,nb,nc];

% Pad any constant terms with "empty" to permit vectorization:
m = vectorizeHack(m, z, empty);
mz = vectorizeHack(mz, z, empty);

end

function x = vectorizeHack(x, z, empty)
%
% This function searches for any elements of x that are not dependent on
% any element of z. In this case, the automatically generated code will
% fail to vectorize properly. One solution is to add an array of zeros
% (empty) to the element.
%
% x = column vector of symbolic expressions
% z = column vector of symbolic variables
% z = symbolic variable, which the user will set equal to zero.
%

% Compute dependencies
g = jacobian(x,z);

% Check for rows of x with no dependence on z
[n,m] = size(g);
idxConst = true(n,1);
for i=1:n
    for j=1:m
        if ~isequal(sym(0),g(i,j))
            idxConst(i) = false;
            break;
        end
    end
end

% Add empty to those enteries
x(idxConst) = x(idxConst) + empty;

end


function centroids = kMeansInitCentroids(X, K)
%KMEANSINITCENTROIDS This function initializes K centroids that are to be
%used in K-Means on the dataset X
%   centroids = KMEANSINITCENTROIDS(X, K) returns K initial centroids to be
%   used with the K-Means on the dataset X
%

% You should return this values correctly
centroids = zeros(K, size(X, 2));

% ====================== YOUR CODE HERE ======================
% Instructions: You should set centroids to randomly chosen examples from
%               the dataset X
%
% Initialize the centroids to be random examples
% Randomly reorder the indices of examples
randidx = randperm(size(X, 1));
% Take the first K examples as centroids
centroids = X(randidx(1:K), :);

% =============================================================

end


% SCAT_ENERGY Calculate scattering energy
%
% Usage
%    energy = scat_energy(S, U)
%
% Input
%    S (cell): The scattering transform.
%    U (cell): The wavelet modulus coefficients (optional).
%
% Output
%    energy (numeric): The energy of the scattering transform (the sum of the
%       squares of the coefficients). If both S and U are given, the energy of
%       both are computed and summed.

function energy = scat_energy(S, U)
	if nargin < 2
		U = [];
	end

	if ~isempty(U)
		energy = scat_energy(S) + scat_energy(U);
	else
		if iscell(S)
			energy = 0;
			for m = 1:numel(S)
				energy = energy + scat_energy(S{m});
			end
		else
			energy = 0;
			for p = 1:numel(S.signal)
				sig = S.signal{p};
				% TODO: fix so that multiple signals work!!
				%sz = size(sig);
				%sig = reshape(sig,[prod(sz(1:end-1)) sz(end)]);
				energy = energy + sum(abs(sig(:)).^2,1);
			end
		end
	end
end


function [B_detail, B_smooth] = train_choose_detail(training_path, unit, col)
% choosing image patches which contain more detail

fileFolder=fullfile(training_path);
dirOutput=dir(fullfile(fileFolder,'*'));
num = length(dirOutput);
% fileNames={dirOutput.name}'; % all names
B = [];
for i = 3:num
    img = imread([training_path,dirOutput(i).name]);
    if size(img,3)>1
        img = rgb2gray(img);
    end

    [t1,t2] = size(img);
    img = img(1:floor(t1/unit)*unit, 1:floor(t2/unit)*unit);

    img = im2double(img);
    b = im2col(img, [unit, unit], 'distinct');
    B = cat(2, B, b);
end
% calculating SD valuee of each column B
B_mean = mean(B,1);
B_mean = repmat(B_mean, unit*unit, 1);
B_sd = sqrt(sum((B - B_mean).*(B - B_mean),1));
% choose >0.5
B_sd(find(B_sd<=0.5)) = 0;
B_sd(find(B_sd>0.5)) = 1;
B_mask = repmat(B_sd, unit*unit, 1);
B_smooth = B.*(1-B_mask);
B_detail = B.*B_mask;
B_smooth(:,all(B_smooth==0,1))=[];
B_detail(:,all(B_detail==0,1))=[];

[m1,n1] = size(B_detail);
[m2,n2] = size(B_smooth);

end


%
% Usage:   [V [val_regularizer]]=mexProximalGraph(U,graph,param);
%
% Name: mexProximalGraph
%
% Description: mexProximalGraph computes a proximal operator. Depending
%         on the value of param.regul, it computes
%
%         Given an input matrix U=[u^1,\ldots,u^n], and a set of groups G,
%         it computes a matrix V=[v^1,\ldots,v^n] such that
%
%         if param.regul='graph'
%         for every column u of U, it computes a column v of V solving
%             argmin 0.5||u-v||_2^2 + lambda\sum_{g \in G} \eta_g||v_g||_inf
%
%         if param.regul='graph+ridge'
%         for every column u of U, it computes a column v of V solving
%             argmin 0.5||u-v||_2^2 + lambda\sum_{g \in G} \eta_g||v_g||_inf + lambda_2||v||_2^2
%
%
%         if param.regul='multi-task-graph'
%            V=argmin 0.5||U-V||_F^2 + lambda \sum_{i=1}^n\sum_{g \in G} \eta_g||v^i_g||_inf + ...
%                                                lambda_2 \sum_{g \in G} \eta_g max_{j in g}||V_j||_{inf}
%
%         it can also be used with any regularization addressed by mexProximalFlat
%
%         for all these regularizations, it is possible to enforce non-negativity constraints
%         with the option param.pos, and to prevent the last row of U to be regularized, with
%         the option param.intercept
%
% Inputs: U:  double p x n matrix   (input signals)
%               m is the signal size
%         graph: struct
%               with three fields, eta_g, groups, and groups_var
%
%               The first fields sets the weights for every group
%                  graph.eta_g            double N vector
%
%               The next field sets inclusion relations between groups
%               (but not between groups and variables):
%                  graph.groups           sparse (double or boolean) N x N matrix
%                  the (i,j) entry is non-zero if and only if i is different than j and
%                  gi is included in gj.
%
%               The next field sets inclusion relations between groups and variables
%                  graph.groups_var       sparse (double or boolean) p x N matrix
%                  the (i,j) entry is non-zero if and only if the variable i is included
%                  in gj, but not in any children of gj.
%
%               examples are given in test_ProximalGraph.m
%
%         param: struct
%               param.lambda  (regularization parameter)
%               param.regul (choice of regularization, see above)
%               param.lambda2  (optional, regularization parameter)
%               param.lambda3  (optional, regularization parameter)
%               param.verbose (optional, verbosity level, false by default)
%               param.intercept (optional, last row of U is not regularized,
%                 false by default)
%               param.pos (optional, adds positivity constraints on the
%                 coefficients, false by default)
%               param.numThreads (optional, number of threads for exploiting
%                 multi-core / multi-cpus. By default, it takes the value -1,
%                 which automatically selects all the available CPUs/cores).
%
% Output: V: double p x n matrix (output coefficients)
%         val_regularizer: double 1 x n vector (value of the regularization
%         term at the optimum).
%
% Author: Julien Mairal, 2010


function [Y,W,SetupStruc] = Process_DSB(s,Transfer,SetupStruc)
K = SetupStruc.DSB.K;
hop = SetupStruc.DSB.hop;
win = hanning(K,'periodic');
win = win/sqrt(sum(win(1:hop:K).^2));
SetupStruc.DSB.win = win;  % Preserve 'win' in 'SetupStruc'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = size(s,2);
for i = 1:N
    X(:,:,i) = fft(enframe(s(:,i),win,hop)');
end
frame_N = size(X,2);
K_m = K/2+1;
Num = size(Transfer,3);
Y = zeros((frame_N-1)*hop+K,Num);
Y_f = zeros(size(X,1),size(X,2),Num);
%%%%%%%%%%%%%%%%%%%%%%%%%% Obtain processing matrix 'W'
W = conj(permute(Transfer,[3 2 1]))/N;
for i = 2:K_m
    X_f = permute(X(i,:,:),[3 2 1]);
    W_f = permute(W(:,:,i),[1 2 3]);
    Y_ = W_f*X_f;
    Y_f(i,:,:) = Y_.';
    if(i~=K_m)
        Y_f(K+2-i,:,:) = Y_';
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Recover signals
if(K/hop==2)
    win = ones(K,1);
end
for i = 1:Num
    Y(:,i) = overlapadd(real(ifft(Y_f(:,:,i)))',win,hop);
end

return;


function [Peg, Peh] = getPe(K, Ec, No, N, pulse);

[mew, sigma] = get_mean_var_G(K, N, Ec, pulse.m1);
Ig = sqrt(N*No/2 + mew);
Th = N*sqrt(Ec);       %Non- Coherent
Peg = qfunc(Th/Ig);
[mew, sigma] = get_mean_var_H(K, N, Ec, pulse.m1, pulse.m2, pulse.w1, pulse.w2);
I1 = sqrt(No*N/2 + mew);
I2 = sqrt(No*N/2 + (mew + sqrt(3) * sigma));
I3 = sqrt(No*N/2 + (mew - sqrt(3) * sigma));
Peh = 2/3*qfunc(Th/I1) + 1/6*qfunc(Th/I2) + 1/6*qfunc(Th/I3);

function [mew_MAI, sigma_MAI] = get_mean_var_H(K, N, P, m1, m2, w1, w2)
T =1;
mew_MAI = (N*P * (K-1)*m1);
sigma_MAI = (N*P) * ((K-1)*( (.375*w1 - m1^2) + ((N-1)/(N^2))*((1.5*w2) + (K-2)*(m2^2) )))^(.5);

function [mew_MAI, sigma_MAI] = get_mean_var_G(K, N, P, m1)
T =1;
mew_MAI = (N*P * (K-1)*m1);
sigma_MAI = 0;


function varargout = sample(f, varargin)
%SAMPLE   Samples a DISKFUN object on a tensor product grid.
%   X = SAMPLE(F) returns the matrix of values of F(theta, r) on a
%   Fourier-Chebyshev tensor product grid. (theta, r) are polar coordinates,
%   with -pi <= theta <= pi and 0 <= r <= 1.
%
%   [U, D, V] = SAMPLE(F) returns the low rank representation of the
%   values of F on a tensor product grid where X = U * D * V'.
%
%   [U, D, V] = SAMPLE(F,M,N) returns the values of F on an M-by-N
%   tensor product grid.

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

% Empty check.
if ( isempty(f) )
    varargout = { [] };
    return
end

if ( nargin == 1 )
    % Get degrees:
    [m, n] = length(f);

elseif ( nargin == 2 )
    error('CHEBFUN:DISKFUN:sample:inputs', 'Dimension not specified.');

else
    m = varargin{ 1 };
    n = varargin{ 2 };
    if ( (m <= 0) || (n <= 0) )
        error('CHEBFUN:DISKFUN:sample:inputs', ['Number of sample ' ...
             'points must be positive.']);
    end
end

% Get the low rank representation for f.
[cols, d, rows] = cdr(f);

C = sample(cols, max(2*n-1, 1));
C = C(n:end, :);

R = real( sample(rows, m));

% Evaluate:
if ( nargout <= 1 )
    varargout = {C * d * R.'};
else
    varargout = {C , d, R};
end

end


%TR2Q	Convert homogeneous transform to a unit-quaternion
%
%	Q = tr2q(T)
%
%	Return a unit quaternion corresponding to the rotational part of the
%	homogeneous transform T.
%
%	See also Q2TR

%	Copyright (C) 1993 Peter Corke
function q = tr2q(t)
	q = zeros(1,4);
	q(1) = sqrt(trace(t))/2;
	kx = t(3,2) - t(2,3);	% Oz - Ay
	ky = t(1,3) - t(3,1);	% Ax - Nz
	kz = t(2,1) - t(1,2);	% Ny - Ox

	if (t(1,1) >= t(2,2)) & (t(1,1) >= t(3,3))
		kx1 = t(1,1) - t(2,2) - t(3,3) + 1;	% Nx - Oy - Az + 1
		ky1 = t(2,1) + t(1,2);			% Ny + Ox
		kz1 = t(3,1) + t(1,3);			% Nz + Ax
		add = (kx >= 0);
	elseif (t(2,2) >= t(3,3))
		kx1 = t(2,1) + t(1,2);			% Ny + Ox
		ky1 = t(2,2) - t(1,1) - t(3,3) + 1;	% Oy - Nx - Az + 1
		kz1 = t(3,2) + t(2,3);			% Oz + Ay
		add = (ky >= 0);
	else
		kx1 = t(3,1) + t(1,3);			% Nz + Ax
		ky1 = t(3,2) + t(2,3);			% Oz + Ay
		kz1 = t(3,3) - t(1,1) - t(2,2) + 1;	% Az - Nx - Oy + 1
		add = (kz >= 0);
	end

	if add
		kx = kx + kx1;
		ky = ky + ky1;
		kz = kz + kz1;
	else
		kx = kx - kx1;
		ky = ky - ky1;
		kz = kz - kz1;
	end
	nm = norm([kx ky kz]);
	if nm == 0,
		q = [1 0 0 0];
	else
		s = sqrt(1 - q(1)^2) / nm;
		q(2:4) = s*[kx ky kz];
	end


function dif_print ( ntab, xtab, diftab, title )

%*****************************************************************************80
%
%% DIF_PRINT prints the polynomial represented by a divided difference table.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NTAB, the dimension of the arrays DIFTAB and XTAB.
%
%    Input, real XTAB(NTAB), the X values for the polynomial.
%
%    Input, real DIFTAB(NTAB), the divided difference table
%    for the polynomial.
%
%    Input, string TITLE, a title.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  p(x) =                           %14f\n', diftab(1) );

  for i = 2 : ntab
    fprintf ( 1, '       + ( x - %14f) * ( %14f\n', xtab(i-1), diftab(i) );
  end

  fprintf ( 1, '  ' );
  for i = 1 : ntab-1
    fprintf ( 1, ')' );
  end
  fprintf ( 1, '\n' );

  return
end


function [ ap, rcond, z, info ] = sppco ( ap, n )

%*****************************************************************************80
%
%% SPPCO factors a real symmetric positive definite matrix in packed form.
%
%  Discussion:
%
%    SPPCO also estimates the condition of the matrix.
%
%    If RCOND is not needed, SPPFA is slightly faster.
%
%    To solve A*X = B, follow SPPCO by SPPSL.
%
%    To compute inverse(A)*C, follow SPPCO by SPPSL.
%
%    To compute determinant(A), follow SPPCO by SPPDI.
%
%    To compute inverse(A), follow SPPCO by SPPDI.
%
%  Packed storage:
%
%    The following program segment will pack the upper triangle of
%    a symmetric matrix.
%
%      k = 0
%      do j = 1, n
%        do i = 1, j
%          k = k + 1
%          ap(k) = a(i,j)
%        end
%      end
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real AP(N*(N+1)/2), the packed form of a symmetric matrix A.
%    The columns of the upper triangle are stored sequentially in a
%    one-dimensional array.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real AP(N*(N+1)/2), an upper riangular matrix R, stored
%    in packed form, so that A = R'*R.  If INFO /= 0, the factorization
%    is not complete.
%
%    Output, real RCOND, an estimate of the reciprocal condition
%    of A.  For the system A*X = B, relative perturbations in A and B of size
%    EPSILON may cause relative perturbations in X of size EPSILON/RCOND.
%    If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0D+00
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate underflows.
%
%    Output, real Z(N), a work vector whose contents are usually
%    unimportant.  If A is singular to working precision, then Z is an
%    approximate null vector in the sense that
%      norm(A*Z) = RCOND * norm(A) * norm(Z).
%    If INFO /= 0, Z is unchanged.
%
%    Output, integer INFO, error flag.
%    0, for normal return.
%    K, signals an error condition.  The leading minor of order K is
%    not positive definite.
%

%
%  Find the norm of A.
%
  j1 = 1;
  for j = 1 : n
    z(j) = sasum ( j, ap(j1:j1+j-1), 1 );
    ij = j1;
    j1 = j1 + j;
    for i = 1 : j-1
      z(i) = z(i) + abs ( ap(ij) );
      ij = ij + 1;
    end
  end

  anorm = max ( z(1:n) );
%
%  Factor.
%
  [ ap, info ] = sppfa ( ap, n );

  if ( info ~= 0 )
    return
  end
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A * Z = Y and A * Y = E.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where R'*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve R' * W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  kk = 0;

  for k = 1 : n

    kk = kk + k;

    if ( z(k) ~= 0.0 )
      ek = -abs ( ek ) * r4_sign ( z(k) );
    end

    if ( ap(kk) < abs ( ek - z(k) ) )
      s = ap(kk) / abs ( ek - z(k) );
      z(1:n) = s * z(1:n);
      ek = s * ek;
    end

    wk = ek - z(k);
    wkm = -ek - z(k);
    s = abs ( wk );
    sm = abs ( wkm );
    wk = wk / ap(kk);
    wkm = wkm / ap(kk);
    kj = kk + k;

    if ( k + 1 <= n )

      for j = k + 1 : n
        sm = sm + abs ( z(j) + wkm * ap(kj) );
        z(j) = z(j) + wk * ap(kj);
        s = s + abs ( z(j) );
        kj = kj + j;
      end

      if ( s < sm )

        t = wkm - wk;
        wk = wkm;
        kj = kk + k;

        for j = k+1 : n
          z(j) = z(j) + t * ap(kj);
          kj = kj + j;
        end

      end

    end

    z(k) = wk;

  end

  z(1:n) = z(1:n) / sasum ( n, z(1:n), 1 );
%
%  Solve R * Y = W.
%
  for k = n : -1 : 1

    if ( ap(kk) < abs ( z(k) ) );
      s = ap(kk) / abs ( z(k) );
      z(1:n) = s * z(1:n);
    end

    z(k) = z(k) / ap(kk);
    kk = kk - k;
    t = -z(k);
    z(1:k-1) = saxpy ( k-1, t, ap(kk+1:kk+k-1), 1, z(1:k-1), 1 );

  end

  z(1:n) = z(1:n) / sasum ( n, z(1:n), 1 );

  ynorm = 1.0;
%
%  Solve R' * V = Y.
%
  for k = 1 : n

    z(k) = z(k) - sdot ( k-1, ap(kk+1:kk+k-1), 1, z(1:k-1), 1 );
    kk = kk + k;

    if ( ap(kk) < abs ( z(k) ) )
      s = ap(kk) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    z(k) = z(k) / ap(kk);

  end

  s = 1.0 / sasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;
%
%  Solve R * Z = V.
%
  for k = n : -1 : 1

    if ( ap(kk) < abs ( z(k) ) )
      s = ap(kk) / abs ( z(k) );
      z(1:n) = s * z(1:n);
      ynorm = s * ynorm;
    end

    z(k) = z(k) / ap(kk);
    kk = kk - k;
    t = -z(k);
    z(1:k-1) = saxpy ( k-1, t, ap(kk+1:kk+k-1), 1, z(1:k-1), 1 );

  end
%
%  Make ZNORM = 1.0.
%
  s = 1.0 / sasum ( n, z(1:n), 1 );
  z(1:n) = s * z(1:n);
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end


function RMC = fmri_mcorrestriction(RM,TR,nHEst,Delta,Tau)
%
% RMC = fmri_mcorrestriction(RM,TR,nHEst,Delta,Tau)
%
% Creates a restriction matrix for correlating with a
% hemodynmic response function (HRF).  The HRF is
% computed using a Gamma function with parameters
% Delta and Tau over the estimation time window.
%
% RM is a restriction matrix created by CreateRM.
%
% See also: CreateRM(), HemoDyn()
%
% $Id: fmri_mcorrrestriction.m,v 1.1 2004/03/11 01:30:16 sayres Exp $

t = TR*[0:nHEst-1]';
hHDIR = fmri_hemodyn(t,Delta,Tau);
nCond = size(RM,2)/nHEst; % excluding fix %
q = repmat(hHDIR',size(RM,1),nCond);
RMC = RM .* q;

return;


function [ tri_num, tri_vert, tri_nabe ] = r8tris2 ( point_num, p )

%*****************************************************************************80
%
%% R8TRIS2 constructs a Delaunay triangulation of 2D vertices.
%
%  Discussion:
%
%    The routine constructs the Delaunay triangulation of a set of 2D vertices
%    using an incremental approach and diagonal edge swaps.  Vertices are
%    first sorted in lexicographically increasing (X,Y) order, and
%    then are inserted one at a time from outside the convex hull.
%
%  Modified:
%
%    07 February 2005
%
%  Author:
%
%    Original FORTRAN77 version by Barry Joe.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Barry Joe,
%    GEOMPACK - a software package for the generation of meshes
%    using geometric algorithms,
%    Advances in Engineering Software,
%    Volume 13, pages 325-331, 1991.
%
%  Parameters:
%
%    Input, integer POINT_NUM, the number of vertices.
%
%    Input, real P(2,POINT_NUM), the vertices.
%
%    Output, integer TRI_NUM, the number of triangles in the triangulation;
%    TRI_NUM is equal to 2*POINT_NUM - NB - 2, where NB is the number
%    of boundary vertices.
%
%    Output, integer TRI_VERT(3,TRI_NUM), the nodes that make up each triangle.
%    The elements are indices of P.  The vertices of the triangles are
%    in counter clockwise order.
%
%    Output, integer TRI_NABE(3,TRI_NUM), the triangle neighbor list.
%    Positive elements are indices of TIL; negative elements are used for links
%    of a counter clockwise linked list of boundary edges; LINK = -(3*I + J-1)
%    where I, J = triangle, edge index; TRI_NABE(J,I) refers to
%    the neighbor along edge from vertex J to J+1 (mod 3).
%
  tri_num = 0;
  tri_vert = [];
  tri_nabe = [];

  tol = 100.0 * r8_epsilon ( );
%
%  Sort the vertices by increasing (x,y).
%
  indx = r82vec_sort_heap_index_a ( point_num, p );

  p = r82vec_permute ( point_num, p, indx );
%
%  Make sure that the data points are "reasonably" distinct.
%
  m1 = 1;

  for i = 2 : point_num

    m = m1;
    m1 = i;

    k = 0;

    for j = 1 : 2

      cmax = max ( abs ( p(j,m) ), abs ( p(j,m1) ) );

      if ( tol * ( cmax + 1.0 ) < abs ( p(j,m) - p(j,m1) ) )
        k = j;
        break
      end

    end

    if ( k == 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8TRIS2 - Fatal error!\n' );
      fprintf ( 1, '  Fails for point number I = %d\n', i );
      fprintf ( 1, '  M = %d\n', m );
      fprintf ( 1, '  M1 = %d\n', m1 );
      fprintf ( 1, '  X,Y(M)  = %f  %f\n', p(1,m), p(2,m) );
      fprintf ( 1, '  X,Y(M1) = %f  %f\n', p(1,m1), p(2,m1) );
      error ( 'R8TRIS2 - Fatal error!' )
      return
    end

  end
%
%  Starting from points M1 and M2, search for a third point M that
%  makes a "healthy" triangle (M1,M2,M)
%
  m1 = 1;
  m2 = 2;
  j = 3;

  while ( 1 )

    if ( point_num < j )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8TRIS2 - Fatal error!\n' );
      error ( 'R8TRIS2 - Fatal error!' )
      return
    end

    m = j;

    lr = lrline ( p(1,m), p(2,m), p(1,m1), p(2,m1), p(1,m2), p(2,m2), 0.0 );

    if ( lr ~= 0 )
      break
    end

    j = j + 1;

  end
%
%  Set up the triangle information for (M1,M2,M), and for any other
%  triangles you created because points were collinear with M1, M2.
%
  tri_num = j - 2;

  if ( lr == -1 )

    tri_vert(1,1) = m1;
    tri_vert(2,1) = m2;
    tri_vert(3,1) = m;
    tri_nabe(3,1) = -3;

    for i = 2 : tri_num

      m1 = m2;
      m2 = i+1;
      tri_vert(1,i) = m1;
      tri_vert(2,i) = m2;
      tri_vert(3,i) = m;
      tri_nabe(1,i-1) = -3 * i;
      tri_nabe(2,i-1) = i;
      tri_nabe(3,i) = i - 1;

    end

    tri_nabe(1,tri_num) = -3 * tri_num - 1;
    tri_nabe(2,tri_num) = -5;
    ledg = 2;
    ltri = tri_num;

  else

    tri_vert(1,1) = m2;
    tri_vert(2,1) = m1;
    tri_vert(3,1) = m;
    tri_nabe(1,1) = -4;

    for i = 2 : tri_num
      m1 = m2;
      m2 = i+1;
      tri_vert(1,i) = m2;
      tri_vert(2,i) = m1;
      tri_vert(3,i) = m;
      tri_nabe(3,i-1) = i;
      tri_nabe(1,i) = -3 * i - 3;
      tri_nabe(2,i) = i - 1;
    end

    tri_nabe(3,tri_num) = -3 * tri_num;
    tri_nabe(2,1) = -3 * tri_num - 2;
    ledg = 2;
    ltri = 1;

  end
%
%  Insert the vertices one at a time from outside the convex hull,
%  determine visible boundary edges, and apply diagonal edge swaps until
%  Delaunay triangulation of vertices (so far) is obtained.
%
  top = 0;

  for i = j+1 : point_num

    m = i;
    m1 = tri_vert(ledg,ltri);

    if ( ledg <= 2 )
      m2 = tri_vert(ledg+1,ltri);
    else
      m2 = tri_vert(1,ltri);
    end

    lr = lrline ( p(1,m), p(2,m), p(1,m1), p(2,m1), p(1,m2), p(2,m2), 0.0 );

    if ( 0 < lr )
      rtri = ltri;
      redg = ledg;
      ltri = 0;
    else
      l = -tri_nabe(ledg,ltri);
      rtri = floor ( l / 3 );
      redg = mod(l,3) + 1;
    end

    [ ltri, ledg, rtri, redg ] = vbedg ( p(1,m), p(2,m), point_num, p, ...
      tri_num, tri_vert, tri_nabe, ltri, ledg, rtri, redg );

    n = tri_num + 1;
    l = -tri_nabe(ledg,ltri);

    while ( 1 )

      t = floor ( l / 3 );
      e = mod ( l, 3 ) + 1;
      l = -tri_nabe(e,t);
      m2 = tri_vert(e,t);

      if ( e <= 2 )
        m1 = tri_vert(e+1,t);
      else
        m1 = tri_vert(1,t);
      end

      tri_num = tri_num + 1;
      tri_nabe(e,t) = tri_num;
      tri_vert(1,tri_num) = m1;
      tri_vert(2,tri_num) = m2;
      tri_vert(3,tri_num) = m;
      tri_nabe(1,tri_num) = t;
      tri_nabe(2,tri_num) = tri_num - 1;
      tri_nabe(3,tri_num) = tri_num + 1;
      top = top + 1;

      if ( point_num < top )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'R8TRIS2 - Fatal error!\n' );
        fprintf ( 1, '  Stack overflow.\n' );
        error ( 'R8TRIS2 - Fatal error!' )
      end

      work(top) = tri_num;

      if ( t == rtri & e == redg )
        break
      end

    end

    tri_nabe(ledg,ltri) = -3 * n - 1;
    tri_nabe(2,n) = -3 * tri_num - 2;
    tri_nabe(3,tri_num) = -l;
    ltri = n;
    ledg = 2;

    [ top, ltri, ledg, tri_vert, tri_nabe ] = swapec ( ...
      m, top, ltri, ledg, point_num, p, tri_num, tri_vert, tri_nabe, work );

  end
%
%  Now account for the sorting that we did.
%
  for i = 1 : 3
    for j = 1 : tri_num
      tri_vert(i,j) = indx ( tri_vert(i,j) );
    end
  end

  indx = perm_inverse ( point_num, indx );

  p = r82vec_permute ( point_num, p, indx );

  return
end


% RecoverByGraInitial.m
% RecoverByGra.m
% -------------------------------------------------------------------
% This function is just the joint of RecoverByGra and initial f0
% Date:    17/03/2013
% Last modified: 16/04/2015
% -------------------------------------------------------------------

function [imgRec, rms, gObj] = RecByGraInitial(img1, img2, ww1, ww2, dxdy, iter, res, alpha, iniMode)

    % ------------ Check parameter --------------
%     narginchk(9, 9);
%     if size(img1, 3) ~=1 || size(img2, 3) ~=1,
%         error('The image should be gray');
%     end
    % -------------------------------------------
    switch lower(iniMode),
        case 'avg',
            f0 = (img1+img2)/2;
        case 'weight',
            f0 = ww1.*img1+ww2.*img2;
        otherwise
            error('There only two mode');
    end

    [imgRec, rms, gObj] = RecoverByGra(f0, dxdy, iter, res, alpha);

end

%%
function [imgRec, rms, gObj] = RecoverByGra(imgOri, Obj, iter, res, alpha)

    lp = [0 1 0;1 -4 1;0 1 0];
    if isreal(Obj),
%         disp('The original is the Laplace');
        gObj = Obj;
    else
%         disp('The original is the Gradient');
        gObj = LaplaceZ(Obj);
    end

    rms = [];
%     f0 = Boundary(imgOri, dxObj, dyObj);
    f0 = imgOri;
    for ii = 1:iter,
%         f0 = Boundary(f0, dxObj, dyObj);
        deltaF = imfilter(int16(f0), lp, 'replicate', 'corr');
        deltaF = double(deltaF);
        delta = alpha * (deltaF-gObj);

        f1 = f0 + delta;

        f1 = max(f1, 0);
        f1 = min(f1, 255);

        f0 = f1;
    end

    disp(['The ' num2str(ii) ' iteration is complete.']);
    imgRec = f1;
end

%%
function lap = LaplaceZ(IMGORGRA)

    if isreal(IMGORGRA),
%         disp('The input should be IMAGE')
        lh=[0,  1, 0;...
            1, -4, 1;...
            0,  1, 0];
        lap = imfilter(IMGORGRA, lh, 'replicate', 'corr');
    else
%         disp('The input should be GRADIENT');
        fx=[0, 0, 0; 0, -1, 1; 0, 0, 0];
        fy=[0, 0, 0;  0, -1, 0; 0, 1, 0];

        dx = real(IMGORGRA);
        dy = imag(IMGORGRA);

        ddx = imfilter(dx, fx, 0, 'corr');
        ddy = imfilter(dy, fy, 0, 'corr');

        lap=ddx + ddy;
    end
end


classdef ZXH_CF10 < PROBLEM
% <multi/many> <real> <large/none> <constrained>
% Constrained benchmark MOP proposed by Zhou, Xiang, and He

%------------------------------- Reference --------------------------------
% Y. Zhou, Y. Xiang, and X. He, Constrained multiobjective optimization:
% Test problem construction and performance evaluations, IEEE Transactions
% on Evolutionary Computation, 2021, 25(1): 172-186.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        k;  % Number of constrained variables
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            if isempty(obj.M); obj.M = 3; end
            if isempty(obj.D); obj.D = obj.M+10;  end
            obj.lower    = zeros(1,obj.D) + 1e-10;
            obj.upper    = ones(1,obj.D)  - 1e-10;
            obj.encoding = ones(1,obj.D);
            if obj.M <= 3
                obj.k = obj.M - 1;
            elseif obj.M > 3 && obj.M <= 8
                obj.k = floor(obj.M/2);
            else
                obj.k = 3;
            end
        end
        %% Evaluate multiple solutions
        function Population = Evaluation(obj,varargin)
            PopDec = varargin{1};
            OptX   = 0.2;
            [N,D]  = size(PopDec);
            M      = obj.M;
            % Step 1: Compute cumsum
            Sx = cumsum(PopDec(:,1:M).^2,2,'reverse');
            % Step 2: Compute theta
            THETA = 2/pi*atan(sqrt(Sx(:,2:end))./PopDec(:,1:M-1));
            % Step 3: Calculate Griewank function
            h = 5*(sum((PopDec(:,M+1:end)-OptX).^2,2)-prod(cos(10*pi*(PopDec(:,M+1:end)-OptX)./repmat(sqrt(1:(D-M)),N,1)),2)+1);
            % Step 4: Compute T_
            T = (1 - Sx(:,1)).^2 + h;
            % Step 5: Objectives (convex)
            G      = 1-[ones(N,1) cumprod(sin(pi/2*THETA),2)] .* [cos(pi/2*THETA) ones(N,1)];
            PopObj = G .* repmat((1+T),1,M);
            % Step 6: Constraints
            PopCon(:,1) = Sx(:,1) + h - 1;
            PopCon(:,2) = -(Sx(:,1) + h - 1/4);
            for i = 1 : obj.k
                PopCon(:,i+2) = min(THETA(:,i)-1/4,3/4-THETA(:,i));
            end
            Population = SOLUTION(varargin{1},PopObj,PopCon,varargin{2:end});
            obj.FE     = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,obj.M);
            R = 1 - R./repmat(sqrt(sum(R.^2,2)),1,obj.M);
            T = zeros(size(R));
            for i = obj.M-1 : -1 : 1
                T(:,i) = atan((1-R(:,i+1))./(1-R(:,i))./cos(T(:,i+1)));
            end
            THETA = T(:,1:obj.k)*2/pi;
            Valid = all(THETA<=1/4|THETA>=3/4,2);
            R     = R(Valid,:);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            if obj.M == 2
                R = obj.GetOptimum(100);
            elseif obj.M == 3
                a  = linspace(0,pi/2,30)';
                R  = {1-sin(a)*cos(a'),1-sin(a)*sin(a'),1-cos(a)*ones(size(a'))};
                T2 = atan((1-R{3})./(1-R{2}));
                T1 = asin((1-R{2})./cos(T2));
                THETA = cat(3,T1,T2)*2/pi;
                Valid = all(THETA<=1/4|THETA>=3/4,3);
                R{1}(~Valid) = nan;
            else
                R = [];
            end
        end
    end
end


% SYNTAX:
% data_dod = hmrR_MotionCorrectRLOESS(data_dod, span, turnon)
%
% UI NAME:
% Motion_Correct_RLOESS
%
% DESCRIPTION:
%
% INPUTS:
% data_dod: SNIRF data structure containing delta_OD
% span:
% turnon:   Optional argument to enable/disable this function in a processing stream chain
%
% OUTPUTS:
% data_dod: SNIRF data structure containing delta_OD after motion correction,
%           same size as dod (Channels that are not in the active ml remain unchanged)
%
% USAGE OPTIONS:
% Motion_Correct_RLOESS: dod = hmrR_MotionCorrectRLOESS(dod, span, turnon)
%
% PARAMETERS:
% span: 0.02
% turnon: 1
%
% PREREQUISITES:
% Intensity_to_Delta_OD: dod = hmrR_Intensity2OD( intensity )
%
% LOG:
%
function data_dod = hmrR_MotionCorrectRLOESS(data_dod, span, turnon)

% span = 0.02 (default)
if span<0
    return
end

% Meryem Yucel, Oct, 2017
% Added turn on/off option Meryem Nov 2017
if ~exist('turnon','var')
    turnon = 1;
end
if turnon==0
    return;
end

for iBlk=1:length(data_dod)
    dod = data_dod(iBlk).GetDataTimeSeries();
    t   = data_dod(iBlk).GetTime();
    for i=1:size(dod,2)
        dod(:,i) = smooth(t, dod(:,i), span, 'rloess');
    end
    data_dod(iBlk).SetDataTimeSeries(dod);
end


function R = unidrndKPM(min, max, nr, nc)

if nargin < 3
  nr = 1; nc = 1;
end

R = unidrnd(max-min+1, nr, nc) + (min-1);


function jaccard = Jaccard_Index(SEG, GT)
    % SEG, GT are the binary segmentation and ground truth areas, respectively.
    % jaccard index
    jaccard = double(sum(uint8(SEG(:) & GT(:)))) / double(sum(uint8(SEG(:) | GT(:))));
end


% Lifted matrices for MPC on horizon N
function [Ab Bb] = createMPCmatrices(A,B,N,includex0)
    if(~exist('includex0','var'))
        includex0 = 0; %first block rows of Ab and Bb corresponding to x0 removed
    end

    n = size(A,1);
    Ab = zeros((N+1)*n, n);
    Ab(1:n,:) = eye(n,n);
    for i = 2:N+1
        if( size(A,1) > 1000 )
            i
        end
        %Ab((i-1)*n+1:i*n,:) = A^(i-1);
        Ab((i-1)*n+1:i*n,:) = Ab((i-2)*n+1:(i-1)*n,:)*A;
    end
    if(includex0 == 0)
        Ab = Ab(n+1:end,:); % Seems to take a lot of time if A is huge, possible to speedup
    end

    for q = 1:length(B)
        m = size(B{q},2);
        Bb{q} = zeros((N+1)*n, N*m);
        for i = 2:N+1
            Bb{q}((i-1)*n+1:i*n,:) = A * Bb{q}((i-2)*n+1:(i-1)*n,:);
            Bb{q}((i-1)*n+1:n*i,(i-2)*m+1:m*(i-1)) = B{q};
        end
        if(includex0 == 0)
            Bb{q} = Bb{q}(n+1:end,:);
        end
    end

end


% Fit a piece-wise linear regression model.
% Here is the model
%
%  X \
%  | |
%  Q |
%  | /
%  Y
%
% where all arcs point down.
% We condition everything on X, so X is a root node. Q is a softmax, and Y is a linear Gaussian.
% Q is hidden, X and Y are observed.

X = 1;
Q = 2;
Y = 3;
dag = zeros(3,3);
dag(X,[Q Y]) = 1;
dag(Q,Y) = 1;
ns = [1 2 1]; % make X and Y scalars, and have 2 experts
dnodes = [2];
onodes = [1 3];
bnet = mk_bnet(dag, ns, 'discrete', dnodes, 'observed', onodes);

IRLS_iter = 10;
clamped = 0;

bnet.CPD{1} = root_CPD(bnet, 1);

% start with good initial params
w = [-5 5];  % w(:,i) is the normal vector to the i'th decisions boundary
b = [0 0];  % b(i) is the offset (bias) to the i'th decisions boundary

mu = [0 0];
sigma = 1;
Sigma = repmat(sigma*eye(ns(Y)), [ns(Y) ns(Y) ns(Q)]);
W = [-1 1];
W2 = reshape(W, [ns(Y) ns(X) ns(Q)]);

bnet.CPD{2} = softmax_CPD(bnet, 2, w, b,  clamped, IRLS_iter);
bnet.CPD{3} = gaussian_CPD(bnet, 3, 'mean', mu, 'cov', Sigma, 'weights', W2);

engine = jtree_inf_engine(bnet);

evidence = cell(1,3);
evidence{X} = 0.68;

engine = enter_evidence(engine, evidence);

m = marginal_nodes(engine, Y);
m.mu


% qrtimax() -  perform Quartimax rotation of rows of a data matrix.
%
% Usage: >> [Q,B] = qrtimax(data);
%        >> [Q,B] = qrtimax(data,tol,'[no]reorder');
%
% Inputs:
%        data      - input matrix
%        tol       - the termination tolerance {default: 1e-4}
%        noreorder - rotate without negation/reordering
%
% Outputs:
%        B         - B=Q*A the Quartimax rotation of A
%        Q         - the orthogonal rotation matrix
%
% Author: Sigurd Enghoff, CNL / Salk Institute, 6/18/98

% Copyright (C) Sigurd Enghoff - CNL / Salk Institute, La Jolla 6/18/98
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% Reference: Jack O. Nehaus and Charles Wrigley (1954)
% The Quartimax Method: an analytic approach to orthogonal
% simple structure, Br J Stat Psychol, 7:81-91.

% 01-25-02 reformated help & license -ad

function [Q,B] = qrtimax(A,tol,reorder)

if nargin < 1
	help qrtimax
	return
end

DEFAULT_TOL = 1e-4;
MAX_ITERATIONS = 50;

if nargin < 3
	reorder = 1;
elseif isempty(reorder) | reorder == 0
	reorder = 1; % set default
else
	reorder = strcmp('reorder',reorder);
end

if nargin < 2
	eps1 = DEFAULT_TOL;
	eps2 = DEFAULT_TOL;
else
	eps1 = tol;
	eps2 = tol;
end

% Do unto 'Q' what is done to A

Q = eye(size(A,1));

% Compute the cross-products of the rows of the squared loadings,
% i.e. the cost function.
%
%  ---  ---
%  \    \     2    2
%  /    /    f    f
%  ---  ---   ij   ik
%   i   j<k
%
%  See reference, p. 85, line 3

B = tril((A.^2)*(A.^2)');
crit = [sum(sum(B)) - trace(B) , 0];

% Initialize variables

inoim = 0;
iflip = 1;
ict = 0;

% Main iterative loop: keep looping while no two consecutive trials
% satisfy tolerance constraint AND less than MAX_ITERATIONS trials
% AND one or more rotations were performed during last trial.

while inoim < 2 & ict < MAX_ITERATIONS & iflip,
	iflip = 0;

% Run through all combinations of j and k

	for j = 1:size(A,1)-1,
		for k = j+1:size(A,1),
%
%          ---                                   ---
%          \                2     2              \       2     2  2      2   2
% fnum = 4 /    [f   f    (f   - f  )]  , fden = /    [(f   - f  )  - 4 f   f  ]
%          ---    ki  kj    ki    kj             ---     ki    kj        ki  kj
%           k                                     k
%
%  See equation (5)

			u = A(j,:) .^ 2 - A(k,:) .^2;
			v = 2 * A(j,:) .* A(k,:);
			c = sum(u .^ 2 - v .^ 2);
			d = sum(u .* v);

			fden = c;
			fnum = 2 * d;

% Skip rotation if angle is too small

			if abs(fnum) > eps1 * abs(fden)
				iflip = 1;
				angl = atan2(fnum, fden);

% Set angle of rotation according to Table I

				if fnum > 0
					if fden > 0
						angl = .25 * angl;
					else
						angl = .25 * (pi - angl);
					end
				else
					if fden > 0
						angl = .25 * (2 * pi - angl);
					else
						angl = .25 * (pi + angl);
					end
				end

% Perform rotation

				tmp    =  cos(angl) * Q(j,:) + sin(angl) * Q(k,:);
				Q(k,:) = -sin(angl) * Q(j,:) + cos(angl) * Q(k,:);
				Q(j,:) = tmp;

				tmp    =  cos(angl) * A(j,:) + sin(angl) * A(k,:);
				A(k,:) = -sin(angl) * A(j,:) + cos(angl) * A(k,:);
				A(j,:) = tmp;
			end
		end
	end

% Compute cost function.

	B = tril((A.^2)*(A.^2)');
	crit = [sum(sum(B)) - trace(B) , crit(1)];

	inoim = inoim + 1;
	ict = ict + 1;

	fprintf('#%d - crit = %g\n',ict,(crit(1)-crit(2))/crit(1));

% Check relative change of cost function (termination criterion).

	if (crit(1) - crit(2)) / crit(1) > eps2
		inoim = 0;
	end
end

% Reorder and negate if required. Determine new row order based on
% row norms and reorder accordingly. Negate those rows in which the
% accumulated sum is negative.

if reorder
	fprintf('Reordering rows...');
	[fnorm index] = sort(sum(A'.^2));
	Q = Q .* ((2 * (sum(A') > 0) - 1)' * ones(1, size(Q,2)));
	A = A .* ((2 * (sum(A') > 0) - 1)' * ones(1, size(A,2)));
	Q = Q(fliplr(index),:);
	A = A(fliplr(index),:);
	fprintf('\n');
else
	fprintf('Not reordering rows.\n');
end

B=A;


%  Figure 6.68      Feedback Control of Dynamic Systems, 6e
%                   Franklin, Powell, Emami
%

clear all
%close all;
clf

k=10;
num=k;
den=[1 1 0];
num=conv(num,[10 1]);
den=conv(den,[100 1]);
numcl=[1 0.1];
dencl=[1 1.01 1.01 0.1];
t=0:.2:50;
y=step(numcl,dencl,t);
%subplot(2,1,1)
plot(t,y);
xlabel('Time (sec)');
ylabel('y');
title('Fig. 6.68 Step response of lag-compensation design.');
nicegrid;


% MHSWTRANS
%
%  MCMC Metropolis-Hastings transition function that
%  utilizes the Swendsen-Wang proposal distribution.
%  A - The current joint assignment.  This should be
%      updated to be the next assignment
%  G - The network
%  F - List of all factors
%  variant - a number (1 or 2) indicating the variant of Swendsen-Wang to use.  In variant 1,
%            all the q_{i,j}'s are equal
%
% Copyright (C) Daphne Koller, Stanford University, 2012

function A = MHSWTrans(A, G, F, variant)

%%%%%%%%%%%%%% Get Proposal %%%%%%%%%%%%%%
% Prune edges from q_list if the nodes don't have the same current value
q_list = G.q_list;
q_keep_indx = find(A(q_list(:, 1)) == A(q_list(:, 2)));
q_list = q_list(q_keep_indx, :);
% Select edges at random based on q_list
selected_edges_q_list_indx = find(q_list(:, 3) > rand(size(q_list,1), 1));
selected_edges = q_list(selected_edges_q_list_indx, 1:2);
% Compute connected components over selected edges
SelEdgeMat = sparse([selected_edges(:,1)'; selected_edges(:,2)'],...
                    [selected_edges(:,2)'; selected_edges(:,1)'],...
                    1, length(G.names), length(G.names));

[var2comp, cc_sizes] = scomponents(SelEdgeMat);
num_cc = length(cc_sizes);

% Select a connected component (the book calls this Y)
selected_cc = ceil(rand() * num_cc);
selected_vars = find(var2comp == selected_cc);
% Check that the dimensions are all the same and they have the same current assignment
assert(length(unique(G.card(selected_vars))) == 1);
assert(length(unique(A(selected_vars))) == 1);

% Pick a new label via sampling
old_value = A(selected_vars(1));
d = G.card(selected_vars(1));
LogR = zeros(1, d);
if variant == 1
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % YOUR CODE HERE
    % Specify the log of the distribution (LogR) from
    % which a new label for Y is selected for variant 1
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    LogR = ones(1,d).*log(1/d);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif variant == 2
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % YOUR CODE HERE
    % Specify the log of the distribution (LogR) from
    % which a new label for Y is selected for variant 2
    %
    % We suggest you read through the preceding code
    % before implementing this, one of the generated
    % data structures may be useful in implementing this section
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    LogR = BlockLogDistribution(selected_vars,G,F,A);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
else
    disp('WARNING: Unrecognized Swendsen-Wang Variant');
end

% Sample the new value from the distribution R
new_value = randsample(d, 1, true, exp(LogR));
A_prop = A;
A_prop(selected_vars) = new_value;

% Get the log-ratio of the probability of picking the connected component Y given A_prop over A
log_QY_ratio = 0.0;
for i = 1:size(G.q_list, 1)  % Iterate through *all* edges, not just the ones we selected earlier
    u = G.q_list(i, 1);
    v = G.q_list(i, 2);
    if length(intersect([u, v], selected_vars)) == 1  % the edge is from Y to outside-Y
        if A(u) == old_value && A(v) == old_value
            log_QY_ratio = log_QY_ratio - log(1 - G.q_list(i, 3));
        end
        if A_prop(u) == new_value && A_prop(v) == new_value
            log_QY_ratio = log_QY_ratio + log(1 - G.q_list(i, 3));
        end
    end
end

p_acceptance = 0.0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% YOUR CODE HERE
% Compute acceptance probability
%
% Read through the preceding code to understand
% how to find the previous and proposed assignments
% of variables, as well as some ratios used in computing
% the acceptance probabilitiy.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
lnum = LogProbOfJointAssignment(F,A_prop);
lden = LogProbOfJointAssignment(F,A);

p_acceptance = min(1,exp(lnum-lden+log_QY_ratio+LogR(old_value)-LogR(new_value)));
%p_acceptance = min(1,exp(log_QY_ratio));
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Accept or reject proposal
if rand() < p_acceptance
    %disp('Accepted');
    A = A_prop;
end


classdef SMMOP2 < PROBLEM
% <multi/many> <real> <large/none> <multimodal> <sparse/none>
% Sparse multi-modal multi-objective optimization problem
% theta --- 0.1 --- Sparsity of the Pareto sets
% np    ---   4 --- Number of the Pareto sets

%------------------------------- Reference --------------------------------
% Y. Tian, R. Liu, X. Zhang, H. Ma, K. C. Tan, and Y. Jin, A
% multipopulation evolutionary algorithm for solving large-scale multimodal
% multiobjective optimization problems, IEEE Transactions on Evolutionary
% Computation, 2021, 25(3): 405-418.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties(Access = private)
        theta = 0.1;    % Sparsity of the Pareto sets
        np    = 4;    	% Number of the Pareto sets
        POS;            % Pareto optimal set for IGDX calculation
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            [obj.theta,obj.np] = obj.ParameterSet(0.1,4);
            if isempty(obj.M); obj.M = 2; end
            if isempty(obj.D); obj.D = 100; end
            obj.lower    = [zeros(1,obj.M-1)+0,zeros(1,obj.D-obj.M+1)-1];
            obj.upper    = [zeros(1,obj.M-1)+1,zeros(1,obj.D-obj.M+1)+2];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            [N,D] = size(X);
            M     = obj.M;
            S     = ceil(obj.theta*(D-M));
            g     = zeros(N,obj.np);
            for i = 1 : obj.np
            	g(:,i) =  sum(g1(X(:,M+(i-1)*S:M+i*S-1),pi/3),2)+sum(g3(X(:,[M:M+(i-1)*S-1,M+i*S:end]),0),2);
            end
            PopObj = repmat(1+min(g,[],2)/(D-M+1),1,M).*fliplr(cumprod([ones(N,1),X(:,1:M-1)],2)).*[ones(N,1),1-X(:,M-1:-1:1)];
        end
        %% Generate Pareto optimal solutions
        function R = GetOptimum(obj,N)
            % Generate points in Pareto optimal set
            A       = GetPS(obj.D-obj.M+1,obj.np,ceil(obj.theta*(obj.D-obj.M)));
            X       = UniformPoint(N/size(A,1),obj.M-1,'grid');
            obj.POS = [repmat(X,size(A,1),1),A(repmat(1:end,size(X,1),1),:)];
            % Generate points on Pareto front
            R = UniformPoint(N,obj.M);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            if obj.M == 2
                a = linspace(0,1,100)';
                R = [a,1-a];
            elseif obj.M == 3
                a = linspace(0,1,10)';
                R = {a*a',a*(1-a'),(1-a)*ones(size(a'))};
            else
                R = [];
            end
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            switch metName
                case 'IGDX'
                    score = feval(metName,Population,obj.POS);
                otherwise
                    score = feval(metName,Population,obj.optimum);
            end
        end
        %% Display a population in the objective space
        function DrawObj(obj,Population)
            PopDec = Population.decs;
            A      = GetPS(obj.D-obj.M+1,obj.np,ceil(obj.theta*(obj.D-obj.M)));
            [~,Label]  = min(pdist2(PopDec(:,obj.M:end),A),[],2);
            tempStream = RandStream('mlfg6331_64','Seed',2);
            if obj.M == 2
                for i = 1 : size(A,1)
                    color = rand(tempStream,1,3);
                    Draw(Population(Label==i).objs+(i-1)*0.05,'o','MarkerSize',6,'Marker','o','Markerfacecolor',sqrt(color),'Markeredgecolor',color,{'\it f\rm_1','\it f\rm_2',[]});
                    Draw(obj.PF+(i-1)*0.05,'-','LineWidth',1,'Color',color);
                end
            elseif obj.M == 3
                for i = 1 : size(A,1)
                    color = rand(tempStream,1,3);
                    ax = Draw(Population(Label==i).objs+(i-1)*0.05,'o','MarkerSize',8,'Marker','o','Markerfacecolor',sqrt(color),'Markeredgecolor',color,{'\it f\rm_1','\it f\rm_2','\it f\rm_3'});
                    surf(ax,obj.PF{1}+(i-1)*0.05,obj.PF{2}+(i-1)*0.05,obj.PF{3}+(i-1)*0.05,'EdgeColor',color,'FaceColor','none');
                end
            else
                for i = 1 : size(A,1)
                    Draw(Population(Label==i).objs,'-','Color',rand(tempStream,1,3),'LineWidth',2);
                end
            end
        end
    end
end

function g = g1(x,t)
    g = (x-t).^2;
end

function g = g3(x,t)
    g = 4-(x-t)-4./exp(100*(x-t).^2);
end

function PS = GetPS(D,np,S)
    PS = zeros(np,D);
    for i = 1 : np
        PS(i,(i-1)*S+1:i*S) = 1;
    end
end


%randsample(V,n,true,distribution) returns a set of n values sampled
% at random from the integers 1 through V with replacement using distribution
% 'distribution'
%
% replacing true with false causes sampling w/out replacement
% omitting the distribution causes a default to the uniform distribution
%
% Copyright (C) Daphne Koller, Stanford University, 2012

function [v] = randsample(vals,numSamp,replace,weightIncrements)

  vals = vals(:);
  if(length(vals)==1)
    maxval = vals;
    vals = 1:maxval;
  else
    maxval = length(vals);
  end

  if(exist('replace','var')~=1)
    replace = true;
  end
  if(exist('weightIncrements','var')~=1)
    weightIncrements = (1/maxval)*ones(maxval,1);
    weights = (1/maxval):(1/maxval):1;
  else
    weightIncrements = weightIncrements(:)/sum(weightIncrements(:));
    weights = zeros(size(weightIncrements));
    weights(1) = weightIncrements(1);
    for i = 2:length(weightIncrements)
      weights(i) = weightIncrements(i)+weights(i-1);
    end
  end

  weights = [0; weights(:)];

  %now do the sampling
  v = [];
  probs = rand(numSamp,1);
  for i=1:numSamp
    curInd = find((weights(1:end-1)<=probs(i))&(weights(2:end)>=probs(i)));
    v(end+1)=vals(curInd);
    if(replace~=true)
      vals(curInd)=[];
      weightIncrements(curInd)=[];
      weightIncrements = weightIncrements(:)/sum(weightIncrements(:));
      weights = zeros(size(weightIncrements));
      for i = 2:length(weightIncrements)
        weights(i) = weightIncrements(i)+weights(i-1);
      end
    end
  end

end


% test minq.m, minqdef.m, minqsep.m

disp('test of minq.m')

n=7;
m=10;
p=0.8;		% approx. fraction of activities and equalities
newdata=1;	% new data?

if newdata,
  % create random data satisfying the KKT conditions
  A=rand(m,n);
  c=rand(n,1);
  d=rand(n,1);
  ydes=rand(m,1)-p;
  act=( ydes>p );		% indices of nonactive inequalities
  ydes(act)=0*ydes(act);
  eq=( ydes<0 );		% indices of equations
  xdes=(A'*ydes-c)./d;
  res=rand(m,1);
  res(~act)=0*res(~act);
  b=A*xdes-res;
  save minq_data
 prt=0;
else
  load minq_data
  warning debug
  prt=1;
end;

for cas=1:2,
  if cas==1,
    disp('test of minqsep.m');
    [x,y,ier]=minqsep(c,d,A,b,eq,prt);
  else
    disp('test of minqdef.m');
    [x,y,ier]=minqdef(c,diag(d),A,b,eq,prt);
  end;

  compldes=[ydes,A*xdes-b];
  compl=[y,A*x-b];
  act_eq=[act,eq]'
  ydif=[ydes,y]
  disp('usually two equal columns');
  disp('but sometimes the dual is not unique')
  xdif=[xdes,x]
  disp('xdif should have two equal columns')
  ier
  if cas==2, break; end;
  cont=input('enter return (next test) or 0 (quit)>');
  if isempty(cont),	% continue
  elseif cont==0,      return;
  end;
end;


classdef CEC2013_F5 < PROBLEM
% <single> <real> <large>
% 7-nonseparable, 1-separable shifted and rotated Rastrigin's function

%------------------------------- Reference --------------------------------
% X. Li, K. Tang, M. N. Omidvar, Z. Yang, and K. Qin, Benchmark functions
% for the CEC'2013 special session and competition on large-scale global
% optimization, RMIT University, Australia, 2013.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        Xopt;	% Optimal decision vector
        R25;    % Rotation matrices
        R50;
        R100;
        p;      % Rank of decision variables
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2013.mat'),'Data');
            obj.Xopt = Data{5}.xopt;
            obj.R25  = Data{5}.R25;
            obj.R50  = Data{5}.R50;
            obj.R100 = Data{5}.R100;
            obj.p    = Data{5}.p;
            obj.M    = 1;
            obj.D    = 1000;
            obj.lower    = zeros(1,obj.D) - 5;
            obj.upper    = zeros(1,obj.D) + 5;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            S = [50 25 25 100 50 25 25 700];
            W = [1.81e-1 9.08e3 2.43e1 1.86e-6 1.77e4 2.82e-4 1.53e-2 1];
            PopDec = PopDec - repmat(obj.Xopt,size(PopDec,1),1);
            PopObj = zeros(size(PopDec,1),1);
            for i = 1 : length(S)
                loc = obj.p(sum(S(1:i-1))+1:sum(S(1:i)));
                switch S(i)
                    case 25
                        PopDec(:,loc) = PopDec(:,loc)*obj.R25;
                    case 50
                        PopDec(:,loc) = PopDec(:,loc)*obj.R50;
                    case 100
                        PopDec(:,loc) = PopDec(:,loc)*obj.R100;
                end
                PopObj = PopObj + W(i)*Rastrigin(Tdiag(Tasy(Tosz(PopDec(:,loc)),0.2),10));
            end
        end
    end
end

function F = Rastrigin(X)
    F = sum(X.^2-10*cos(2*pi*X)+10,2);
end

function Z = Tosz(X)
    X1 = zeros(size(X));
    X1(X~=0) = log(abs(X(X~=0)));
    C1 = zeros(size(X)) + 5.5;
    C1(X>0) = 10;
    C2 = zeros(size(X)) + 3.1;
    C2(X>0) = 7.9;
    Z = sign(X).*exp(X1+0.049*(sin(C1.*X1)+sin(C2.*X1)));
end

function Z = Tasy(X,beta)
    Z = X.^(1+repmat(beta*linspace(0,1,size(X,2)),size(X,1),1).*sqrt(X));
    Z(X<=0) = X(X<=0);
end

function Z = Tdiag(X,alpha)
    Z = X.*repmat(sqrt(alpha).^linspace(0,1,size(X,2)),size(X,1),1);
end