function dz = dynamics_6_link(~,z,P)
%DZ = DYNAMICS_6_LINK(T,Z,P)
%
%FUNCTION:  This function computes the dynamics of a 6
%    link pendulum, and is designed to be called from ode45.
%    The model allows for arbitrary mass and inertia for each
%    link, but no friction or actuation
%
%INPUTS:
%    t = time. Dummy input for ode45. Not used.
%    z = [12 X nTime]  matrix of states
%    P = struct of parameters
%OUTPUTS:
%    dz = [12 X nTime]  matrix of state derivatives
%
%NOTES:
%    This file was automatically generated by writeDynamics.m

g  = P.g ; %gravity
m1 = P.m(1); % Link 1 mass
m2 = P.m(2); % Link 2 mass
m3 = P.m(3); % Link 3 mass
m4 = P.m(4); % Link 4 mass
m5 = P.m(5); % Link 5 mass
m6 = P.m(6); % Link 6 mass
l1 = P.l(1); % Link 1 length
l2 = P.l(2); % Link 2 length
l3 = P.l(3); % Link 3 length
l4 = P.l(4); % Link 4 length
l5 = P.l(5); % Link 5 length
l6 = P.l(6); % Link 6 length
I1 = P.I(1); % Link 1 moment of inertia about its center of mass
I2 = P.I(2); % Link 2 moment of inertia about its center of mass
I3 = P.I(3); % Link 3 moment of inertia about its center of mass
I4 = P.I(4); % Link 4 moment of inertia about its center of mass
I5 = P.I(5); % Link 5 moment of inertia about its center of mass
I6 = P.I(6); % Link 6 moment of inertia about its center of mass
d1 = P.d(1); % Link 1 distance between center of mass and parent joint
d2 = P.d(2); % Link 2 distance between center of mass and parent joint
d3 = P.d(3); % Link 3 distance between center of mass and parent joint
d4 = P.d(4); % Link 4 distance between center of mass and parent joint
d5 = P.d(5); % Link 5 distance between center of mass and parent joint
d6 = P.d(6); % Link 6 distance between center of mass and parent joint

nTime = size(z,2);
dz = zeros(size(z));
M = zeros(6,6,nTime);
f = zeros(6,nTime);

th1 = z(1,:);
th2 = z(2,:);
th3 = z(3,:);
th4 = z(4,:);
th5 = z(5,:);
th6 = z(6,:);

dth1 = z(7,:);
dth2 = z(8,:);
dth3 = z(9,:);
dth4 = z(10,:);
dth5 = z(11,:);
dth6 = z(12,:);

dz(1,:) = dth1;
dz(2,:) = dth2;
dz(3,:) = dth3;
dz(4,:) = dth4;
dz(5,:) = dth5;
dz(6,:) = dth6;

M(1,1,:) = - I1 - d1.^2.*m1 - l1.^2.*m2 - l1.^2.*m3 - l1.^2.*m4 - l1.^2.*m5 - l1.^2.*m6;
M(1,2,:) = -l1.*cos(th1 - th2).*(d2.*m2 + l2.*m3 + l2.*m4 + l2.*m5 + l2.*m6);
M(1,3,:) = -l1.*cos(th1 - th3).*(d3.*m3 + l3.*m4 + l3.*m5 + l3.*m6);
M(1,4,:) = -l1.*cos(th1 - th4).*(d4.*m4 + l4.*m5 + l4.*m6);
M(1,5,:) = -l1.*cos(th1 - th5).*(d5.*m5 + l5.*m6);
M(1,6,:) = -d6.*l1.*m6.*cos(th1 - th6);
M(2,1,:) = -l1.*cos(th1 - th2).*(d2.*m2 + l2.*m3 + l2.*m4 + l2.*m5 + l2.*m6);
M(2,2,:) = - I2 - d2.^2.*m2 - l2.^2.*m3 - l2.^2.*m4 - l2.^2.*m5 - l2.^2.*m6;
M(2,3,:) = -l2.*cos(th2 - th3).*(d3.*m3 + l3.*m4 + l3.*m5 + l3.*m6);
M(2,4,:) = -l2.*cos(th2 - th4).*(d4.*m4 + l4.*m5 + l4.*m6);
M(2,5,:) = -l2.*cos(th2 - th5).*(d5.*m5 + l5.*m6);
M(2,6,:) = -d6.*l2.*m6.*cos(th2 - th6);
M(3,1,:) = -l1.*cos(th1 - th3).*(d3.*m3 + l3.*m4 + l3.*m5 + l3.*m6);
M(3,2,:) = -l2.*cos(th2 - th3).*(d3.*m3 + l3.*m4 + l3.*m5 + l3.*m6);
M(3,3,:) = - I3 - d3.^2.*m3 - l3.^2.*m4 - l3.^2.*m5 - l3.^2.*m6;
M(3,4,:) = -l3.*cos(th3 - th4).*(d4.*m4 + l4.*m5 + l4.*m6);
M(3,5,:) = -l3.*cos(th3 - th5).*(d5.*m5 + l5.*m6);
M(3,6,:) = -d6.*l3.*m6.*cos(th3 - th6);
M(4,1,:) = -l1.*cos(th1 - th4).*(d4.*m4 + l4.*m5 + l4.*m6);
M(4,2,:) = -l2.*cos(th2 - th4).*(d4.*m4 + l4.*m5 + l4.*m6);
M(4,3,:) = -l3.*cos(th3 - th4).*(d4.*m4 + l4.*m5 + l4.*m6);
M(4,4,:) = - I4 - d4.^2.*m4 - l4.^2.*m5 - l4.^2.*m6;
M(4,5,:) = -l4.*cos(th4 - th5).*(d5.*m5 + l5.*m6);
M(4,6,:) = -d6.*l4.*m6.*cos(th4 - th6);
M(5,1,:) = -l1.*cos(th1 - th5).*(d5.*m5 + l5.*m6);
M(5,2,:) = -l2.*cos(th2 - th5).*(d5.*m5 + l5.*m6);
M(5,3,:) = -l3.*cos(th3 - th5).*(d5.*m5 + l5.*m6);
M(5,4,:) = -l4.*cos(th4 - th5).*(d5.*m5 + l5.*m6);
M(5,5,:) = - I5 - d5.^2.*m5 - l5.^2.*m6;
M(5,6,:) = -d6.*l5.*m6.*cos(th5 - th6);
M(6,1,:) = -d6.*l1.*m6.*cos(th1 - th6);
M(6,2,:) = -d6.*l2.*m6.*cos(th2 - th6);
M(6,3,:) = -d6.*l3.*m6.*cos(th3 - th6);
M(6,4,:) = -d6.*l4.*m6.*cos(th4 - th6);
M(6,5,:) = -d6.*l5.*m6.*cos(th5 - th6);
M(6,6,:) = - I6 - d6.^2.*m6;

f(1,:) = - d1.*g.*m1.*cos(th1) - g.*l1.*m2.*cos(th1) - g.*l1.*m3.*cos(th1) - g.*l1.*m4.*cos(th1) - g.*l1.*m5.*cos(th1) - g.*l1.*m6.*cos(th1) - d2.*dth2.^2.*l1.*m2.*sin(th1 - th2) - d3.*dth3.^2.*l1.*m3.*sin(th1 - th3) - d4.*dth4.^2.*l1.*m4.*sin(th1 - th4) - d5.*dth5.^2.*l1.*m5.*sin(th1 - th5) - d6.*dth6.^2.*l1.*m6.*sin(th1 - th6) - dth2.^2.*l1.*l2.*m3.*sin(th1 - th2) - dth2.^2.*l1.*l2.*m4.*sin(th1 - th2) - dth2.^2.*l1.*l2.*m5.*sin(th1 - th2) - dth2.^2.*l1.*l2.*m6.*sin(th1 - th2) - dth3.^2.*l1.*l3.*m4.*sin(th1 - th3) - dth3.^2.*l1.*l3.*m5.*sin(th1 - th3) - dth3.^2.*l1.*l3.*m6.*sin(th1 - th3) - dth4.^2.*l1.*l4.*m5.*sin(th1 - th4) - dth4.^2.*l1.*l4.*m6.*sin(th1 - th4) - dth5.^2.*l1.*l5.*m6.*sin(th1 - th5);
f(2,:) = d2.*dth1.^2.*l1.*m2.*sin(th1 - th2) - g.*l2.*m3.*cos(th2) - g.*l2.*m4.*cos(th2) - g.*l2.*m5.*cos(th2) - g.*l2.*m6.*cos(th2) - d2.*g.*m2.*cos(th2) - d3.*dth3.^2.*l2.*m3.*sin(th2 - th3) - d4.*dth4.^2.*l2.*m4.*sin(th2 - th4) - d5.*dth5.^2.*l2.*m5.*sin(th2 - th5) - d6.*dth6.^2.*l2.*m6.*sin(th2 - th6) + dth1.^2.*l1.*l2.*m3.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m4.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m5.*sin(th1 - th2) + dth1.^2.*l1.*l2.*m6.*sin(th1 - th2) - dth3.^2.*l2.*l3.*m4.*sin(th2 - th3) - dth3.^2.*l2.*l3.*m5.*sin(th2 - th3) - dth3.^2.*l2.*l3.*m6.*sin(th2 - th3) - dth4.^2.*l2.*l4.*m5.*sin(th2 - th4) - dth4.^2.*l2.*l4.*m6.*sin(th2 - th4) - dth5.^2.*l2.*l5.*m6.*sin(th2 - th5);
f(3,:) = d3.*dth1.^2.*l1.*m3.*sin(th1 - th3) - g.*l3.*m4.*cos(th3) - g.*l3.*m5.*cos(th3) - g.*l3.*m6.*cos(th3) - d3.*g.*m3.*cos(th3) + d3.*dth2.^2.*l2.*m3.*sin(th2 - th3) - d4.*dth4.^2.*l3.*m4.*sin(th3 - th4) - d5.*dth5.^2.*l3.*m5.*sin(th3 - th5) - d6.*dth6.^2.*l3.*m6.*sin(th3 - th6) + dth1.^2.*l1.*l3.*m4.*sin(th1 - th3) + dth1.^2.*l1.*l3.*m5.*sin(th1 - th3) + dth1.^2.*l1.*l3.*m6.*sin(th1 - th3) + dth2.^2.*l2.*l3.*m4.*sin(th2 - th3) + dth2.^2.*l2.*l3.*m5.*sin(th2 - th3) + dth2.^2.*l2.*l3.*m6.*sin(th2 - th3) - dth4.^2.*l3.*l4.*m5.*sin(th3 - th4) - dth4.^2.*l3.*l4.*m6.*sin(th3 - th4) - dth5.^2.*l3.*l5.*m6.*sin(th3 - th5);
f(4,:) = d4.*dth1.^2.*l1.*m4.*sin(th1 - th4) - g.*l4.*m5.*cos(th4) - g.*l4.*m6.*cos(th4) - d4.*g.*m4.*cos(th4) + d4.*dth2.^2.*l2.*m4.*sin(th2 - th4) + d4.*dth3.^2.*l3.*m4.*sin(th3 - th4) - d5.*dth5.^2.*l4.*m5.*sin(th4 - th5) - d6.*dth6.^2.*l4.*m6.*sin(th4 - th6) + dth1.^2.*l1.*l4.*m5.*sin(th1 - th4) + dth1.^2.*l1.*l4.*m6.*sin(th1 - th4) + dth2.^2.*l2.*l4.*m5.*sin(th2 - th4) + dth2.^2.*l2.*l4.*m6.*sin(th2 - th4) + dth3.^2.*l3.*l4.*m5.*sin(th3 - th4) + dth3.^2.*l3.*l4.*m6.*sin(th3 - th4) - dth5.^2.*l4.*l5.*m6.*sin(th4 - th5);
f(5,:) = d5.*dth1.^2.*l1.*m5.*sin(th1 - th5) - g.*l5.*m6.*cos(th5) - d5.*g.*m5.*cos(th5) + d5.*dth2.^2.*l2.*m5.*sin(th2 - th5) + d5.*dth3.^2.*l3.*m5.*sin(th3 - th5) + d5.*dth4.^2.*l4.*m5.*sin(th4 - th5) - d6.*dth6.^2.*l5.*m6.*sin(th5 - th6) + dth1.^2.*l1.*l5.*m6.*sin(th1 - th5) + dth2.^2.*l2.*l5.*m6.*sin(th2 - th5) + dth3.^2.*l3.*l5.*m6.*sin(th3 - th5) + dth4.^2.*l4.*l5.*m6.*sin(th4 - th5);
f(6,:) = d6.*m6.*(dth1.^2.*l1.*sin(th1 - th6) - g.*cos(th6) + dth2.^2.*l2.*sin(th2 - th6) + dth3.^2.*l3.*sin(th3 - th6) + dth4.^2.*l4.*sin(th4 - th6) + dth5.^2.*l5.*sin(th5 - th6));

for i=1:nTime
    MM = M(:,:,i);  ff = f(:,i);
    dz(7:12,i) = -MM \ ff;
end

end


function digit = r8_digit ( x, idigit )

%*****************************************************************************80
%
%% R8_DIGIT returns a particular decimal digit of an R8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the number whose NDIG-th decimal digit
%    is desired.  If X is zero, all digits will be returned as 0.
%
%    Input, integer IDIGIT, the position of the desired decimal digit.
%    A value of 1 means the leading digit, a value of 2 the second digit
%    and so on.
%
%    Output, integer DIGIT, the value of the IDIGIT-th decimal digit of X.
%
  if ( x == 0.0 )
    digit = 0;
    return
  end

  if ( idigit <= 0 )
    digit = 0;
    return
  end
%
%  Force X to lie between 1 and 10.
%
  x = abs ( x );

  while ( x < 1.0 )
    x = x * 10.0;
  end

  while ( 10.0 <= x )
    x = x / 10.0;
  end

  for i = 1 : idigit
    ival = floor ( x );
    x = ( x - ival ) * 10.0;
  end

  digit = ival;

  return
end


function srad = solarradiation(dem,lat,cs,r)
% PUPROSE: Calculate solar radiation for a digital elevation model (DEM)
%          over one year for clear sky conditions in W/m2
% -------------------------------------------------------------------
% USAGE: srad = solarradiation(dem,lat,cs)
% where: dem is the DEM to calculate hillshade for
%        lat is the latitude vector for the DEM - same size as size(dem,2)
%        cs is the cellsize in meters
%        r is the ground reflectance (global value or map, default is 0.2)
%
%       srad is the solar radiation in W/m2 over one year per grid cell
%
% EXAMPLE:
%       srad = solarradiation(peaks(50)*100,54.9:-0.1:50,1000,0.2);
%       - calculates the solar radiation for an example 50x50 peak surface.
%
% See also: GRADIENT, CART2POL
%
% Note: Follows the approach of Kumar et al 1997. Calculates clear sky
%       radiation corrected for the incident angle (selfshading) plus
%       diffuse and reflected radiation. Insolation is depending on time of year (and day),
%       latitude, elevation, slope and aspect.
%       Relief shading is not considered.
%       Script uses simple unweighed gradient of 4 nearest neighbours for slope
%       calculation.
%
% Reference: Kumar, L, Skidmore AK and Knowles E 1997: Modelling topographic variation in solar radiation in
%            a GIS environment. Int.J.Geogr.Info.Sys. 11(5), 475-497
%
%
% Felix Hebeler, Dept. of Geography, University Zurich, May 2008.

%% parameters
%It ;               % total hours of daily sunshine (calculated inline)
%M ;                % air mass ratio parameter (calculated inline)
%r = 0.20;          % ground reflectance coefficient (more sensible to give as input)
%L=lat;             %latitude
n = 1;              % timestep of calculation over sunshine hours: 1=hourly, 0.5=30min, 2=2hours etc
tau_a    = 365;     %length of the year in days
S0 = 1367;          % solar constant W m^-2   default 1367

dr= 0.0174532925;   % degree to radians conversion factor

%%  convert factors
[slop,asp]=get_ders(dem,cs);   % calculate slope and aspect in radians using given cellsize cs
[dummy,L]=meshgrid(1:size(dem,2),lat);   % grid latitude
clear dummy;
L=L*dr;                     % convert to radians
fcirc = 360*dr; % 360 degrees in radians

%% some setup calculations
srad=0;
sinL=sin(L);
cosL=cos(L);
tanL=tan(L);
sinSlop=sin(slop);
cosSlop=cos(slop);
cosSlop2=cosSlop.*cosSlop;
sinSlop2=sinSlop.*sinSlop;
sinAsp=sin(asp);
cosAsp=cos(asp);
term1 = ( sinL.*cosSlop - cosL.*sinSlop.*cosAsp);
term2 = ( cosL.*cosSlop + sinL.*sinSlop.*cosAsp);
term3 = sinSlop.*sinAsp;
%% loop over year
for d = 1:tau_a;
    %display(['Calculating melt for day ',num2str(d)])
    % clear sky solar radiation
    I0 = S0 * (1 + 0.0344*cos(fcirc*d/tau_a)); % extraterr rad per day
    % sun declination dS
    dS = 23.45 * dr* sin(fcirc * ( (284+d)/tau_a ) ); %in radians, correct/verified
    % angle at sunrise/sunset
    % t = 1:It; % sun hour
    hsr = real(acos(-tanL*tan(dS)));  % angle at sunrise
    % this only works for latitudes up to 66.5 deg N! Workaround:
    % hsr(hsr<-1)=acos(-1);
    % hsr(hsr>1)=acos(1);
    It=round(12*(1+mean(hsr(:))/pi)-12*(1-mean(hsr(:))/pi)); % calc daylength
%%  daily loop
    I=0;
    for t=1:n:It % loop over sunshine hours
        % if accounting for shading should be included, calc hillshade here
        % hourangle of sun hs
        hs=hsr-(pi*t/It);               % hs(t)
        %solar angle and azimuth
        %alpha = asin(sinL*sin(dS)+cosL*cos(dS)*cos(hs));% solar altitude angle
        sinAlpha = sinL.*sin(dS)+cosL.*cos(dS).*cos(hs);
        %alpha_s = asin(cos(dS)*sin(hs)/cos(alpha)); % solar azimuth angle
        % correction  using atmospheric transmissivity taub_b
        M=sqrt(1229+((614.*sinAlpha)).^2)-614.*sinAlpha; % Air mass ratio
        tau_b = 0.56 * (exp(-0.65*M) + exp(-0.095*M));
        tau_d = 0.271-0.294*tau_b; % radiation diffusion coefficient for diffuse insolation
        tau_r = 0.271+0.706*tau_b; % reflectance transmitivity
        % correct for local incident angle
        cos_i = (sin(dS).*term1) + (cos(dS).*cos(hs).*term2) + (cos(dS).*term3.*sin(hs));
        Is = I0 * tau_b; % potential incoming shortwave radiation at surface normal (equator)
        % R = potential clear sky solar radiation W m2
        R = Is .* cos_i;
        R(R<0)=0;  % kick out negative values
        Id = I0 .* tau_d .* cosSlop2./ 2.*sinAlpha; %diffuse radiation;
        Ir = I0 .* r .* tau_r .* sinSlop2./ 2.* sinAlpha; % reflectance
        R= R + Id + Ir;
        R(R<0)=0;
        I=I+R;% solar radiation per day (sunshine hours)
     end % end of sun hours in day loop
%%  add up radiation part melt for every day
    srad = srad + I;
end   % end of days in year loop

%%
function [grad,asp] = get_ders(dem,cs)
% calculate slope and aspect (deg) using GRADIENT function
[fx,fy] = gradient(dem,cs,cs); % uses simple, unweighted gradient of immediate neighbours
[asp,grad]=cart2pol(fy,fx); % convert to carthesian coordinates
grad=atan(grad); %steepest slope
asp=asp.*-1+pi; % convert asp 0 facing south


function J = evalObjectiveFCN(u,x,xref,Q,R,Ru)
% [J,Js,Ju]
%% Cost function of nonlinear MPC for Lotka-Volterra system
%
% Inputs:
%   u:      optimization variable, from time k to time k+N-1
%   x:      current state at time k
%   Ts:     controller sample time
%   N:      prediction horizon
%   xref:   state references, constant from time k+1 to k+N
%   u0:     previous controller output at time k-1
%
% Output:
%   J:      objective function cost
%

%% Nonlinear MPC design parameters

%% Cost Calculation
% Set initial cost and input
N = size(x,2);
J = zeros(N,1);
u0 = 0;

% Loop through each time step
for ct=1:N

    % Accumulate state tracking cost from x(k+1) to x(k+N)
    J(ct) = (x(:,ct)-xref(:,ct))'*Q*(x(:,ct)-xref(:,ct));

    % Accumulate MV rate of change cost from u(k) to u(k+N-1)
    if ct==1
        J(ct) = J(ct) + (u(:,ct)-u0)'*R*(u(:,ct)-u0) + u(:,ct)'*Ru*u(:,ct);
    else
        J(ct) = J(ct) + (u(:,ct)-u(:,ct-1))'*R*(u(:,ct)-u(:,ct-1)) + u(:,ct)'*Ru*u(:,ct);
    end
end


function desired_state = desiredState(traj_obj, t)
%desiredState computes desired position (x,y,z), yaw and its derivatives
% INPUT
%

tau_vec = traj_obj.tau_vec';
P = traj_obj.P;
path = traj_obj.path;

% Check dimensions
if size(path,2) ~= size(P,2)
    path = path';
end

cumsum_tau_vec = cumsum(tau_vec);
ts = [0; cumsum_tau_vec(:)];
D = size(P,2);

% Declare variables
pos = zeros(1,D);
vel = zeros(1,D);
acc = zeros(1,D);
jerk = zeros(1,D);
snap = zeros(1,D);

% If time is out of range, return the end point of the path
if t < 0
    error('time has to be greater than zero.')
elseif t >= sum(tau_vec)
    pos = path(end,:);
else
    k = find(ts<=t);
    k = k(end);
    for m = 1:D
        tau = t-ts(k);
        pos(m) = [tau^9,tau^8,tau^7,tau^6,tau^5,tau^4,tau^3,tau^2,tau,1] * P(10*k:-1:10*(k-1)+1,m);
        vel(m) = [9*tau^8,8*tau^7,7*tau^6,6*tau^5,5*tau^4,4*tau^3,3*tau^2,2*tau,1,0] * P(10*k:-1:10*(k-1)+1,m);
        acc(m) = [72*tau^7,56*tau^6,42*tau^5,30*tau^4,20*tau^3,12*tau^2,6*tau,2,0,0] * P(10*k:-1:10*(k-1)+1,m);
        jerk(m) = [504*tau^6,336*tau^5,210*tau^4,120*tau^3,60*tau^2,24*tau,6,0,0,0] * P(10*k:-1:10*(k-1)+1,m);
        snap(m) = [3024*tau^5,1680*tau^4,840*tau^3,360*tau^2,120*tau,24,0,0,0,0] * P(10*k:-1:10*(k-1)+1,m);
%         pos(m) = polyval(P(10*k:-1:10*(k-1)+1,m),t-ts(k));
%         vel(m) = polyval(polyder(P(10*k:-1:10*(k-1)+1,m)),t-ts(k));
%         acc(m) = polyval(polyder(polyder(P(10*k:-1:10*(k-1)+1,m))),t-ts(k));
%         jerk(m) = polyval(polyder(polyder(polyder(P(10*k:-1:10*(k-1)+1,m)))),t-ts(k));
%         snap(m) = polyval(polyder(polyder(polyder(polyder(P(10*k:-1:10*(k-1)+1,m))))),t-ts(k));
    end
end

% Desired yaw and its derivatives are all zero.
yaw = 0;
yawdot = 0;
yawddot = 0;

desired_state.pos = pos(:);
desired_state.vel = vel(:);
desired_state.acc = acc(:);
desired_state.jerk = jerk(:);
desired_state.snap = snap(:);
desired_state.yaw = yaw;
desired_state.yawdot = yawdot;
desired_state.yawddot = yawddot;


function p = eval_pdf_clg(X,Y,mu,Sigma,W)
% function p = eval_pdf_clg(X,Y,mu,Sigma,W)
%
% p(c,t) = N(Y(:,t); mu(:,c) + W(:,:,c)*X(:,t), Sigma(:,:,c))

[d T] = size(Y);
[d nc] = size(mu);
p = zeros(nc,T);
for c=1:nc
  denom = (2*pi)^(d/2)*sqrt(abs(det(Sigma(:,:,c))));
  M = repmat(mu(:,c), 1, T) + W(:,:,c)*X;
  mahal = sum(((Y-M)'*inv(Sigma(:,:,c))).*(Y-M)',2);
  p(c,:) = (exp(-0.5*mahal) / denom)';
end


function [ t, w ] = u_quadrature_rule ( n )

%*****************************************************************************80
%
%% U_QUADRATURE_RULE: quadrature rule for U(n,x).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the rule.
%
%    Output, real T(N,1), W(N,1), the points and weights of the rule.
%
  aj = zeros ( n, 1 );

  bj = 0.5 * ones ( n, 1 );

  w = zeros ( n, 1 );
  w(1,1) = sqrt ( pi / 2.0 );

  [ t, w ] = imtqlx ( n, aj, bj, w );

  w(1:n,1) = w(1:n,1).^2;

  return
end


function value = box_contains_segment_nd ( dim_num, p1, p2, pa, pb  )

%*****************************************************************************80
%
%% BOX_CONTAINS_SEGMENT_ND reports if a box contains a line segment in ND.
%
%  Discussion:
%
%    A box is assumed to be a rectangle with sides aligned on coordinate
%    axes.  It can be described by its low and high corner, P1 and P2:
%
%      points P so that P1(1:DIM_NUM) <= P(1:DIM_NUM) <= P2(1:DIM_NUM).
%
%    A line segment is the finite portion of a line that lies between
%    two points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 March 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real P1(DIM_NUM), P2(DIM_NUM), the low and high corners of the box.
%
%    Input, real PA(DIM_NUM), PB(DIM_NUM), the endpoints of the line segment.
%
%    Output, logical VALUE, is TRUE if the box contains
%    the line segment.
%
  value = 0;

  if ( ~box_contains_point_nd ( dim_num, p1, p2, pa ) )
    return
  end

  if ( ~box_contains_point_nd ( dim_num, p1, p2, pb ) )
    return
  end

  value = 1;

  return
end


%% Canny Edge Detection
% This sample demonstrates Canny edge detection.
%
% In this demo, we show how to use the OpenCV function |cv.Canny| to implement
% the Canny Edge Detector.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/edge.cpp>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/python/edge.py>
% * <https://docs.opencv.org/3.2.0/da/d5c/tutorial_canny_detector.html>
% * <https://docs.opencv.org/3.2.0/da/d22/tutorial_py_canny.html>
% * <https://github.com/opencv/opencv/blob/3.2.0/samples/cpp/tutorial_code/ImgTrans/CannyDetector_Demo.cpp>
%

%% Theory
%
% The _Canny Edge detector_ was developed by John F. Canny in 1986. Also known
% to many as the _optimal detector_, the Canny algorithm aims to satisfy three
% main criteria:
%
% * *Low error rate:* Meaning a good detection of only existent edges.
% * *Good localization:* The distance between edge pixels detected and real
%   edge pixels have to be minimized.
% * *Minimal response:* Only one detector response per edge.
%
%% Steps
%
% 1) Filter out any noise. The Gaussian filter is used for this purpose. An
%    example of a Gaussian kernel of $size = 5$ that might be used is shown
%    below:
%
% $$K = \frac{1}{159}
%       \left[{\matrix{
%           2 &  4 &  5 &  4 & 2 \cr
%           4 &  9 & 12 &  9 & 4 \cr
%           5 & 12 & 15 & 12 & 5 \cr
%           4 &  9 & 12 &  9 & 4 \cr
%           2 &  4 &  5 &  4 & 2
%       }}\right]$$
%
% 2) Find the intensity gradient of the image. For this, we follow a procedure
%    analogous to Sobel:
%
% * Apply a pair of convolution masks (in $x$ and $y$ directions):
%
% $$G_{x} = \left[{\matrix{
%               -1 & 0 & +1 \cr
%               -2 & 0 & +2 \cr
%               -1 & 0 & +1
%           }}\right]$$
%
% $$G_{y} = \left[{\matrix{
%               -1 & -2 & -1 \cr
%                0 &  0 &  0 \cr
%               +1 & +2 & +1
%           }}\right]$$
%
% * Find the gradient strength and direction. The direction is rounded to
%   one of four possible angles (namely 0, 45, 90 or 135)
%
% $$G = \sqrt{ G_{x}^{2} + G_{y}^{2} }$$
%
% $$\theta = \arctan(\frac{ G_{y} }{ G_{x} })$$
%
% 3) _Non-maximum suppression_ is applied. This removes pixels that are not
%    considered to be part of an edge. Hence, only thin lines (candidate edges)
%    will remain.
%
% 4) _Hysteresis_: The final step. Canny does use two thresholds
%    (_upper_ and _lower_). Canny recommended a |upper:lower| ratio between
%    2:1 and 3:1.
%
% * If a pixel gradient is higher than the _upper_ threshold, the pixel is
%   accepted as an edge
% * If a pixel gradient value is below the _lower_ threshold, then it is
%   rejected.
% * If the pixel gradient is between the two thresholds, then it will be
%   accepted only if it is connected to a pixel that is above the _upper_
%   threshold.
%
% For more details, you can always consult your favorite Computer Vision book.
%

%% Code
%
% This program:
%
% * Asks the user to enter a numerical value to set the lower threshold for
%   our _Canny Edge Detector_ (by means of a slider)
% * Applies the _Canny Detector_ and generates a *mask* (bright lines
%   representing the edges on a black background)
% * Applies the mask obtained on the original image and display it in a window
%

function varargout = edge_demo_gui(im)
    % load source image
    if nargin < 1
        im = fullfile(mexopencv.root(),'test','fruits.jpg');
        src = cv.imread(im, 'Color',true);
    elseif ischar(im)
        src = cv.imread(im, 'Color',true);
    else
        src = im;
    end

    % create the UI
    h = buildGUI(src);
    if nargout > 0, varargout{1} = h; end
end

function onChange(~,~,h)
    %ONCHANGE  Event handler for UI controls

    % retrieve current values from UI controls
    apertures = [3, 5, 7];
    aIdx = get(h.pop, 'Value');
    thresh = round(get(h.slid, 'Value'));
    set(h.txt, 'String',sprintf('Threshold: %3d',thresh));

    % convert image to grayscale, and blur to reduce the noise
    gray = cv.cvtColor(h.src, 'RGB2GRAY');
    gray = cv.blur(gray, 'KSize',[3 3]);

    % detect edges, with 3:1 as threshold ratio
    if true
        % default canny (Sobel gradient)
        edges = cv.Canny(gray, thresh*[1 3], 'ApertureSize',apertures(aIdx));
    else
        % canny with custom gradient (Scharr)
        dx = cv.Scharr(gray, 'DDepth','int16', 'XOrder',1, 'YOrder',0);
        dy = cv.Scharr(gray, 'DDepth','int16', 'XOrder',0, 'YOrder',1);
        edges = cv.Canny2(dx, dy, thresh*[1 3]);
    end

    % apply edges mask on original image
    if true
        out = cv.copyTo(h.src, 'Mask',edges);
    else
        out = bsxfun(@times, h.src, uint8(edges~=0));
    end

    % show result
    set(h.img, 'CData',out);
    drawnow;
end

function h = buildGUI(img)
    %BUILDGUI  Creates the UI

    % parameters
    thresh = 10;
    max_thresh = 150;
    sz = size(img);
    sz(2) = max(sz(2), 300);  % minimum figure width

    % build the user interface (no resizing to keep it simple)
    h = struct();
    h.src = img;
    h.fig = figure('Name','Edge map', ...
        'NumberTitle','off', 'Menubar','none', 'Resize','off', ...
        'Position',[200 200 sz(2) sz(1)+29]);
    if ~mexopencv.isOctave()
        %HACK: not implemented in Octave
        movegui(h.fig, 'center');
    end
    h.ax = axes('Parent',h.fig, ...
        'Units','pixels', 'Position',[1 30 sz(2) sz(1)]);
    if ~mexopencv.isOctave()
        h.img = imshow(img, 'Parent',h.ax);
    else
        %HACK: https://savannah.gnu.org/bugs/index.php?45473
        axes(h.ax);
        h.img = imshow(img);
    end
    uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[5 5 65 20], 'String','Aperture');
    h.pop = uicontrol('Parent',h.fig, 'Style','popupmenu', 'Value',1, ...
       'Position',[70 5 40 20], 'String',{'3','5','7'});
    h.txt = uicontrol('Parent',h.fig, 'Style','text', 'FontSize',11, ...
        'Position',[110 5 120 20], 'String',sprintf('Threshold: %3d',thresh));
    h.slid = uicontrol('Parent',h.fig, 'Style','slider', 'Value',thresh, ...
        'Min',0, 'Max',max_thresh, 'SliderStep',[1 10]./(max_thresh-0), ...
        'Position',[230 5 sz(2)-230-5 20]);

    % hook event handlers, and trigger default start
    set([h.pop, h.slid], 'Callback',{@onChange,h}, ...
        'Interruptible','off', 'BusyAction','cancel');
    onChange([],[],h);
end


classdef TP5 < PROBLEM
% <multi> <real> <large/none> <robust>
% Test problem for robust multi-objective optimization
% delta --- 0.05 --- Maximum disturbance degree
% H     ---   50 --- Number of disturbances

%------------------------------- Reference --------------------------------
% A. Gaspar-Cunha, J. Ferreira, and G. Recio, Evolutionary robustness
% analysis for multi-objective optimization: benchmark problems, Structural
% and Multidisciplinary Optimization, 2014, 49: 771-793.
%------------------------------- 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
        delta;      % Maximum disturbance degree
        H;          % Number of disturbances
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            [obj.delta,obj.H] = obj.ParameterSet(0.05,50);
            obj.M = 2;
            if isempty(obj.D); obj.D = 10; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            PopObj(:,1) = PopDec(:,1);
            g = 1 + 10*mean(PopDec(:,2:end),2);
            h = sin(4*pi*PopDec(:,1))/15 - PopDec(:,1) + 1;
            PopObj(:,2) = h.*g;
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(~,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = sin(4*pi*R(:,1))/15 - R(:,1) + 1;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            switch metName
                case {'Mean_IGD','Mean_HV','Worst_IGD','Worst_HV'}
                    score = feval(metName,Population,obj);
                otherwise
                    score = feval(metName,Population,obj.optimum);
            end
        end
        %% Perturb solutions multiple times
        function PopX = Perturb(obj,PopDec,N)
            if nargin < 3; N = obj.H; end
            Delta = repmat(obj.delta.*(obj.upper-obj.lower),N*size(PopDec,1),1);
            w     = UniformPoint(N,obj.D,'Latin');
            Dec   = 2*Delta.*w(reshape(repmat(1:end,size(PopDec,1),1),1,[]),:) + repmat(PopDec,N,1) - Delta;
            Dec   = obj.CalDec(Dec);
            PopX  = SOLUTION(Dec,obj.CalObj(Dec),obj.CalCon(Dec));
        end
    end
end


% Fig. 5.38  Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
clf
numG = 160*conv ([1 2.5],[1 0.7]);
denG = conv([1 5 40],[1  .03  .06]);
sysG = tf(numG,denG);
sysD=tf([1   3],[1   20]);
sysDG=sysD*sysG;
K = 0.3;
sysH=tf(1,1);
sysT = feedback (K*sysG,sysH);
sysTD=feedback(sysDG,sysH);
step(sysT)
hold on
step(sysTD)
grid on
title('Step responses of auto-pilot with P and lead control')
hold off


clear; clc; close all;
dataset = 'grid';
img = imread(strcat('../noncvx-lowrank(ICDM 2015)/data/images/', dataset, '.jpg'));

if(size(img, 2) > size(img, 1))
    img = permute(img, [2,1,3]);
end

imgSize = size(img);

img = double(img)/255;
imgMean = mean(img(:));
img = img - imgMean;
img = img/std(img(:));

img = reshape(img, imgSize(1), prod(imgSize)/imgSize(1));

missRatio = 0.95;
noisRatio = 0.05;

mask = (rand(size(img)) > missRatio);
G = randn(size(img))*noisRatio;
traD = img + G;
traD = traD.*mask;
traD = sparse(traD);

clear maxk missRatio noisRatio mask G;

para.maxIter = 5000;
para.tol = 1e-6;
para.maxR = 50 ;

%% ---------------------------------------------------------------
mask = (rand(size(img)) > 0.9);
tstD = img.*mask;
tstD = sparse(tstD);

[tstRow, tstCol, tstVal] = find(tstD);

para.test.row  = tstRow;
para.test.col  = tstCol;
para.test.data = tstVal;
para.test.m = size(tstD, 1);
para.test.n = size(tstD, 2);

clear tstRow tstCol tstVal tstD;

%% ---------------------------------------------------------------
lambdaMax = 4;
gridLambda = lambdaMax*(0.8).^(0:9);

gridNMSE = zeros(2, size(gridLambda,2 ));
gridRank = zeros(1, size(gridLambda,2 ));
for g = 1:length(gridLambda)
    lambda = gridLambda(g);

    % [U, S, V] = SoftImpute(traD, lambda, para );
    [U, S, V] = AISImpute(traD, lambda, para );
    gridRank(g) = nnz(S);

    X = U*S*V';
    gridNMSE(1, g) = sqrt(norm(X - img, 'fro')^2/numel(img));

    [ U, S, V ] = PostProcess(traD, U, V, S);
    X = U*S*V';

    gridNMSE(2, g) = sqrt(norm(X - img, 'fro')^2/numel(img));

    if(g > 1 && gridNMSE(2, g) > gridNMSE(2, g - 1))
        break;
    end
end

gridNMSE = gridNMSE(2,1:g);
[~, lambda] = min(gridNMSE);
gndRank = gridRank(lambda);
para.maxR = ceil(gndRank*1.2);

lambda = gridLambda(lambda);

clear gridNMSE gridRank g X U S V gridLambda;

%% active --------------------------------------------------------
method = 1;
t = tic;
[U, S, V, out{method}] = ActiveSubspace(traD, lambda, para );
% [U, S, V, out{1}]=mc_alt(traData', lambda, para);
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
hold on;
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
hold on;
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('active');

%% boost ---------------------------------------------------------
method = 2;
t = tic;
[U, S, V, out{method}] = Boost( traD, lambda, para);
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
hold on;
plot(out{method}.Time, out{method}.RMSE);
figure(2);
hold on;
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('boost');

%% TR ------------------------------------------------------------
method = 3;
t = tic;
[U, S, V, out{method}] = MMBS( traD, lambda, para );
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('TR');

%% ALT-Impute ----------------------------------------------------
method = 4;
t = tic;
[U, S, V, out{method}] = SoftImputeALS( traD, lambda, para.maxR, para );
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('ALT-Impute');

%% SSGD ----------------------------------------------------------
method = 5;
t = tic;
[U, S, V, out{method}] = SSGD( traD, lambda, gndRank, para );
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('SSGD');

%% LMaFit --------------------------------------------------------

gridNMSE = zeros(1, 10);
for g = 1:10
    [U, S, V] = FixedRank( traD, g, para );

    X = U*S*V';
    gridNMSE(g) = sqrt(norm(X - img, 'fro')^2/numel(img));

    if(g > 1 && gridNMSE(g) > gridNMSE(g - 1))
        break;
    end
end

gridNMSE = gridNMSE(1:g);
[~, rnk] = min(gridNMSE);

clear gridNMSE X g U S V;

method = 6;
t = tic;
[U, S, V, out{method}] = FixedRank( traD, rnk, para );
Time(method) = toc(t);

X = U*S*V';
RMSE(method) = out{method}.RMSE(end);
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('LMaFit');

%% APG -----------------------------------------------------------
method = 7;
t = tic;
[U, S, V, out{method}] = APGMatComp( traD, lambda, para );
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('APG');

%% Soft-Impute ---------------------------------------------------
method = 8;
t = tic;
[U, S, V, out{method}] = SoftImpute( traD, lambda, para );
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('Soft-Impute');

%% AIS-Impute ----------------------------------------------------
method = 9;
t = tic;
[U, S, V, out{method}] = AISImpute( traD, lambda, para );
Time(method) = toc(t);

[ U, S, V ] = PostProcess(traD, U, V, S);
X = U*S*V';
RMSE(method) = sqrt(norm(X - img, 'fro')^2/numel(img));
clear U S V t;

figure(1);
plot(out{method}.Time, out{method}.RMSE);
figure(2);
semilogy(out{method}.Time, out{method}.obj - min(out{method}.obj));
figure;
X = X + imgMean;
X = reshape(X, imgSize);
imshow(X, []);
title('AIS-Impute');

% clear gndRank img imgMean lambda lambdaMax method rnk traD X mask;


function [rs, rs2, rs3] = dimensions(s, bins)

%tstoolbox/@signal/dimensions
%   Syntax:
%     * [bc,in,co] = dimensions(s, bins)
%
%   Input arguments:
%     * s - data points (row vectors)
%     * bins - maximal number of partition per axis, default is 100
%
%   Output arguments:
%     * bc - scaling of boxes with partititon sizes (log[2]-log[2])
%     * in - scaling of information with partititon sizes (log[2]-log[2])
%     * co - scaling of correlation with partititon sizes (log[2]-log[2])
%
%   Compute boxcounting, information and correlation dimension of a
%   time-delay reconstructed timeseries s for dimensions from 1 to D,
%   where D is the dimension of the input vectors using boxcounting
%   approach.
%
%   Scale data to be within 0 and 1. Give a sortiment of (integer)
%   partitionsizes with almost exponential behaviour.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

narginchk(1,2);

if ndim(s) ~= 2
    error('Signal must contain vector data');
end

if nargin<2
    bins = 100;
end

points = data(s);
[N,dim] = size(points);

% scale data to be within 0 and 1
points = points - min(min(points));
points = points / max(max(points));

% give a sortiment of (integer) partitionsizes with almost exponential behaviour
par = [2 3 4 5 6 7 8 10 12 14 16 20 23 27 32 39 46 54 64 77 91 108 128 153 182 216 256 ...
      305 363 431 512 609 725 862 1024 1218 1449 1723 2048 2436 2897 3445 4096 4871 ...
	  5793 6889 8192 9742 11586 13778 16384 19484 23171 27555 32768 38968 46341 55109 65536];

partitions = par(find(par<=bins));	% use no sizes greater than bins

[c,d,e] = boxcount(points, partitions);

c = [zeros(1,dim) ; c];             % add zeros for partition size 1
d = [zeros(1,dim) ; d];             % add zeros for partition size 1
e = [zeros(1,dim) ; e];             % add zeros for partition size 1
partitions = [1 ; partitions(:)];

a1 = achse(-log2(partitions));     		% create axis with arbitrary spacing
a1 = setname(a1, 'ld r');

a2 = setname(achse(unit, 1, 1), 'Embedding dimension');

rs = signal(core(c), s);
rs = setaxis(rs, 1, a1);
rs = setaxis(rs, 2, a2);
rs = setplothint(rs, 'multigraph');
rs = addhistory(rs,  ['Computed boxcounting dimension']);
rs = addcommandlines(rs, 's = boxdim(s', bins);
rs = setyname(rs, 'ld N(r)');
rs = setlabel(rs, 'Scaling of D0');

rs2 = signal(core(d), s);
rs2 = setaxis(rs2, 1, a1);
rs2 = setaxis(rs2, 2, a2);
rs2 = setplothint(rs2, 'multigraph');
rs2 = addhistory(rs2,  ['Computed information dimension']);
rs2 = addcommandlines(rs2, 's = infodim(s', bins);
rs2 = setyname(rs2, 'I(r)');
rs2 = setlabel(rs2, 'Scaling of D1');

rs3 = signal(core(e), s);
rs3 = setaxis(rs3, 1, a1);
rs3 = setaxis(rs3, 2, a2);
rs3 = setplothint(rs3, 'multigraph');
rs3 = addhistory(rs3,  ['Computed correlation dimension']);
rs3 = addcommandlines(rs3, 's = corrdim(s', bins);
rs3 = setyname(rs3, 'ld C(r)');
rs3 = setlabel(rs3, 'Scaling of D2');


% plot_UWB_channel.m
clear, clf
no_output_files = 0;  % non-zero: avoids writing output files of continuous-time responses
Ts = 0.167;        % sampling time (nsec)
num_ch=100; % number of channel impulse responses to generate
randn('state',12);  % initialize state of function for repeatability
rand('state',12);   % initialize state of function for repeatability
cm = 1;  % channel model number from 1 to 4
% get channel model params based on this channel model number
[Lam,lam,Gam,gam,nlos,sdi,sdc,sdr] = UWB_parameters(cm);
fprintf(1,['Model Parameters\n' ' Lam= %.4f, lam= %.4f, Gam= %.4f, gam= %.4f\n NLOS flag= %d, std_shdw= %.4f, std_ln_1= %.4f, std_ln_2= %.4f\n'],...
 Lam,lam,Gam,gam,nlos,sdi,sdc,sdr);
% get a bunch of realizations (impulse responses)
[h_ct,t_ct,t0,np] = UWB_model_ct(Lam,lam,Gam,gam,num_ch,nlos,sdi,sdc,sdr);
% now reduce continuous-time result to a discrete-time result
[hN,N] = convert_UWB_ct(h_ct,t_ct,np,num_ch,Ts);
% if we wanted complex baseband model or to impose some filtering function,
% this would be a good place to do it
h = resample(hN,1,N);  % decimate the columns of hN by factor N
h = h*N;  % correct for 1/N scaling imposed by decimation
channel_energy = sum(abs(h).^2);  % channel energy
h_len = size(h,1);
t = [0:(h_len-1)]*Ts;  % for use in computing excess & RMS delays

for k=1:num_ch
  % determine excess delay and RMS delay
  sq_h = abs(h(:,k)).^2/channel_energy(k);
  t_norm = t - t0(k);  % remove the randomized arrival time of first cluster
  excess_delay(k) = t_norm*sq_h;
  rms_delay(k) = sqrt((t_norm-excess_delay(k)).^2*sq_h);
  % determine # of significant paths (paths within 10 dB from peak)
  threshold_dB = -10;   % dB
  temp_h = abs(h(:,k));
  temp_thresh = 10^(threshold_dB/20)*max(temp_h);
  num_sig_paths(k) = sum(temp_h>temp_thresh);
  % determine number of sig. paths (captures x % of energy in channel)
  x = 0.85;
  temp_sort = sort(temp_h.^2);  % sorted in ascending order of energy
  cum_energy = cumsum(temp_sort(end:-1:1));  % cumulative energy
  index_e = min(find(cum_energy >= x*cum_energy(end)));
  num_sig_e_paths(k) = index_e;
end
energy_mean = mean(10*log10(channel_energy));
energy_stddev = std(10*log10(channel_energy));
mean_excess_delay = mean(excess_delay);
mean_rms_delay = mean(rms_delay);
mean_sig_paths = mean(num_sig_paths);
mean_sig_e_paths = mean(num_sig_e_paths);

fprintf(1,'Model Characteristics\n');
fprintf(1,'  Mean delays: excess (tau_m) = %.1f ns, RMS (tau_rms) = %1.f\n', ...
    mean_excess_delay, mean_rms_delay);
fprintf(1,'  # paths: NP_10dB =  %.1f, NP_85%% = %.1f\n', ...
    mean_sig_paths, mean_sig_e_paths);
fprintf(1,'  Channel energy: mean = %.1f dB, std deviation = %.1f dB\n', ...
  energy_mean, energy_stddev);

subplot(421), plot(t,h), grid on
title('Impulse response realizations'), xlabel('Time (nS)')

subplot(422), plot([1:num_ch], excess_delay, 'b-', ...
  [1 num_ch], mean_excess_delay*[1 1], 'r--' );
grid on, title('Excess delay (nS)'), xlabel('Channel number')

subplot(423), plot([1:num_ch], rms_delay, 'b-', ...
  [1 num_ch], mean_rms_delay*[1 1], 'r--' );
grid on, title('RMS delay (nS)'), xlabel('Channel number')

subplot(424), plot([1:num_ch], num_sig_paths, 'b-', ...
  [1 num_ch], mean_sig_paths*[1 1], 'r--');
grid on, title('Number of significant paths within 10 dB of peak')
xlabel('Channel number')

subplot(427), plot([1:num_ch], num_sig_e_paths, 'b-', ...
  [1 num_ch], mean_sig_e_paths*[1 1], 'r--');
grid on, title('Number of significant paths capturing > 85% energy')
xlabel('Channel number')

temp_average_power = sum(h'.*(h)')/num_ch;
temp_average_power = temp_average_power/max(temp_average_power);
average_decay_profile_dB = 10*log10(temp_average_power);
subplot(425), plot(t,average_decay_profile_dB); grid on
axis([0 t(end) -60 0]), title('Average Power Decay Profile')
xlabel('Delay (nsec)'), ylabel('Average power (dB)')

subplot(426)
figh = plot([1:num_ch],10*log10(channel_energy),'b-', ...
  [1 num_ch], energy_mean*[1 1], 'g--', ...
  [1 num_ch], energy_mean+energy_stddev*[1 1], 'r:', ...
  [1 num_ch], energy_mean-energy_stddev*[1 1], 'r:');
xlabel('Channel number'), ylabel('dB'), title('Channel Energy');
legend(figh, 'Per-channel energy', 'Mean', '\pm Std. deviation', 0)

if no_output_files, return;  end
%%% save continuous-time (time,value) pairs to files
save_fn = sprintf('cm%d_imr', cm);
% A complete self-contained file for Matlab users
save([save_fn '.mat'], 't_ct', 'h_ct', 't0', 'np', 'num_ch', 'cm');
% Two comma-delimited text files for non-Matlab users:
% File #1: cmX_imr_np.csv lisTs the number of paths in each realization
dlmwrite([save_fn '_np.csv'], np, ',');  % number of paths
% File #2: cmX_imr.csv can open with Excel
%     n'th pair of columns contains the (time,value) pairs for the n'th realization
th_ct = zeros(size(t_ct,1),2*size(t_ct,2));
th_ct(:,1:2:end) = t_ct;  % odd columns are time
th_ct(:,2:2:end) = h_ct; % even columns are values
fid = fopen([save_fn '.csv'], 'w');
if fid < 0,
  error('unable to write .csv file for impulse response, file may be open in another application');
end
for k = 1:size(th_ct,1)
  fprintf(fid,'%.4f,%.6f,', th_ct(k,1:end));
  fprintf(fid,'\r\n'); % \r\n for Windoze end-of-line
end
fclose(fid);


% POLARCONT Polar contour plot
%
% Richard Rieber
% rrieber@gmail.com
% April 4, 2007
% Updated June 15, 2007
%
% function [C,h] = polarcont(r,theta,z,N,s)
%
% Purpose: This function creates polar contour plots on the current active
%          figure
%
% Inputs:  o r     - Radius vector of length m
%          o theta - Angle vector in radians of length n
%          o z     - Magnitude at the points specified in r and theta of
%                    size m x n
%          o N     - The number of contours to plot [OPTIONAL]
%          o s     - Linespec as described in PLOT [OPTIONAL]
%
% Outputs: o C     - returns contour matrix C as described in CONTOURC
%          o h     - Column vector H of handles to LINE or PATCH objects,
%                    one handle per line.
%
% OTHER NOTES:
% - Both C and h can be used as inputs to CLABEL
% - Colors are defined in colormap
% - Treat this function as a standard contour plot

function [C,h] = polarcont(r,theta,z,N,s)

[a,b] = size(z);

if a ~= length(r)
    error('r is not the same length as the first dimension of z')
end

if b ~= length(theta)
    error('theta is not the same length as the second dimension of z')
end

x = zeros(a,b);
y = zeros(a,b);

for j = 1:a
    for k = 1:b
        x(j,k) = r(j)*cos(theta(k));
        y(j,k) = r(j)*sin(theta(k));
    end
end

if nargin == 3
    [C,h] = contour(x,y,z);
elseif nargin == 4
    [C,h] = contour(x,y,z,N);
elseif nargin == 5
    [C,h] = contour(x,y,z,N,s);
else
    error('Incorrect number of inputs')
end


function X = tendiag(v,sz)
%TENDIAG Creates a tensor with v on the diagonal.
%
%   TENDIAG(V) creates a tensor with N dimensions, each of size N, where N
%   is the number of elements of V. The elements of V are placed on the
%   superdiagonal.
%
%   TENDIAG(V,SZ) is the same as above but creates a tensor of size SZ. If
%   SZ is not big enough, the tensor will be enlarged to accommodate the
%   elements of V on the superdiagonal.
%
%   Examples
%   X = tendiag([0.1 0.22 0.333]) %<-- creates a 3x3x3 tensor
%
%   See also TENSOR, SPTENDIAG.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

% Make sure v is a column vector
v = reshape(v,[numel(v) 1]);

N = numel(v);
if ~exist('sz','var')
    sz = repmat(N,1,N);
end

X = tenzeros(sz);
subs = repmat((1:N)', 1, length(sz));
X(subs) = v;


function [L,S,obj,err,iter] = rmsc(X,lambda,opts)

% Solve the Robust Multi-view Spectral Clustering (RMSC) problem by M-ADMM
%
% min_{L,S_i} ||L||_*+lambda*\sum_i ||S_i||_1,
% s.t. X_i=L+S_i, i=1,...,m, L>=0, L1=1.
% ---------------------------------------------
% Input:
%       X       -    d*n*m tensor
%       lambda  -    >0, parameter
%       opts    -    Structure value in Matlab. The fields are
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       L       -    d*n matrix
%       S       -    d*n*m tensor
%       obj     -    objective function value
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 19/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'tol');         tol = opts.tol;              end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

[d,n,m] = size(X);
L = zeros(d,n);
S = zeros(d,n,m);
Z = L;
Y = S;
dY = S;
Y2 = L;
iter = 0;
for iter = 1 : max_iter
    Lk = L;
    Sk = S;
    Zk = Z;
    % first super block {Z,S_i}
    [Z,nuclearnormZ] = prox_nuclear(L+Y2/mu,1/mu);
    for i = 1 : m
        S(:,:,i) = prox_l1(-L+X(:,:,i)-Y(:,:,i)/mu,lambda/mu);
    end
    % second super block {L}
    temp = (sum(X-S-Y/mu,3)+Z-Y2/mu)/(m+1);
    L = project_simplex(temp);

    for i = 1 : m
        dY(:,:,i) = L+S(:,:,i)-X(:,:,i);
    end
    dY2 = L-Z;
    chgL = max(abs(Lk(:)-L(:)));
    chgZ = max(abs(Zk(:)-Z(:)));
    chgS = max(abs(Sk(:)-S(:)));
    chg = max([chgL chgS chgZ max(abs(dY(:))) max(abs(dY2(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = nuclearnormZ+lambda*norm(S(:),1);
            err = sqrt(norm(dY(:))^2+norm(dY2,'fro')^2);
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y = Y + mu*dY;
    Y2 = Y2 + mu*dY2;
    mu = min(rho*mu,max_mu);
end
obj = nuclearnormZ+lambda*norm(S(:),1);
err = sqrt(norm(dY(:))^2+norm(dY2,'fro')^2);


function [ prob, ier ] = mdbeta ( x, p, q )

%*****************************************************************************80
%
%% MDBETA evaluates the incomplete beta function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2008
%
%  Author:
%
%    Original FORTRAN77 version by Oliver Ludwig;
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Oliver Ludwig,
%    Algorithm 179:
%    Incomplete Beta Ratio,
%    Communications of the ACM,
%    Volume 6, Number 6, June 1963, page 314.
%
%  Parameters:
%
%    Input, real X, the value to which function is to be
%    integrated.  X must be in the range [0,1] inclusive.
%
%    Input, real P, the first parameter.  P must be greater
%    than 0.0.
%
%    Input, real Q, the second parameter.  Q must be greater
%    than 0.0.
%
%    Output, real PROB.  The probability that a random variable
%    from a Beta distribution having parameters P and Q will be less than
%    or equal to X.
%
%    Output, integer IER, error parameter.
%    0, normal exit.
%    1, X is not in the range [0,1] inclusive.
%    2, P or Q is less than or equal to 0.
%
%  Local parameters:
%
%    Local, real ALEPS, the logarithm of EPS1.
%
%    Local, real EPS, the machine precision.
%
%    Local, real EPS1, the smallest representable number.
%
  aleps = - 179.6016;
  eps = 2.2E-16;
  eps1 = 1.0E-78;
%
%  Check ranges of the arguments.
%
  prob = 0.0;
  y = x;

  if ( x < 0.0 || 1.0 < x )
    ier = 1;
    return
  end

  if ( p <= 0.0 || q <= 0.0 )
    ier = 2;
    return
  end

  ier = 0;

  if ( x <= 0.5 )
    interval = 0;
  else
    interval = 1;
    temp = p;
    p = q;
    q = temp;
    y = 1.0 - y;
  end

  if ( x == 0.0 || x == 1.0 )

    prob = 0.0;

    if ( interval ~= 0 )
      prob = 1.0 - prob;
      temp = p;
      p = q;
      q = temp;
    end

    return
  end

  ib = q;
  temp = ib;
  ps = q - ib;

  if ( q == temp )
    ps = 1.0;
  end

  dp = p;
  dq = q;
  px = dp * log ( y );
  pq = alogam ( dp + dq );
  p1 = alogam ( dp );
  c = alogam ( dq );
  d4 = log ( dp );
  xb = px + alogam ( ps + dp ) - alogam ( ps ) - d4 - p1;
%
%  Scaling
%
  ib = floor ( xb / aleps );
  infsum = 0.0;
%
%  First term of a decreasing series will underflow.
%
  if ( ib == 0 )

    infsum = exp ( xb );
    cnt = infsum * dp;
%
%  CNT will equal dexp ( temp ) * ( 1.d0 - ps ) * i * p * y**i / factorial ( i ).
%
    wh = 0.0;

    while ( 1 )

      wh = wh + 1.0;
      cnt = cnt * ( wh - ps ) * y / wh;
      xb = cnt / ( dp + wh );
      infsum = infsum + xb;

      if ( xb / eps < infsum )
        break
      end

    end

  end

  finsum = 0.0;

  if ( dq <= 1.0 )

    prob = finsum + infsum;

    if ( interval ~= 0 )
      prob = 1.0 - prob;
      temp = p;
      p = q;
      q = temp;
    end

    return
  end

  xb = px + dq * log ( 1.0 - y ) + pq - p1 - log ( dq ) - c;
%
%  Scaling.
%
  ib = floor ( xb / aleps );

  if ( ib < 0 )
    ib = 0;
  end

  c = 1.0 / ( 1.0 - y );
  cnt = exp ( xb - ib * aleps );
  ps = dq;
  wh = dq;

  while ( 1 )

    wh = wh - 1.0;

    if ( wh <= 0.0 )

      prob = finsum + infsum;

      if ( interval ~= 0 )
        prob = 1.0 - prob;
        temp = p;
        p = q;
        q = temp;
      end

      break

    end

    px = ( ps * c ) / ( dp + wh );

    if ( px <= 1.0 )

      if ( cnt / eps <= finsum || cnt <= eps1 / px )

        prob = finsum + infsum;

        if ( interval ~= 0 )
          prob = 1.0 - prob;
          temp = p;
          p = q;
          q = temp;
        end

        break

      end

    end

    cnt = cnt * px;
%
%  Rescale.
%
    if ( 1.0 < cnt )
      ib = ib - 1;
      cnt = cnt * eps1;
    end

    ps = wh;

    if ( ib == 0 )
      finsum = finsum + cnt;
    end

  end

  return
end


function z = proj_sdp(z,n)
if n==0
    return;
elseif n==1
    z = max(z,0);
    return;
end

% expand to full size matrix
b = tril(ones(n));
b(b == 1) = z;
z = b;
z = (z + z');
z = z - diag(diag(z)) / 2;

% rescale so projection works, and matrix norm preserved
% see http://www.seas.ucla.edu/~vandenbe/publications/mlbook.pdf pg 3
% scale diags by sqrt(2)
z(eye(n) == 1) = z(eye(n) == 1) .* sqrt(2);

[V,S] = eig(z);
S = diag(S);

idx = find(S>0);
V = V(:,idx);
S = S(idx);
z = V*diag(S)*V';

% scale diags by 1/sqrt(2)
z(eye(n) == 1) = z(eye(n) == 1) ./ sqrt(2);

z = z(tril(ones(n)) == 1);
end


function u=rotrack(u0,x,y,nu,T)
%
%  Solves the linear equation u_t + y u_x - x u_y = 0 by "front tracking" +
%  dimensional splitting, using a time step dictated by dt/dx=nu for each
%  substep.
%  Neumann boundary condition are implicit.
%
dx=x(2)-x(1);
dt=nu*dx;
nstep=ceil(T/dt);
dt=T/nstep;
nu=dt/dx;
u=u0;
for i=1:nstep,
	u=transp(u,nu*y,1);
	u=transp(u,-nu*x,2);
end;


%% Demo of gaimc - 'Graph Algorithms In Matlab Code'
% Matlab includes great algorithms to work with sparse matrices but does
% provide a reasonable set of algorithms to work with sparse matrices as
% graph data structures.  My other project -- MatlabBGL -- provides a
% high-performance solution to this problem by directly interfacing the
% Matlab sparse matrix data structure with the Boost Graph Library.
% That library, however, suffers from enormous complication because
% it must be compiled for each platform.  The Boost Graph Library
% heavily uses advanced C++ features that impair easy portability
% between platforms.  In contrast, the gaimc library is implemented in
% pure Matlab code, making it completely portable.
%
% The cost of the portability for this library is a 2-4x slowdown in
% the runtime of the algorithms as well as significantly fewer
% algorithms to choose from.

%% Sparse matrices as graphs
% To store the connectivity structure of the graph, gaimc uses the
% adjacency matrix of a graph.
%
% A graph is represented by a set of vertices and a set of
% edges between the vertices.  Often, we write $G = (V,E)$ to denote the
% graph, the set of vertices, and the set of edges, respectively.  In
% gaimc, like in my other package MatlabBGL, we represent graphs with their
% adjacency matrices.  This representation is handy in Matlab because
% Matlab is rather efficient at working with the large sparse matrices that
% typically arise as adjacency matrices.
%
% To convert from $G=(V,E)$ to an adjacency matrix, we identify each vertex
% with a row of the matrix via a bijective map.  The adjacency matrix is
% then a $|V| \times |V|$ matrix called A.  The entry A(i,j) = 1 for
% any edge between in $E$ and 0 otherwise.  Let's look at an example.

load_gaimc_graph('bfs_example');
graph_draw(A,xy,'labels',labels)
full(A)
labels'

%%
% This output means that vertex 'r' is row 1, vertex 's' is row 2 and
% because A(1,2) = 1, then there is an edge between them, just like in the
% picture.
%
% One funny property is that A(2,1) = 1 too!  So we actually have to store
% each edge twice in the adjacency matrix.  This might seem wasteful, but
% its hard to avoid as I've learned while working on graph algorithms.  So
% don't worry about it!  It also makes the generalization to directed
% graphs (below) easy.
%
% For more information about the adjacency matrix representation of a
% graph, see a standard book on graph algorithms.
%

%% Weighted and directed graphs
% Our previous case handled the situation for undirected graphs only.  To
% encode weighted and directed graphs, we use weighted and non-symmetric
% adjacency matrices.
%
% For a weighted matrix, A(i,j) = distance between i and j for most of the
% algorithms in gaimc.  But A(i,j) = 0 means there is no edge, and so
% sometimes things can get a little tricky to get what you want.
%
% For a directed graph, just set A(i,j) ~= A(j,i).  The adjacency matrix
% won't be symmetric, but that's what you want!
%
% To understand more, explore the examples or read up on adjacency matrices
% in graph theory books.

%% Loading helper
% To make loading our sample graphs easy, gaimc defines it's own function
% to load graphs.

load_gaimc_graph('dfs_example'); % loads one of our example graphs
whos

%%
% This helps make our examples work regardless of where the current
% directory lies.

%% Search algorithms
% The two standard graph search algorithms are depth first search and
% breadth first search.  This library implements both.

%%
% Load the example matrix from the Boost Graph Library
load_gaimc_graph('dfs_example');
figure(1); graph_draw(A,xy,'labels',labels);

%%
% Run a depth first search.  The output records the distance to the other
% vertices, except where the vertices are not reachable starting from the
% first node A.
d=dfs(A,1)

%%
% From this example, we see that vertices a-f are reachable from vertex a,
% but that verice g-i are not reachable.  Given the of the edges, this
% makes sense.

%%
% Let's look at breadth first search too, using a different example.
load_gaimc_graph('bfs_example');
figure(1); clf; graph_draw(A,xy,'labels',labels);

%%
% The breadth first search algorithm records the distance from the starting
% vertex to each vertex it visits in breadth first order.  This means it
% visits all the vertices in order of their distance from the starting
% vertex.  The d output records the distance, and the dt output records the
% step of the algorithm when the breadth first search saw the node.
[d dt] = bfs(A,2);
% draw the graph where the label is the "discovery time" of the vertex.
figure(1); clf; graph_draw(A,xy,'labels',num2str(dt));

%%
% Notice how the algorithm visits all vertices one edge away from the start
% vertex (0) before visiting those two edges away.

%% Shortest paths
% In the previous two examples, the distance between vertices was
% equivalent to the number of edges.  Some graphs, however, have specific
% weights, such as the graph of flights between airports.  We can use this
% information to build information about the _shortest path_ between two
% nodes in a network.

% Find the minimum travel time between Los Angeles (LAX) and
% Rochester Minnesota (RST).
load_gaimc_graph('airports')
A = -A; % fix funny encoding of airport data
lax=247; rst=355;

[d pred] = dijkstra(A,lax); % find all the shorest paths from Los Angeles.

fprintf('Minimum time: %g\n',d(rst));
% Print the path
fprintf('Path:\n');
path =[]; u = rst; while (u ~= lax) path=[u path]; u=pred(u); end
fprintf('%s',labels{lax});
for i=path; fprintf(' --> %s', labels{i}); end, fprintf('\n');

%% Minimum spanning trees
% A minimum spanning tree is a set of edges from a graph that ...
%
% This demo requires the mapping toolbox for maximum effect, but we'll do
% okay without it.

%%
% Our data comes from a graph Brendan Frey prepared for his affinity
% propagation clustering tool.  For 456 cities in the US, we have the mean
% travel time between airports in those cities, along with their latitude
% and longitude.
load_gaimc_graph('airports')

%%
% For some reason, the data is stored with the negative travel time between
% cities.  (I believe this is so that closer cities have larger edges
% between them.)  But for a minimum spanning tree, we want the actual
% travel time between cities.
A = -A;

%%
% Now, we just call MST and look at the result.
% T = mst_prim(A);
% This command means we can't run the demo, so it's commented out.

%%
% Oops, travel time isn't symmetric!  Let's just pick the longest possible
% time.
A = max(A,A');
T = mst_prim(A);
sum(sum(T))/2 % total travel time in tree

%%
% Well, the total weight isn't that helpful, let's _look_ at the data
% instead.
clf;
gplot(T,xy);

%%
% Hey!  That looks like the US!  You can see regional airports and get some
% sense of the overall connectivity.

%% Connected components
% The connected components of a network determine which parts of the
% network are reachable from other parts.  One of your first questions
% about any network should generally be: is it connected?
%
% There are two types of connected components: components and strongly
% connected components.  gaimc only implements an algorithm for the latter
% case, but that's okay!  It turns out it computes exactly the right thing
% for connected components as well.  The difference only occurs when the
% graph is undirected vs. directed.

load_gaimc_graph('dfs_example')
graph_draw(A,xy)

%%
% This picture shows there are 3 strongly connected components and 2
% connected components

% get the number of strongly connected components
max(scomponents(A))

%%

% get the number of connected components
max(scomponents(A|A'))  % we make the graph symmetric first by "or"ing each entry

%%
% Let's look at the vertices in the strongly connected components
cc = scomponents(A)
%%
% The output tells us that vertices 1,2,3,5,6 are in one strong component,
% vertex 4 is it's own strong component, and vertices 7,8,9 are in another
% one.  Remember that a strong component is all the vertices mutually
% reachable from a given vertex.  If you start at vertex 4, you can't get
% anywhere else!  That's why it is in a different component than vertices
% 1,2,3,5,6.

%%
% We also have a largest_component function that makes it easy to just get
% the largest connected component.
clf;
[Acc,f] = largest_component(A);
graph_draw(Acc,xy(f,:))
%%
% The filter variable f, tells us which vertices in the original graph made
% it into the largest strong component.  We can just apply that filter to
% the coordinates xy and reuse them for drawing the graph!

%% Statistics
% Graph statistics are just measures that indicate a property of the graph
% at every vertex, or at every edges.  Arguably the simplest graph
% statistic would be the average vertex degree.  Because such statistics
% are easy to compute with the adjaceny matrix in Matlab, they do not have
% special functions in gaimc.

%%
% Load a road network to use for statistical computations
load_gaimc_graph('minnesota');
gplot(A,xy);

%%
% Average vertex degree
d = sum(A,2);
mean(d)

%%
% So the average number of roads at any intersection is 2.5.  My guess is
% that many roads have artificial intersections in the graph structure that
% do not correspond to real intersections.  Try validating that hypothesis
% using the library!

%%
% Average clustering coefficients
ccfs = clustercoeffs(A);
mean(ccfs)
% The average clustering coefficient is a measure of the edge density
% throughout the graph.  A small value indicates that the network has few
% edges and they are well distributed throughout the graph.

%%
% Average core numbers
cn = corenums(A);
mean(cn)

%% Efficient repetition
% Every time a gaimc function runs, it converts the adjacency matrix into a
% set of compressed sparse row arrays.  These arrays yield efficient access
% to the edges of the graph starting at a particular vertex.  For many
% function calls on the same graph, this conversion process slows the
% algorithms.  Hence, gaimc also accepts pre-converted input, in which case
% it skips it's conversion.
%
% Let's demonstrate how this works by calling Dijkstra's algorithm to
% compute the shortest paths between all vertices in the graph.  The
% Floyd Warshall algorithm computes these same quantities more efficiently,
% but that would just be one more algorithm to implement and maintain.

%%
% Load and convert the graph.
load_gaimc_graph('all_shortest_paths_example');
A = spfun(@(x) x-min(min(A))+1,A); % remove the negative edges
As = convert_sparse(A);
%%
% Now, we'll run Dijkstra's algorithm for every vertex and save the result
% On my 2GHz laptop, this takes 0.000485 seconds.
n = size(A,1);
D = zeros(n,n);
tic
for i=1:n
    D(i,:) = dijkstra(As,i);
end
toc
%%
% Let's try it without the conversion to see if we can notice the
% difference in speed.
% On my 2GHz laptop, this takes 0.001392 seconds.
D2 = zeros(n,n);
tic
for i=1:n
    D2(i,:) = dijkstra(A,i);
end
toc
%%
% And just to check, let's make sure the output is the same.
isequal(D,D2)


function cvt_movie ( )

%*****************************************************************************80
%
%% CVT_MOVIE generates and animates a CVT dataset.
%
%  Discussion:
%
%    This program is meant to be used interactively.  It's also
%    possible to prepare a simple input file beforehand and use it
%    in batch mode.
%
%    The program requests input values from the user:
%
%    * NDIM, the spatial dimension,
%    * N, the number of points to generate,
%    * SEED, a seed to use for random number generation;
%    * INIT, initialize the points:
%      ** file, by reading data from file;
%      ** 'GRID', picking points from a grid;
%      ** 'HALTON', from a Halton sequence;
%      ** 'RAND', using MATLAB's RAND function;
%      ** 'UNIFORM', using a simple uniform RNG;
%    * IT_MAX, the maximum number of iterations;
%    * IT_FIXED, the number of iterative steps to take
%      using a fixed set of sampling points.
%    * SAMPLE, how to conduct the sampling:
%      ** 'GRID', picking points from a grid;
%      ** 'HALTON', from a Halton sequence;
%      ** 'RAND', using MATLAB's RAND function;
%      ** 'UNIFORM', using a simple uniform RNG;
%    * SAMPLE_NUM, the number of sampling points;
%    * BATCH, the number of sampling points to create at one time;
%    * MOVIE_NAME is the name of the file in which the movie is stored;\n' );
%    * OUTPUT, a file into which the data can be stored.
%
%    To indicate that no further computations are desired, it is
%    enough to input a nonsensical value, such as -1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 December 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Qiang Du, Vance Faber, and Max Gunzburger,
%    Centroidal Voronoi Tessellations: Applications and Algorithms,
%    SIAM Review, Volume 41, 1999, pages 637-676.
%
  DEBUG = 1;

  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_MOVIE\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Generate and animate a CVT dataset.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  This program is meant to be used interactively.\n' );
  fprintf ( 1, '  It is also possible to prepare a simple input \n' );
  fprintf ( 1, '  file beforehand and use it in batch mode.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The program requests input values from the user:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  * NDIM, the spatial dimension,\n' );
  fprintf ( 1, '  * N, the number of points to generate,\n' );
  fprintf ( 1, '  * SEED, a seed to use for random number generation,\n' );
  fprintf ( 1, '  * INIT, initialize the points:\n' );
  fprintf ( 1, '    ** file, read data from a file;\n' );
  fprintf ( 1, '    ** ''GRID'', by picking points from a grid;\n' );
  fprintf ( 1, '    ** ''HALTON'', from a Halton sequence;\n' );
  fprintf ( 1, '    ** ''RAND'', using MATLAB''s RAND function;\n' );
  fprintf ( 1, '    ** ''UNIFORM'', using a simple uniform RNG;\n' );
  fprintf ( 1, '  * IT_MAX, the maximum number of iterations.\n' );
  fprintf ( 1, '  * IT_FIXED, the number of iterative steps to take\n' );
  fprintf ( 1, '    using a fixed set of sampling points.\n' );
  fprintf ( 1, '  * SAMPLE, how to conduct the sampling.\n' );
  fprintf ( 1, '    ** ''GRID'', by picking points from a grid;\n' );
  fprintf ( 1, '    ** ''HALTON'', from a Halton sequence;\n' );
  fprintf ( 1, '    ** ''RAND'', using MATLAB''s RAND function;\n' );
  fprintf ( 1, '    ** ''UNIFORM'', using a simple uniform RNG;\n' );
  fprintf ( 1, '  * SAMPLE_NUM, the number of sample points;\n' );
  fprintf ( 1, '  * BATCH, the number of sampling points to create at one time;\n' );
  fprintf ( 1, '  * OUTPUT, a file into which the data is stored.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  To indicate that no further computations are \n' );
  fprintf ( 1, '  desired, it is enough to input a nonsensical value, \n' );
  fprintf ( 1, '  such as -1.\n' );

  while ( 1 )

    fprintf ( 1, '  *\n' );
    fprintf ( 1, ' *\n' );
    fprintf ( 1, '*  Ready to generate a new dataset:\n' );
    fprintf ( 1, ' *\n' );
    fprintf ( 1, '  *\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  NDIM is the spatial dimension.\n' );
    fprintf ( 1, '  (Try ''2'' if you have no preference.)\n' );
    fprintf ( 1, '  (Any value less than 1 terminates execution.)\n' );
    ndim = [];
    ndim = input ( '  Enter NDIM:  ' );

    fprintf ( 1, '  Default NDIM = %12d\n', ndim );

    if ( ndim < 1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of NDIM = %12d\n', ndim );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  N is the number of points to generate.\n' );
    fprintf ( 1, '  (Try ''25'' if you have no preference.)\n' );
    fprintf ( 1, '  (Any value less than 1 terminates execution.)\n' );
    n = [];
    n = input ( '  Enter N:  ' );

    fprintf ( 1, '  User input N = %12d\n', n );

    if ( n < 1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of N = %12d\n', n );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

%   fprintf ( 1, '\n' );
%   fprintf ( 1, '  SEED is a seed for the random number generation.\n' );
%   fprintf ( 1, '  (Try ''123456789'' if you have no preference.)\n' );
%   fprintf ( 1, '  (Any value less than 0 terminates execution.)\n' );
%   seed = [];
%   seed = input ( '  Enter SEED:  ' );
    seed = 123456789;
    fprintf ( 1, '  Default SEED = %d\n', seed );

    if ( seed < 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of SEED = %12d\n', seed );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  INIT is the method of initializing the data:\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  file       read data from a file;\n' );
    fprintf ( 1, '  ''GRID''     by picking points from a grid;\n' );
    fprintf ( 1, '  ''HALTON''   from a Halton sequence;\n' );
    fprintf ( 1, '  ''RAND''     using MATLAB''s RAND function;\n' );
    fprintf ( 1, '  ''UNIFORM''  using a simple uniform RNG;\n' );
    fprintf ( 1, ' \n' );
    fprintf ( 1, '  (Try ''RAND'' if you have no preference.)\n' );
    fprintf ( 1, '  (A blank value terminates execution).\n' );
    fprintf ( 1, '  (Be sure to INCLUDE QUOTES around the string!\n' );
    fprintf ( 1, ' \n' );

    init_string = [];
    init_string = input ( '  Enter INIT:  ' );

    fprintf ( 1, '  User input INIT = "%s".\n', init_string );

    if ( s_len_trim ( init_string ) <= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of INIT \n' );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    if ( s_eqi ( init_string, 'RAND'  ) )
      init = -1;
    elseif ( s_eqi ( init_string, 'RANDOM' ) )
      init_string = 'RAND';
      init = -1;
    elseif ( s_eqi ( init_string, 'UNIFORM' ) )
      init = 0;
    elseif ( s_eqi ( init_string, 'HALTON'  ) )
      init = 1;
    elseif ( s_eqi ( init_string, 'GRID'    ) )
      init = 2;
    elseif ( 0 < s_len_trim ( init_string ) )
      init = 3;
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of INIT \n' );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  IT_MAX is the maximum number of iterations.\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  An iteration carries out the following steps:\n' );
    fprintf ( 1, '  * the Voronoi region associated with each\n' );
    fprintf ( 1, '    generator is estimated by sampling;\n' );
    fprintf ( 1, '  * the centroid of each Voronoi region is estimated.\n' );
    fprintf ( 1, '  * the generator is replaced by the centroid.\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  If "enough" sampling points are used,\n' );
    fprintf ( 1, '  and "enough" iterations are taken, this process\n' );
    fprintf ( 1, '  will converge.\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  (Try ''50'' if you have no preference.)\n' );
    fprintf ( 1, '  (A negative value terminates execution).\n' );
    fprintf ( 1, '\n' );
    it_max = [];
    it_max = input ( '  Enter IT_MAX:  ' );

    fprintf ( 1, '  User input IT_MAX = %12d\n', it_max );

    if ( it_max < 0 )
      fprintf ( 1, ' \n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of IT_MAX = %12d\n', it_max );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  IT_FIXED is the number of consecutive iterations\n' );
    fprintf ( 1, '  to take with a fixed set of sample points.\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Setting IT_FIXED to 1 means a new set of sample\n' );
    fprintf ( 1, '  points is generated on every iterative step;\n' );
    fprintf ( 1, '  Setting IT_FIXED equal to IT_MAX means a single set\n' );
    fprintf ( 1, '  of sample points is used for the entire iteration.\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Any value between 1 and IT_MAX is reasonable.\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  (Try "%d" if you do not have a preference).\n', it_max );
    fprintf ( 1, '  (A 0 or negative value terminates execution).\n' );
    fprintf ( 1, '\n' );
    it_fixed = [];
    it_fixed = input ( '  Enter IT_FIXED:  ' );

    fprintf ( 1, '  User input IT_FIXED = %12d\n', it_fixed );

    if ( it_max < 0 )
      fprintf ( 1, ' \n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of IT_FIXED = %12d\n', it_fixed );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  SAMPLE is the method of sampling the region:\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  ''GRID''     by picking points from a grid;\n' );
    fprintf ( 1, '  ''HALTON''   from a Halton sequence;\n' );
    fprintf ( 1, '  ''RAND''     using MATLAB''s RAND function;\n' );
    fprintf ( 1, '  ''UNIFORM''  using a simple uniform RNG;\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  (Try ''RAND'' if you have no preference.)\n' );
    fprintf ( 1, '  (A blank value terminates execution).\n' );
    fprintf ( 1, '  (Be sure to INCLUDE QUOTES around the string!\n' );
    fprintf ( 1, '\n' );

    sample_string = [];
    sample_string = input ( '  Enter SAMPLE:  ' );

    fprintf ( 1, '  User input SAMPLE = "%s".\n', sample_string );

    if ( s_len_trim ( sample_string ) <= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of SAMPLE \n' );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    if ( s_eqi ( sample_string, 'RAND'  ) )
      sample = -1;
    elseif ( s_eqi ( sample_string, 'RANDOM' ) )
      sample = -1;
      sample_string = 'RAND';
    elseif ( s_eqi ( sample_string, 'UNIFORM' ) )
      sample = 0;
    elseif ( s_eqi ( sample_string, 'HALTON'  ) )
      sample = 1;
    elseif ( s_eqi ( sample_string, 'GRID'    ) )
      sample = 2;
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of SAMPLE \n' );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  SAMPLE_NUM is the number of sample points.\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  The Voronoi regions will be explored by generating\n' );
    fprintf ( 1, '  SAMPLE_NUM points.  For each sample point, the\n' );
    fprintf ( 1, '  nearest generator is found.  Using more points\n' );
    fprintf ( 1, '  gives a better estimate of these regions.\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  SAMPLE_NUM should be much larger than N, the\n' );
    fprintf ( 1, '  number of generators.\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  (Try ''10000'' if you have no preference.)\n' );
    fprintf ( 1, '  (A zero or negative value terminates execution.)\n' );
    fprintf ( 1, '\n' );

    sample_num = [];
    sample_num = input ( '  Enter SAMPLE_NUM:  ' );

    fprintf ( 1, '  User input SAMPLE_NUM = %12d\n', sample_num );

    if ( sample_num <= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of SAMPLE_NUM = %12d\n', sample_num );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

%   fprintf ( 1, '\n' );
%   fprintf ( 1, '  BATCH is the number of sample points to create\n' );
%   fprintf ( 1, '  at one time\n' );
%   fprintf ( 1, '\n' );
%   fprintf ( 1, '  BATCH should be between 1 and SAMPLE_NUM.\n' );
%   fprintf ( 1, '\n' );
%   fprintf ( 1, '  It is FASTER to set BATCH to SAMPLE_NUM;\n' );
%   fprintf ( 1, '  setting BATCH to 1 requires the least memory.\n' );
%   fprintf ( 1, '\n' );
%   fprintf ( 1, '  (Try ''%d'' if you have no preference.)\n', ...
%     min ( 1000, sample_num ) );
%   fprintf ( 1, '  (A zero or negative value terminates execution.)\n' );
%   fprintf ( 1, '\n' );

%   batch = [];
%   batch = input ( '  Enter BATCH:  ' );
    batch = 1000;

    fprintf ( 1, '  Default BATCH = %12d\n', batch );

    if ( batch <= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of BATCH = %12d\n', batch );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    fprintf ( 1, '\n' );
    fprintf ( 1, '  MOVIE_NAME is the name of the file in which the movie is stored;\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  (Try ''movie.avi'' if you have no preference.)\n' );
    fprintf ( 1, '  (A blank value terminates execution).\n' );
    fprintf ( 1, '  (Be sure to INCLUDE QUOTES around the string!\n' );
    fprintf ( 1, ' \n' );

    movie_name = [];
    movie_name = input ( '  Enter MOVIE_NAME:  ' );

    fprintf ( 1, '  User input MOVIE_NAME = %s\n', movie_name );

%   fprintf ( 1, '\n' );
%   fprintf ( 1, '  OUTPUT is a file into which the data is stored;\n' );
%   fprintf ( 1, '\n' );
%   fprintf ( 1, '  (Try ''cvt.txt'' if you have no preference.)\n' );
%   fprintf ( 1, '  (A blank value terminates execution).\n' );
%   fprintf ( 1, '  (Be sure to INCLUDE QUOTES around the string!\n' );
%   fprintf ( 1, ' \n' );

%   file_out_name = [];
%   file_out_name = input ( '  Enter OUTPUT:  ' );

    file_out_name = 'cvt.txt';

    fprintf ( 1, '  Default OUTPUT = "%s".\n', file_out_name );

    if ( s_len_trim ( file_out_name ) <= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of OUTPUT \n' );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end

    if ( s_len_trim ( file_out_name ) <= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CVT_MOVIE\n' );
      fprintf ( 1, '  The input value of OUTPUT \n' );
      fprintf ( 1, '  is interpreted as a request for termination.\n' );
      fprintf ( 1, '  Normal end of execution.\n' );
      break
    end
%
%  Initialize the data.
%
    if ( init == 3 )
      r = data_read ( sample_string, ndim, n );
    else
      r = [];
    end

    seed_init = seed;
%
%  Initialize the data unless the user has already done that.
%
    if ( init ~= 3 )

      initialize = 1;

      [ r, seed ] = cvt_sample ( ndim, n, n, init, initialize, seed );

    end
%
%  If the initialization and sampling steps use the same random number
%  scheme, then the sampling scheme does not have to be initialized.
%
    if ( init == sample )
      initialize = 0;
    else
      initialize = 1;
    end

    if ( file_exist ( movie_name ) )
      file_delete ( movie_name )
    end

    if ( DEBUG )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  Step    SEED          L2-Change     Energy\n' );
      fprintf ( 1, '\n' );
    end

    num_frames_per_second = 10;
    aviobj = avifile ( movie_name, 'fps', num_frames_per_second );
    box = [ 0.0, 0.0; 1.0, 0.0; 1.0, 1.0; 0.0, 1.0 ]';

    it_num = 0;
    it_diff = 0.0;
    seed_base = seed_init;
    seed = seed_init;

    while ( it_num < it_max )
%
%  Once every IT_FIXED steps, update the base value of SEED,
%  either to SEED_INIT before the first call to CVT_ITERATE, or to the
%  output value of SEED from the previous call to CVT_ITERATE.
%
%  Otherwise, reset the value of SEED to SEED_BASE.
%
      if ( mod ( it_num, it_fixed ) == 0 )
        seed_base = seed;
      else
        seed = seed_base;
      end

      it_num = it_num + 1;

      seed_init2 = seed;

      [ r, seed, it_diff, energy ] = cvt_iterate ( ndim, n, batch, sample, ...
        initialize, sample_num, seed, r );

      scatter ( r(1,1:n), r(2,1:n), [], 'r', 'filled' );
      line ( [ 0.0, 1.0, 1.0, 0.0, 0.0 ], [ 0.0, 0.0, 1.0, 1.0, 0.0 ] );
      axis ( [ -0.05, 1.05, -0.05, 1.05 ] );
      axis equal
%
%  Label the axes and the plot.
%
    xlabel ( 'X', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
      'FontSize', 16 );

    ylabel ( 'Y', 'FontName', 'Helvetica', 'FontWeight', 'bold', ...
      'FontSize', 16, 'Rotation', 0 );

    it_string = sprintf ( 'Step %4d', it_num );

    title ( it_string, 'FontName', 'Helvetica', 'FontWeight', ...
      'bold', 'FontSize', 16 );

      frame = getframe ( gca );
      aviobj = addframe ( aviobj, frame );

      initialize = 0;

      if ( DEBUG )
        fprintf ( 1, '  %4d  %12d  %14e  %14e\n', ...
          it_num, seed_init2, it_diff, energy );
      end

    end

    aviobj = close ( aviobj );
%
%  Write the final data to a file.
%
    cvt_write ( ndim, n, batch, seed_init, seed, init_string, it_max, ...
      it_fixed, it_num, it_diff, energy, sample_string, sample_num, ...
      r, file_out_name );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  The data was written to the file "%s".\n', ...
      file_out_name );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Press RETURN to proceed.\n' );

    pause

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Final value of SEED = %d\n', seed );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end


function [X p ei ej] = chrobak_payne_straight_line_drawing(A,varargin)
% CHROBAK_PAYNE_STRAIGHT_LINE_DRAWING Draw planar graphs with straight lines
%
% X = chrobak_payne_straight_line_drawing(A) generates coordinates for each
% vertex such that a planar graph A can be drawn without any edge
% crossings.  This function reports an error if A is not planar.
%
% [X,p,ei,ej] = ... returns additional information.  p is a
% canonical planar ordering of the vertices, and [ei ej] are additional
% edges required to make A a maximal_planar graph.
%
% ... = chrobak_payne_straight_line_drawing(A,...) takes a set of
% key-value pairs or an options structure.  See set_matlab_bgl_options
% for the standard options.
%   options.is_maximal: is A already a maximal planar graph [{0} | 1]
%
% Note: Be careful with is_maximal=1 and nocheck=1.  If the graph is not
% maximal, then the call will crash Matlab.
%
% Example:
%   [A,xy] = grid_graph(6,5);
%   X = chrobak_payne_straight_line_drawing(A);
%   gplot(A,X,'.-'); hold on; gplot(A,xy*20,'r.-'); hold off
%   % it's still planar, but not obviously a grid!

% David Gleich
% Copyright, Stanford University, 2008

%% History
%  2007-10-06: Initial coding
%%

[trans check full2sparse] = get_matlab_bgl_options(varargin{:});
if full2sparse && ~issparse(A), A = sparse(A); end

options = struct('is_maximal',0);
options = merge_options(options,varargin{:});
if check
    check_matlab_bgl(A,struct('sym',1));
    if options.is_maximal,
        [i j] = make_maximal_planar(A);
        if ~isempty(i), error('matlab_bgl:checkFailed',...
            'The graph was not a maximal planar but is_maximal was set.'); end
    end
end

[ei ej p X] = planar_drawing_mex(A,options.is_maximal,0);


function f = f08_f0 ( n, x, y )

%*****************************************************************************80
%
%% F08_F0 returns the value of function 8.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N,1), Y(N,1), the evalution points.
%
%    Output, real F(N,1), the function values.
%
  t1(1:n,1) = 5.0 - 10.0 * x(1:n,1);
  t2(1:n,1) = 5.0 - 10.0 * y(1:n,1);
  t3(1:n,1) = exp ( - 0.5 * t1(1:n,1) .* t1(1:n,1) );
  t4(1:n,1) = exp ( - 0.5 * t2(1:n,1) .* t2(1:n,1) );
  f(1:n,1) = t3(1:n,1) + 0.75 * t4(1:n,1) .* ( 1.0 + t3(1:n,1) );

  return
end


function [vert,econ,tria] = cfmtri2(vert,econ)
%CFMTRI2 compute a conforming 2-simplex Delaunay triangulat-
%ion in the two-dimensional plane.
%   [VERT,CONN,TRIA]=CFMTRI2(VERT,CONN) computes the confor-
%   ming Delaunay trianguation, given the points VERT, and
%   edge constraints CONN. New points are inserted to bisect
%   edge constraints until all are recovered. VERT is a
%   V-by-2 array of XY coordinates to be triangulated, TRIA
%   is a T-by-3 array of vertex indexing, with each row
%   defining a triangle, such that VERT(TRIA(II,1),:),
%   VERT(TRIA(II,2),:) and VERT(TRIA(II,3),:) are the coord-
%   inates of the II-TH triangle. CONN is a C-by-2 array of
%   constraining edges, where each row defines an edge, as
%   per the TRIA array.
%
%   See also DELTRI2, DELAUNAYN

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 07/07/2017

%---------------------------------------------- basic checks
    if ( ~isnumeric(vert) || ...
         ~isnumeric(econ) )
        error('cfmtri2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(vert) ~= +2 || ndims(econ) ~= +2)
        error('cfmtri2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    if (size(vert,2)~= +2 || size(econ,2)~= +2)
        error('cfmtri2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

%-- the DELAUNAYN routine is *not* well-behaved numerically,
%-- so explicitly re-scale the problem about [-1,-1; +1,+1].
    vmax = max(vert,[],1) ;
    vmin = min(vert,[],1) ;

    vdel = vmax - vmin;
    vdel = mean(vdel) ;
    vdel = vdel * +.5 ;

    vmid = vmax + vmin;
    vmid = vmid * +.5 ;

    vert = vert - vmid;
    vert = vert / vdel;

%-- keep bisecting edge constraints until they are all reco-
%-- vered!
    while (true)

    %----------------- un-constrained delaunay triangulation
        tria = delaunay2(vert) ;

        nv = size(vert,+1);
        nt = size(tria,+1);

    %----------------------------- build non-unique edge-set
        ee = zeros(nt*3,2);
        ee((1:nt)+nt*0,:) = tria(:,[1,2]);
        ee((1:nt)+nt*1,:) = tria(:,[2,3]);
        ee((1:nt)+nt*2,:) = tria(:,[3,1]);

    %----------------- find constraints within tria-edge set
       [in] = setset2(econ,ee) ;

    %----------------------------- done when have contraints
        if (all(in)), break; end

    %----------------------------- un-recovered edge centres
        vm = vert(econ(~in,1),:) ...
           + vert(econ(~in,2),:) ;
        vm = vm * +.5 ;

    %----------------------------- un-recovered edge indexes
        ev = nv+(1:size(vm,1))';
        en = [econ(~in,+1), ev;
              econ(~in,+2), ev];

    %----------------------------- push new vert/edge arrays
        vert = [vert( :,:); vm];
        econ = [econ(in,:); en];

    end

%--------------------------------- undo geomertic re-scaling
    vert = vert * vdel ;
    vert = vert + vmid ;

end

function [tria] = delaunay2(vert)
%DELAUNAY2 thin wrapper for DELAUNAYN, so that we can have a
%   more efficient version in OCTAVE...

    isoctave = exist( ...
        'OCTAVE_VERSION','builtin')>+0;

    if (isoctave)

    %-- call QHULL and then filter zero-volume simplexes via
    %-- vectorised area comparisons.

    %-- note silliness re. EVAL, so that MATLAB doesn't com-
    %-- plain re. OCTAVE '__' names.

        tria = eval( ...
          '__delaunayn__(vert)') ;

        ab = vert(tria(:,2),:) ...
           - vert(tria(:,1),:) ;
        ac = vert(tria(:,3),:) ...
           - vert(tria(:,1),:) ;

        aa = ab(:,1).* ac(:,2) ...
           - ab(:,2).* ac(:,1) ;

        lb = sumsq(ab,2) ;
        lc = sumsq(ac,2) ;

        ll = max (lb,lc) ;

        keep = abs(aa) >= ll * eps^.8 ;

        tria = tria(keep,:);

    else

    %-- the default call in MATLAB seems to be fast enough!!

        tria = ...
        delaunay (vert(:,1),vert(:,2));

    end

end


function [p, npix] = histroi(f, c, r)
%HISTROI Computes the histogram of an ROI in an image.
%   [P, NPIX] = HISTROI(F, C, R) computes the histogram, P, of a
%   polygonal region of interest (ROI) in image F.  The polygonal
%   region is defined by the column and row coordinates of its
%   vertices, which are specified (sequentially) in vectors C and R,
%   respectively. All pixels of F must be >= 0. Parameter NPIX is the
%   number of pixels in the polygonal region.

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.5 $  $Date: 2003/09/05 16:14:35 $

% Generate the binary mask image.
B = roipoly(f, c, r);

% Compute the histogram of the pixels in the ROI.
p = imhist(f(B));

% Obtain the number of pixels in the ROI if requested in the output.
if nargout > 1
   npix = sum(B(:));
end


function [deltaTNew,xNew,tNew,k,dxdtCur,exitCode]=performOneAdaptiveRKStep(xCur,tCur,f,deltaT,deltaTMinMag,deltaTMaxMag,dxdtCur,order,solutionChoice,AbsTol,RelTol)
%%PERFORMONEADAPTIVERKSTEP Perform a single adaptive Runge-Kutta step,
%                   returning the new adjusted stepsize. This function is a
%                   subroutine of the function RKAdaptiveOverRange. Though
%                   most folks will just directly use the function
%                   RKAdaptiveOverRange to perform adaptive Runge-Kutta
%                   integration over a particular timespan, this subroutine
%                   has been broken out separately as one might want to
%                   program Runge-Kutta integration routines that
%                   adaptively integrate until a certain criterion is
%                   satisfied. As this function is meant as an efficient
%                   subroutine, none of the inputs provide default
%                   parameters if omitted.
%
%INPUTS: xCur The NX1 state vector at time tCur.
%        tCur The scalar current time of integration.
%           f The function handle for f(x,t)=dxdt over which integration is
%             to be performed. The output is NX1-dimensional.
%      deltaT The current stepsize to be taken in t. This can be positive
%             or negative.
% deltaTMinMag The minimum allowable magnitude of the step size.
% deltaTMaxMag The maximum allowable magnitude of the step size.
%      dxdtCur The value f(xCur,tCur). This is requested so that methods
%             that are FSAL (See comments to the function RungeKStep) can
%             avoid additional computations.
% order,solutionChoice  A pair of optional parameters that specify the
%              highest order of the embedded Runge-Kutta pair to use as
%              well as the specific algorithm to use. Details are given in
%              the comments to the RungeKStep function. If omitted or empty
%              matrices are passed, the default order of 5 is used and the
%              default solutionChoice of 0 is used.
%       RelTol The maximum relative error tolerance allowed, a positive
%              scalar (its use is explained in more detail below).
%       AbsTol The absolute error tolerance allowed, a positive scalar, of
%              the same for all components of x, or a positive NX1 vector.
%
%OUTPUTS: deltaTNew The value of deltaT that can be used for the next
%               adaptive step (i.e. pass it to this function). If the
%               function fails (exitCode~=0), then this will be set to the
%               last step size used.
%          xNew The updated NX1 state vector.
%          tNew The time of the updated state vector.
%             k The values of the derivatives f evaluated at various
%               points as determined by the algorithm used for the
%               step. This can be passed to functions like RKInterpPolys to
%               perform interpolation.
%      exitCode A code indicating whether the step could be successfully
%               performed. Possible values are
%               0: Integration was successful.
%               1: Unable to get a small enough step size.
%               3: Non-finite number encountered.
%
%The algorithm used for the step is described in the comments to the
%function RKAdaptiveOverRange.
%
%June 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    %Determine the orders of the main and subsidiary embedded Runge-Kutta
    %formulae that were chosen. The order of convergence for the step size
    %testing is the smallest order taken. The isFSAL flag indicates whether
    %k(:,end) is the value of f evaluated at the next step.
    [orders,isFSAL]=RungeKStep(order,solutionChoice);
    RKOrder=min(orders);

    deltaTMag=abs(deltaT);
    deltaTSign=sign(deltaT);

    %The first time the choice in step size fails, it is adjusted the
    %"optimal" way in the Runge-Kutta-Fehlberg method. Additional times,
    %the step size is just halved in the hope that it will reach an
    %accepted value more quickly.
    failedReducingStepSize=false;
    moveOnToNextStep=false;
    while(moveOnToNextStep==false)
        %The returned value dxValdt in k(:,1) is for xCur and tCur, which
        %is curStep-1.
        [xPredMain,xPredSubsid,k]=RungeKStep(xCur,tCur,f,deltaT,dxdtCur,order,solutionChoice);

        %Integration can only be over finite functions.
        if(any(~isfinite(xPredMain))||any(~isfinite(xPredSubsid)))
            xNew=[];
            tNew=[];
            exitCode=3;
            deltaTNew=deltaTMag;
            return;
        end

        %The local error estimate. This must be transformed into a
        %combination relative/ absolute error term to determine whether
        %the step should be rejected.
        normFactor=max(max(abs(xPredMain),abs(xCur)),AbsTol/RelTol);
        theError=max(abs((xPredMain-xPredSubsid)./normFactor));

        if(theError>RelTol)
            if(deltaTMag<deltaTMinMag)
                %If the step size got too small, then return.
                xNew=[];
                tNew=[];
                exitCode=1;
                deltaTNew=[];
                return;
            end

            if(failedReducingStepSize==false)
                failedReducingStepSize=true;

                %The Fehlberg step reduction (using the relative error).
                deltaTMag=max(deltaTMinMag,deltaTMag*max(0.1, 0.8*(RelTol/theError)^(1/RKOrder)));
            else
                %Just halve the step size.
                deltaTMag=deltaTMag/2;
            end
            deltaT=deltaTSign*deltaTMag;
        else
            %The step was successful; leave the loop.
            moveOnToNextStep=true;
        end
    end

    %The step is successful, save the results from the step, including
    %information so that interpolation can be perfromed, if necessary.
    xNew=xPredMain;
    tNew=tCur+deltaT;

    %Save the current value to be reused on the next step, if the method
    %is an FSAL function, so that an evaluation of f can be avoided.
    if(isFSAL)
        dxdtCur=k(:,end);
    else
        dxdtCur=f(xNew,tNew);
    end

    %If a step is successful, then increase the step size for the
    %next step in the standard manner used with Runge-Kutta-
    %Fehlberg methods, but limit the maximum size of the increase
    %to a scale factor of 4. This avoid huge step sizes when the
    %predicted error is very small.
    deltaTMag=min(deltaTMaxMag,deltaTMag*min(4,0.8*(RelTol/theError)^(1/RKOrder)));

    %Since the minimum step size changes every loop, this makes sure that
    %deltaT does not go beneath it just because the loop changed.
    deltaTMag=max(deltaTMag,deltaTMinMag);
    deltaTNew=deltaTMag*deltaTSign;

    exitCode=0;
end

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