close all
clear

%--------------------------------------iMPC-DCBF------------------------------------------------
xo = 0;
yo = 0;
r = 1;
N = 24;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 4;
gamma2 = 4;
nsim = 100;%Total time steps
Itera_x = [];%This is to store iterative convergence of location x
Itera_y = [];%This is to store iterative convergence of location y
Itera_theta = [];%This is to store iterative convergence of orientation theta
Itera_v = [];%This is to store iterative convergence of speed v

% Constraints

umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10;  10;  10;  10];

% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = [-3 0 0 0];
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];

% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];

% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
    x_0 = [x_0 x0new];
    x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end

abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);

% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];%Highest order mcbf=2
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];

% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();

% Check solver status
if ~strcmp(res.info.status, 'solved')
   error('OSQP did not solve the problem!')
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);

for j = 1 : (K1-1)
    storagex = ctrlx;
    storageu = ctrlu;
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];%Highest order mcbf=2
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
       error('OSQP did not solve the problem!')
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;

for i = 1 : (nsim-1)

   for j = 1 : K
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];%Highest order mcbf=2
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
       error('OSQP did not solve the problem!')
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    if i == 6 %Get several open-loop trajectories at different iterations predicted at t = 6
     Itera_x = [Itera_x; ctrlx(1:4:(N*nx)+1)'];
     Itera_y = [Itera_y; ctrlx(2:4:(N*nx)+2)'];
     Itera_theta = [Itera_theta; ctrlx(3:4:(N*nx)+3)'];
     Itera_v = [Itera_v; ctrlx(4:4:(N*nx)+4)'];
    end
    x0 = ctrlx(1:nx);
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
    storagex = ctrlx;
    storageu = ctrlu;
   end
   ctrl = ctrlu(1:nu);
   x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
   ctrlu = rewrite(ctrlu, nu, N);
   x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
   u_0 = transu(ctrlu, nu, N);
   impc(:, i+2) = x0;
end

save('trajectory','impctra');%Close-loop 2D-trajectory where tsim=100
save('iteration_x','Itera_x');
save('iteration_y','Itera_y');
save('iteration_theta','Itera_theta');
save('iteration_v','Itera_v');

% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
    for j = 1 : yy
        xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
    end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end

% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
    Acbfx(i, (i*nx)+1) = abc(1, (i+1));
    Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
    Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end

% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
 res = zeros(nxx, nyy);
 res(:,1) = x0;
 for i = 1 : (nyy -1)
     res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
 end
end

% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
 resu = zeros(nxx, nyy);
 for i = 1 : (nyy)
     resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
 end
end

% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end

% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
 x_0 = zeros(nx, N+1);
 x_0(:, 1) = x0;
 for i=1 : N
    u0 = ctrlu((i-1)*nu+1:i*nu);
    x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
    x_0(:, i + 1) = x0;
 end
end

% Get AA matrix
function AA = getaa(x0, dt)
 AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
 CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
   x0 = x_0(:,i+1);
   AA = getaa(x0, dt);
   Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
    u0 = u_0(:,i);
    x0 = x_0(:,i);
    x1 = x_0(:,i+1);
    CC = getcc(x0, x1, u0, dt);
    Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
    Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
    Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
    Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
    Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
    Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
    Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
    Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
    lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end


function a = r8vec_indicator ( n )

%*****************************************************************************80
%
%% R8VEC_INDICATOR sets an R8VEC to the indicator vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Output, real A(N), the vector with entries (1, 2, ..., N ).
%
  a = ( ( 1 : n ) )';

  return
end


function c = randchar(n)

u = rand(1,n);
u = floor(u*26)+65;
c=char(u);


%MDL_QUADCOPTER Dynamic parameters for a quadrotor.
%
% MDL_QUADCOPTER is a script creates the workspace variable quad which
% describes the dynamic characterstics of a quadrotor flying robot.
%
% Properties::
%
% This is a structure with the following elements:
%
% nrotors   Number of rotors (1x1)
% J         Flyer rotational inertia matrix (3x3)
% h         Height of rotors above CoG (1x1)
% d         Length of flyer arms (1x1)
% nb        Number of blades per rotor (1x1)
% r         Rotor radius (1x1)
% c         Blade chord (1x1)
% e         Flapping hinge offset (1x1)
% Mb        Rotor blade mass (1x1)
% Mc        Estimated hub clamp mass (1x1)
% ec        Blade root clamp displacement (1x1)
% Ib        Rotor blade rotational inertia (1x1)
% Ic        Estimated root clamp inertia (1x1)
% mb        Static blade moment (1x1)
% Ir        Total rotor inertia (1x1)
% Ct        Non-dim. thrust coefficient (1x1)
% Cq        Non-dim. torque coefficient (1x1)
% sigma     Rotor solidity ratio (1x1)
% thetat    Blade tip angle (1x1)
% theta0    Blade root angle (1x1)
% theta1    Blade twist angle (1x1)
% theta75   3/4 blade angle (1x1)
% thetai    Blade ideal root approximation (1x1)
% a         Lift slope gradient (1x1)
% A         Rotor disc area (1x1)
% gamma     Lock number (1x1)
%
%
% Notes::
% - SI units are used.
%
% References::
% - Design, Construction and Control of a Large Quadrotor micro air vehicle.
%   P.Pounds, PhD thesis,
%   Australian National University, 2007.
%   http://www.eng.yale.edu/pep5/P_Pounds_Thesis_2008.pdf
% - This is a heavy lift quadrotor
%
% See also sl_quadrotor.

% MODEL: quadrotor

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

quad.nrotors = 4;                %   4 rotors
quad.g = 9.81;                   %   g       Gravity                             1x1
quad.rho = 1.184;                %   rho     Density of air                      1x1
quad.muv = 1.5e-5;               %   muv     Viscosity of air                    1x1

% Airframe
quad.M = 4;                      %   M       Mass                                1x1
Ixx = 0.082;
Iyy = 0.082;
Izz = 0.149;%0.160;
quad.J = diag([Ixx Iyy Izz]);    %   I       Flyer rotational inertia matrix     3x3

quad.h = -0.007;                 %   h       Height of rotors above CoG          1x1
quad.d = 0.315;                  %   d       Length of flyer arms                1x1

%Rotor
quad.nb = 2;                      %   b       Number of blades per rotor          1x1
quad.r = 0.165;                  %   r       Rotor radius                        1x1

quad.c = 0.018;                  %   c       Blade chord                         1x1

quad.e = 0.0;                    %   e       Flapping hinge offset               1x1
quad.Mb = 0.005;                 %   Mb      Rotor blade mass                    1x1
quad.Mc = 0.010;                 %   Mc      Estimated hub clamp mass            1x1
quad.ec = 0.004;                 %   ec      Blade root clamp displacement       1x1
quad.Ib = quad.Mb*(quad.r-quad.ec)^2/4 ;        %   Ib      Rotor blade rotational inertia      1x1
quad.Ic = quad.Mc*(quad.ec)^2/4;           %   Ic      Estimated root clamp inertia        1x1
quad.mb = quad.g*(quad.Mc*quad.ec/2+quad.Mb*quad.r/2);    %   mb      Static blade moment                 1x1
quad.Ir = quad.nb*(quad.Ib+quad.Ic);             %   Ir      Total rotor inertia                 1x1

quad.Ct = 0.0048;                %   Ct      Non-dim. thrust coefficient         1x1
quad.Cq = quad.Ct*sqrt(quad.Ct/2);         %   Cq      Non-dim. torque coefficient         1x1

quad.sigma = quad.c*quad.nb/(pi*quad.r);         %   sigma   Rotor solidity ratio                1x1
quad.thetat = 6.8*(pi/180);      %   thetat  Blade tip angle                     1x1
quad.theta0 = 14.6*(pi/180);     %   theta0  Blade root angle                    1x1
quad.theta1 = quad.thetat - quad.theta0;   %   theta1  Blade twist angle                   1x1
quad.theta75 = quad.theta0 + 0.75*quad.theta1;%   theta76 3/4 blade angle                     1x1
quad.thetai = quad.thetat*(quad.r/quad.e);      %   thetai  Blade ideal root approximation      1x1
quad.a = 5.5;                    %   a       Lift slope gradient                 1x1

% derived constants
quad.A = pi*quad.r^2;                 %   A       Rotor disc area                     1x1
quad.gamma = quad.rho*quad.a*quad.c*quad.r^4/(quad.Ib+quad.Ic);%   gamma   Lock number                         1x1

quad.b = quad.Ct*quad.rho*quad.A*quad.r^2; % T = b w^2
quad.k = quad.Cq*quad.rho*quad.A*quad.r^3; % Q = k w^2

quad.verbose = false;


function y = nansum(x,dim)
%Replacement for Matlab NANSUM Sum, ignoring NaNs.
%
x(isnan(x)) = 0;
if nargin == 1 % let sum figure out which dimension to work along
    y = sum(x);
else           % work along the explicitly given dimension
    y = sum(x,dim);
end

return;


function ax = axis_pct(pct)
% AXIS_PCT       Set reasonable axis limits.
%
% AXIS_PCT(pct) sets axis limits to extend pct% beyond limits of plotted
% objects.  Default is 5%.
% Works for linear or log scale.
% Unfortunately, the axes won't change when new points are plotted.

% Written by Tom Minka

if nargin < 1
  pct = 0.05;
end
ax = [Inf -Inf Inf -Inf Inf -Inf];

% find bounding box of plotted objects
children = get(gca,'children');
for child = children'
  if strcmp(get(child,'type'),'text')
    xyz = get(child,'position');
    % need to determine bounding box of the text
    if strcmp(get(gca,'xscale'), 'log')
      xyz(1) = log10(xyz(1));
    end
    if strcmp(get(gca,'yscale'), 'log')
      xyz(2) = log10(xyz(2));
    end
    if strcmp(get(gca,'zscale'), 'log')
      xyz(3) = log10(xyz(3));
    end
    c([1 2]) = xyz(1);
    c([3 4]) = xyz(2);
    c([5 6]) = xyz(3);
  else
    x = get(child,'xdata');
    x = x(finite(x));
    if strcmp(get(gca,'xscale'), 'log')
      x = x(x > 0);
      x = log10(x);
    end
    if isempty(x)
      c([1 2]) = 0;
    else
      c([1 2]) = [min(x(:)) max(x(:))];
    end
    y = get(child,'ydata');
    y = y(finite(y));
    if strcmp(get(gca,'yscale'), 'log')
      y = y(y > 0);
      y = log10(y);
    end
    if isempty(y)
      c([3 4]) = 0;
    else
      c([3 4]) = [min(y(:)) max(y(:))];
    end
    try
      z = get(child,'zdata');
      z = z(finite(z));
      if isempty(z)
	c([5 6]) = 0;
      else
	if strcmp(get(gca,'zscale'), 'log')
	  z = z(z > 0);
	  z = log10(z);
	end
	c([5 6]) = [min(z(:)) max(z(:))];
      end
    end
  end
  ax([1 3 5]) = min(ax([1 3 5]), c([1 3 5]));
  ax([2 4 6]) = max(ax([2 4 6]), c([2 4 6]));
end
dx = ax(2)-ax(1);
if dx == 0
  dx = 1;
end
dy = ax(4)-ax(3);
if dy == 0
  dy = 1;
end
dz = ax(6)-ax(5);
if dz == 0
  dz = 1;
end
ax = ax + [-dx dx -dy dy -dz dz]*pct;
if strcmp(get(gca,'xscale'), 'log')
  ax([1 2]) = 10.^(ax([1 2]));
end
if strcmp(get(gca,'yscale'), 'log')
  ax([3 4]) = 10.^(ax([3 4]));
end
if strcmp(get(gca,'zscale'), 'log')
  ax([5 6]) = 10.^(ax([5 6]));
end
% clip for 2D
ax = ax(1:length(axis));
if ~isempty(children)
  axis(ax);
end
if nargout < 1
  clear ax
end


function lengths = trk_length(tracks)
%TRK_LENGTH - Calculate the lengths of tracks
%
% Syntax: lengths = trk_length(tracks)
%
% Inputs:
%    tracks - TrackVis track group. For tracks all of the same length (i.e.
%    TRK_INTERP has been run), either structure or matrix form is fine.
%
% Outputs:
%    lengths [1 x nTracks]
%
% Example:
%    [header tracks] = read_trk(trkPath);
%    lengths         = trk_length(tracks);
%    mean(lengths), std(lengths)
%
% Other m-files required: trk_restruc
% Subfunctions: none
% MAT-files required: none
%
% See also: TRK_READ

% Author: John Colby (johncolby@ucla.edu)
% UCLA Developmental Cognitive Neuroimaging Group (Sowell Lab)
% Apr 2010

% Put in matrix form if possible
if isstruct(tracks) && length(unique(cat(tracks.nPoints)))==1
    tracks = trk_restruc(tracks);
end

% Fast matrix operation if all tracks are the same length
if isnumeric(tracks)
    lengths = sum(sqrt(squeeze(sum((tracks(2:end,1:3,:) - tracks(1:(end-1),1:3,:)).^2, 2))), 1);

% Slow forloop if tracks are not the same length
else
    lengths = zeros(1,length(tracks));
    for i=1:length(tracks)
        lengths(i) = sum(sqrt(squeeze(sum((tracks(i).matrix(2:end,1:3) - tracks(i).matrix(1:(end-1),1:3)).^2, 2))), 1);
    end
end


function [TB] = bit2TB(bit)
% Convert computery things from bits to terabytes.
% Chad A. Greene 2012
TB = bit*2^-43;


function [PA,PO,dX] = marginalise(Xdata,ydata,Xtest,GP,ns)
%                       Xdata,Ydata,GPvar,RBF_GP_var,20,20
%
%   input -----------------------------------------------------------------
%
%       ------ Sample points ----------------------
%
%       o Xtest: (D x N), points to compute the joint distribution
%
%       ------ Gaussian Process parameters --------
%
%       o X: (D x N), set of stat points
%
%       o y: (1 x N), value at the each point
%
%       -------------------------------------------
%
%       o ns: (1 x D), sizes of joint dimensions nx = ns(1), ny = ns(2),...
%
%   output ----------------------------------------------------------------
%
%       o Mu: (1 x M), Gaussian Process smoothed y values at query points
%
%       o hy: (1 x M), marginal values at chosen discrete points
%

[D,N] = size(Xtest);

max_x = max(Xtest')';
min_x = min(Xtest')';

dX = abs(max_x - min_x)./ns';

PAO = GP(Xdata,ydata,Xtest);
PAO = reshape(PAO,ns);

if D == 2

    PA = sum(PAO.*dX(2),1);
    PO = sum(PAO.*dX(1),2);
    PA = PA(:);
    PO = PO(:);

else

    PA = sum(sum(PAO .* dX(3),4) .* dX(4),3);
    PO = sum(sum(PAO .* dX(2),1) .* dX(1),2);
    PA = squeeze(PA);
    PO = squeeze(PO);

end

end


function imgOut = TumblinTMO(img, L_da, Ld_Max, C_Max, L_wa)
%
%        imgOut = TumblinTMO(img, L_da, Ld_Max, C_Max, L_wa)
%
%
%        Input:
%           -img: an HDR image
%           -L_da: adaptation display luminance in [10,30] cd/m^2
%           -Ld_Max: maximum display luminance in [80, 180] cd/m^2
%           -C_Max: maximum LDR monitor contrast typically between 30 to 100
%
%        Output:
%           -imgOut: a tone mapped image in [0,1]
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%     The paper describing this technique is:
%     "Two Methods for Display of High Contrast Images"
% 	  by JACK TUMBLIN, JESSICA K. HODGINS, and BRIAN K. GUENTER
%     in ACM Transactions on Graphics, Vol. 18, No. 1, January 1999, Pages 56?94.
%

%is it a three color channels image?
check13Color(img);

checkNegative(img);

%check parameters
if(~exist('L_da', 'var'))
    L_da = 20;
end

if(~exist('Ld_Max', 'var'))
    Ld_Max = 100;
end

if(~exist('C_Max', 'var'))
    C_Max = 100;
end

if(~exist('L_wa', 'var'))
    L_wa = logMean(lum(img)); %luminance world adaptation
end

%compute luminance channel
L = lum(img);

%range compression
gamma_w = StevensCSF(L_wa);
gamma_d = StevensCSF(L_da);
gamma_wd = gamma_w / (1.855 + 0.4 * log(L_da));
m = C_Max.^((gamma_wd - 1) / 2.0);
Ld = L_da * m .* ((L ./ L_wa).^(gamma_w / gamma_d));
Ld = Ld / Ld_Max;

%change luminance
imgOut = ChangeLuminance(img, L, Ld);

end


%
Nfilt = size(rez.W,2);
ik = ceil(rand * Nfilt);
% ik = 1;
%
sp = find(rez.st3(:,2)==ik);
sp = sort(sp);

st = rez.st3(sp, 1);

clp = rez.cProjPC(sp, :, :);
clp = clp - mean(clp,1);
% clp = clp - my_conv2(clp, 50, 1);

nspikes = size(clp,1);
[u s v] = svdecon(clp(:,:)');

figure(1)

plotmatrix(v(:,1:4), '.')

figure(2)
vplot = clp(:,:) * u(:,1:2);
plot(st, my_conv2(vplot(:,1:2), 1, 1))

figure(3)
clp = rez.cProjPC(sp, :, :);
clp = clp - mean(clp,1);

nspikes = size(clp,1);
[u s v] = svdecon(clp(:,:)');

cls = (clp(:,:) * u(:,1:4)) * u(:,1:4)';

clp = rez.cProjPC(sp, :, :);
clp = clp - mean(clp,1);

for i = 1:16
    subplot(4,4,i)
    l = randperm(size(clp,1), 2);
    w = cls(l(1), :) - cls(l(2), :);
    w = zscore(w);

    Y = clp(:,:) * w';

    hist(Y, 100)
end
%%

hist(Y, 100)


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

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

    global schurfun schurfun_par
%%
%% compute NT scaling matrix
%%
    [par.W,par.G,par.sv,par.gamx,par.gamz,par.dd,par.ee,par.ff] = ...
     NTscaling(blk,X,Z,Zchol,invZchol);
%%
%% compute schur matrix
%%
    m = length(rp);
    schur = sparse(m,m);
    UU = []; EE = []; Afree = [];
    dX = cell(size(blk,1),1); dy = []; dZ = cell(size(blk,1),1);
%%
    for p = 1:size(blk,1)
       pblk = blk(p,:);
       if strcmp(pblk{1},'l')
          [schur,UU,EE] = schurmat_lblk(blk,At,par,schur,UU,EE,p,par.dd);
       elseif strcmp(pblk{1},'q');
          [schur,UU,EE] = schurmat_qblk(blk,At,par,schur,UU,EE,p,par.dd,par.ee);
       elseif strcmp(pblk{1},'s')
          if isempty(schurfun{p})
             schur = schurmat_sblk(blk,At,par,schur,p,par.W);
          elseif isstr(schurfun{p})
             schurtmp = sparse(m,m);
             if ~isempty(par.permZ{p})
                Wp = par.W{p}(par.permZ{p},par.permZ{p});
             else
                Wp = par.W{p};
             end
             eval(['schurtmp = ',schurfun{p},'(Wp,Wp,schurfun_par(p,:));']);
             schur = schur + schurtmp;
          end
       elseif strcmp(pblk{1},'u')
          Afree = [Afree, At{p}'];
       end
    end
%%
%% compute rhs
%%
    [rhs,EinvRc,hRd] = NTrhsfun(blk,At,par,X,Z,rp,Rd,sigmu);
%%
%% solve linear system
%%
    [xx,coeff,L] = linsysolve(par,schur,UU,Afree,EE,rhs);
%%
%% compute (dX,dZ)
%%
    [dX,dy,dZ] = NTdirfun(blk,At,par,Rd,EinvRc,xx,m);
%%**********************************************************************


function [values,modes] = max(SO3F,varargin)
% global, local and pointwise maxima of functions on SO(3)
%
% Syntax
%   [v,pos] = max(SO3F) % the position where the maximum is atained
%
%   [v,pos] = max(SO3F,'numLocal',5) % the 5 largest local maxima
%
%   SO3F = max(SO3F, c) % maximum of a rotational functions and a constant
%   SO3F = max(SO3F1, SO3F2) % maximum of two rotational functions
%   SO3F = max(SO3F1, SO3F2, 'bandwidth', bw) % specify the new bandwidth
%
%   % compute the maximum of a multivariate function along dim
%   SO3F = max(SO3Fmulti,[],dim)
%
% Input
%  SO3F, SO3F1, SO3F2 - @SO3Fun
%  SO3Fmulti          - a multivariate @SO3Fun
%  c                  - double
%
% Output
%  v - double
%  pos - @rotation / @orientation
%
% Options
%  kmax          - number of iterations
%  numLocal      - number of peaks to return
%  startingNodes - @rotation / @orientation
%  tolerance     - minimum distance between two peaks
%  resolution    - minimum step size
%  maxStepSize   - maximm step size
%
% Example
%
%   %find the local maxima of the <SantaFe.html SantaFe> ODF
%   mode = calcModes(SantaFe)
%   plotPDF(SantaFe,Miller(0,0,1,mode.CS))
%   annotate(mode)
%
% See also
% SO3Fun/min SO3Fun/max

if isa(SO3F,'SO3FunHarmonic') && ~SO3F.isReal
  SO3F = SO3F.isReal;
  warning('By taking the maxima of SO3Funs, the functions should be real valued.')
end
if nargin>1 && isa(varargin{1},'SO3FunHarmonic') && ~varargin{1}.isReal
  varargin{1}.isReal = 1;
  warning('By taking the maxima of SO3Funs, the functions should be real valued.')
end

if numel(SO3F)==1
  [values,modes] = max@SO3Fun(SO3F,varargin{:});
  return
end

% multivariate functions
s = size(SO3F);

if nargin>1 && (isa(varargin{1},'SO3FunHarmonic') || isnumeric(varargin{1}))
  t = size(varargin{1});
  SO3F1 = SO3F.*ones(t);
  SO3F2 = varargin{1}.*ones(s);
  values = [];
  for k=1:numel(SO3F1)
    if isa(SO3F2,'SO3FunHarmonic')
      A = max@SO3Fun(SO3F1.subSet(k),SO3F2.subSet(k),varargin{:});
    else
      A = max@SO3Fun(SO3F1.subSet(k),SO3F2(k),varargin{:});
    end
    values = [values,A];
  end
  values = reshape(values,size(SO3F1));
  return
end

len = get_option(varargin,'numLocal',1);
values = zeros(len,prod(s));
modes = rotation.id(len,prod(s));
for k=1:numel(SO3F)
  [v,m] = max@SO3Fun(SO3F.subSet(k),varargin{:});
  values(:,k)=v; modes(:,k)=m;
end

end


function Vo = camview(varargin)
%CAMVIEW  Records or sets the viewpoint of the current axes
%
%   Vo = camview([hAx], [Vi])
%
% Records or sets the viewpoint of the current axes, including projection
% type. This is useful for giving multiple axes (with the same coordinate
% frame) the same viewpoint, or resetting the viewpoint to a known point.
% A data aspect ratio of [1 1 1] is assumed.
%
% The view is recorded and set using a 4x4 matrix. The upper 3x4 quadrant
% of this matrix is a projection matrix from the axes coordinate frame to
% the camera coordinate frame (the view frustrum coordinates range from -1
% to 1 in x and y directions). Padding the last row with [0 0 0 1] makes it
% possible to use projection matrices computed externally.
%
% IN:
%    hAx - Handle to the axes in question. Default: gca.
%    Vi - 4x4 matrix defining the viewpoint to set the current axes to. The
%         matrix should defined by calling camview on previous axes.
%
% OUT:
%    Vo - 4x4 matrix specifying the viewpoint of the current axes just
%         prior to the function being called.

% Set default inputs
hAx = gca;
Vi = [];
% Parse the inputs
for a = 1:nargin
    if ishandle(varargin{a})
        hAx = varargin{a};
    else
        Vi = varargin{a};
    end
end

if nargout > 0
    % Get the current viewpoint
    t = get(hAx, 'CameraPosition');
    d = get(hAx, 'CameraTarget') - t;
    K = eye(3);
    K([1 5]) = 1 / tan(get(hAx, 'CameraViewAngle') * pi / 360);
    R(:,3) = d / norm(d);
    R(:,2) = get(hAx, 'CameraUpVector');
    R(:,1) = cross(R(:,3), R(:,2));
    Vo = K * R' * [eye(3) -t'];
    Vo(4,:) = [norm(d) 0 0 strcmp(get(hAx, 'Projection'), 'perspective')];
end
if ~isempty(Vi)
    % Decompose the projection matrix
    st = @(M) M(end:-1:1,end:-1:1)';
    [R, K] = qr(st(Vi(1:3,1:3)));
    K = st(K);
    I = diag(K) < 0;
    K(:,I) = -K(:,I);
    R = st(R);
    R(I,:) = -R(I,:);
    t = (K * R) \ -Vi(1:3,4);
    K = K / K(3,3);

    % Set the current viewpoint
    projection = {'perspective', 'orthographic'};
    set(hAx, 'CameraTarget', t'+R(3,:)*(Vi(4)+(Vi(4)==0)), ...
             'CameraPosition', t, ...
             'CameraUpVector', R(2,:), ...
             'CameraViewAngle', atan(1/K(5))*360/pi, ...
             'Projection', projection{(Vi(16)==0)+1});
end
return


function r=v_rotqr2ro(q)
%ROTQR2RO converts a real quaternion to a 3x3 rotation matrix
% Inputs:
%
%     Q(4,...)      Real-valued quaternion array (possibly unnormalized)
%
% Outputs:
%
%     R(3,3,...)    Rotation matrix array
%                   Plots a diagram if no output specified
%
% In the quaternion representation of a rotation, and q(1) = cos(t/2)
% where t is the angle of rotation in the range 0 to 2pi
% and q(2:4)/sin(t/2) is a unit vector lying along the axis of rotation
% a positive rotation about [0 0 1] takes the X axis towards the Y axis.
%
%      Copyright (C) Mike Brookes 2007-2018
%      Version: $Id: v_rotqr2ro.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

persistent a b c d e f g h m
if isempty(a)
    a=[1 5 9];
    b=[11 16 6];
    c=[16 6 11];
    d=[4 8 3];
    e=[10 15 14];
    f=[4 2 3];
    g=[2 6 7];
    h=[1 2 3 4 1 2 3 4 1 2 3 4 1 2 3 4]';
    m=[1 1 1 1 2 2 2 2 3 3 3 3 4 4 4 4]';
end
sz=size(q);
q=reshape(q,4,[]);      % convert to 2D matrix
nq =size(q,2);          % number of quaternions to convert
p=2*q(h,:).*q(m,:)./repmat(sum(q.^2,1),16,1); % force normalized and calculate quadratic terms
r=zeros(9,nq);          % space for nq rotation matrices
r(a,:)=1-p(b,:)-p(c,:);
r(d,:)=p(e,:)-p(f,:);
r(g,:)=p(e,:)+p(f,:);
r=reshape(r,[3 3 sz(2:end)]);
if ~nargout
    % display rotated cube
    q1=q(:,1);
    q1=q1/sqrt(q1'*q1); % normalize the first quaternion
    clf('reset'); % clear current axis
    v0=[-1 1 1 -1 -1 1 1 -1; -1 -1 1 1 -1 -1 1 1; -1 -1 -1 -1 1 1 1 1]*0.5; % unrotated coordinates
    v=r(:,:,1)*v0; % use first elemnt of r to rotate
    fv=[4 1 5 8; 2 3 7 6; 1 2 6 5; 3 4 8 7; 2 1 4 3; 5 6 7 8]; % verices for each face
    fc=[0 1 1; 1 0 0; 1 0 1; 0 1 0; 1 1 0; 0 0 1]; % colours for faces
    xc={[25 46 46 25; 55 55 49 49]/100,
        [25 33 33 39 39 46 46 39 39 33 33 25; 55 55 63 63 55 55 49 49 42 42 49 49]/100,
        [48 56 62 69 76 66 76 69 62 56 48 59; 68 68 58 68 68 53 37 37 47 37 37 53]/100,
        [48 55 62 69 77 65 65 59 59; 68 68 55 68 68 50 37 37 50]/100,
        [50 74 74 57 74 74 50 50 67 50; 68 68 62 43 43 37 37 43 62 62]/100};
    xf=[1 3; 2 3; 1 4; 2 4; 1 5; 2 5]; % characters to plot on each face
    nf=size(fv,1); % number of faces
    for i=1:6
        p(i)=patch(v(1,fv(i,:)),v(2,fv(i,:)),v(3,fv(i,:)),fc(i,:));
        set(p(i),'FaceAlpha',0.65);
        k=1.001; % factor to move out labels slightly to get correct depth ordering
        for j=1:2
            xij=xc{xf(i,j)}; % relative coordinates of character vertices
            patch(k*(v(1,fv(i,1))+(v(1,fv(i,2))-v(1,fv(i,1)))*xij(1,:)+(v(1,fv(i,4))-v(1,fv(i,1)))*xij(2,:)), ...
                k*(v(2,fv(i,1))+(v(2,fv(i,2))-v(2,fv(i,1)))*xij(1,:)+(v(2,fv(i,4))-v(2,fv(i,1)))*xij(2,:)), ...
                k*(v(3,fv(i,1))+(v(3,fv(i,2))-v(3,fv(i,1)))*xij(1,:)+(v(3,fv(i,4))-v(3,fv(i,1)))*xij(2,:)),1-fc(i,:));
        end
    end
    qa=q1(2:4);
    qm=max(abs(qa));
    if qm>1e-6*abs(q1(1))
        qa=qa/(qm/0.7); % scale so axis extends outside the cube
        hold on;
        plot3([1 -1]*qa(1),[1 -1]*qa(2),[1 -1]*qa(3),'-');
        hold off
    end
    th=360/pi*acos(abs(q1(1)));
    xlabel('x axis');
    ylabel('y axis');
    zlabel('z axis');
    q=q(:,1)*((2*(q(find(q1(:)~=0,1))>0)-1)); % force leading coefficient to be positive
    title(sprintf('%d^\\circ, qr'' = [%.2f,%.2f,%.2f,%.2f], eu_{xyzo}'' = [%d, %d, %d]^\\circ',round(th),q1(:),round(v_rotqr2eu('xyz',q1)*180/pi)));
    axis([-1 1 -1 1 -1 1 0 1]*sqrt(3)/2);
    axis equal
    grid on
    view(3);
end


classdef TestAddWeighted
    %TestAddWeighted

    methods (Static)
        function test_images
            img1 = cv.imread(fullfile(mexopencv.root(),'test','img001.jpg'), 'ReduceScale',2);
            img2 = randi([0 255], size(img1), 'uint8');

            out = cv.addWeighted(img1,1.0, img2,0.4, 0.2);
            validateattributes(out, {class(img1)}, {'size',size(img1)});
            if mexopencv.require('images')
                expected = my_add_weighted(img1,1.0, img2,0.4, 0.2);
                assert(isequal(out, expected));
            end
        end

        function test_2d_matrices
            A = uint8([100 150; 200 250]);
            B = uint8([200 100; 100 100]);

            out = cv.addWeighted(A,0.2, B,0.8, 0.0);
            validateattributes(out, {class(A)}, {'size',size(A)});
            expected = A*0.2 + B*0.8 + 0;
            assert(isequal(out, expected));

            out = cv.addWeighted(A,0.2, 1,0.8, 0.0);
            validateattributes(out, {class(A)}, {'size',size(A)});
            expected = A*0.2 + 1*0.8 + 0;
            assert(isequal(out, expected));

            out = cv.addWeighted(1,0.2, B,0.8, 0.0);
            validateattributes(out, {class(B)}, {'size',size(B)});
            expected = 1*0.2 + B*0.8 + 0;
            assert(isequal(out, expected));

            out = cv.addWeighted(1,0.2, 1,0.8, 0.0);
            validateattributes(out, {'double'}, {'scalar'});
            expected = 1*0.2 + 1*0.8 + 0;
            assert(isequal(out, expected));
        end

        function test_output_depth
            out = cv.addWeighted(uint16(1),0.5, int8(1),0.5, 0.0, 'DType','single');
            validateattributes(out, {'single'}, {'scalar'});
            expected = single(1*0.5 + 1*0.5 + 0);
            assert(isequal(out, expected));
        end

        function test_error_argnum
            try
                cv.addWeighted();
                throw('UnitTest:Fail');
            catch e
                assert(strcmp(e.identifier,'mexopencv:error'));
            end
        end
    end

end

function out = my_add_weighted(src1, a, src2, b, c, dtype)
    %MY_ADD_WEIGHTED  Similar to cv.addWeighted using imlincomb from IPT

    if nargin < 6, dtype = class(src1); end

    % add two weighted images with specified output class
    out = imlincomb(a,src1, b,src2, c, dtype);
end


function csd = CSD(lfp)

%CSD - Compute current source density.
%
%  USAGE
%
%    csd = CSD(lfp)
%
%    lfp            local field potential samples
%
%  SEE
%
%    See also PlotCSD.

% Copyright (C) 2008-2011 by Michaël Zugaro
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.

% Check number of parameters
if nargin < 1,
  error('Incorrect number of parameters (type ''help CSD'' for details).');
end

t = lfp(:,1);
y = lfp(:,2:end);
y = y - repmat(mean(y),length(t),1);
d = -diff(y,2,2);
csd = [t d];


principal = 1000;
alpha = 0.1;
debt = principal * (1 + alpha) ^ 2;
disp(debt);


function [uGal] = cmps22uGal(cmps2)
% Convert acceleration from centimeters per square centimeter to microgalileos
% Chad A. Greene 2012
uGal = cmps2*1e+6;
end


function [data,units] = compute_dmin_wing_length(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);

  dlengthl = diff(trx(fly).wing_lengthl_mm,1,2);
  dlengthr = diff(trx(fly).wing_lengthr_mm,1,2);
  data{i} = dlengthl;
  idx = trx(fly).wing_lengthr_mm(1:end-1) <= trx(fly).wing_lengthl_mm(1:end-1);
  data{i}(idx) = dlengthr(idx);

  data{i} = data{i} ./ trx(fly).dt;

end
units = parseunits('mm/s');


function [Fconv,info] = nonlineartocone(F)
%NONLINEARTOCONE Convert nonlinear constraint to second order cone

if islinear(F)
    Fconv = F;
    info = 0;
    return
end

[n,m]=size(F);
if (n*m==1)
    Fconv = [];
    % Involved in polynomial constraints
    sqrList = yalmip('nonlinearvariables');
    h = sort(unique(sqrList));
    % % SDP relaxation
    for i = 1:size(sqrList,1)
        left_index = find(h==sqrList(i,1));
        x1_index = find(h==sqrList(i,2));
        x2_index = find(h==sqrList(i,3));
        Z{i}=zeros(length(h),length(h));
        if x1_index==x2_index
            Z{i}(x2_index,x1_index) = 1;
        else
            Z{i}(x1_index,x2_index) = 0.5;
            Z{i}(x2_index,x1_index) = 0.5;
        end
    end
    vars = getvariables(F);
    base = getbase(F);
    nonlinvars = find(ismember(vars,sqrList(:,1)));
    linvars = find(~ismember(vars,sqrList(:,1)));
    % Construct quadratic
    Q = zeros(length(h));
    for i = 1:length(nonlinvars)
        indexinlist = find(vars(nonlinvars(i))==sqrList(:,1));
        indexinh = find(vars(nonlinvars(i))==h);
        Q = Q-Z{indexinlist}*base(1+find(h(indexinh)==vars));
    end
    used = find(any(Q));
    [B,r] = chol(Q(used,used));
    if r==0
        linear = base(1);
        if ~isempty(linvars)
            linear = linear+base(1+find(ismember(linvars,vars)))*recover(linvars);
        end
        ConesAxb=[2*B*recover(h(used));1-linear];
        Conescxd=1+linear;
        Fconv = cone(ConesAxb,Conescxd);
        info = 0;
    else
        Fconv = F;
        if nargout == 2
            info = 1;
        else
            warning('Cannot re-write to second order cone constraint');
        end
    end
else
    if nargout == 2
        Fconv = F;
        info = 1;
    else
        error('nonlineartocone can only be applied to scalar inequalities')
    end
end


function [bounds, best_cover] = pascal_overlap_bound(orig_sp, sp_parts, gt, gt_ind)
%function bounds = pascal_overlap_bound(orig_sp, sp_parts, gt, gt_ind)
% Computes theoretical upper bounds of Pascal overlap score for given
% initial superpixelation sp and ground truth segmentation gt.

assert(max(gt_ind) == length(gt_ind)); % This function assumes that gt_ind = 1:n, where n >= 1.

% remove empty superpixels
% for q = length(sp):-1:1
%     if size(sp{q}.pixels,1) == 0
%         sp(q) = [];
%     end
% end

scores = zeros(max(gt_ind),length(sp_parts));

for i = 1:length(sp_parts) % for each sp
    %pix = double(sp{i}.pixels); % using double() here is extremely important. Otherwise sub2ind below caps variable s at 65536, breaking everything.
    %s{i} = sub2ind([h,w], pix(:,1), pix(:,2)); % linear indices of pixels of current superpixel in the image

    indl = 0;
    for sus = sp_parts{i}
        indl = indl + length(orig_sp{sus}.spind);
    end

    s{i} = zeros(1, indl);
    indl = 0;
    for sus = sp_parts{i} % for each original sp part of the current sp
        s{i}(indl+1:indl+length(orig_sp{sus}.spind)) = orig_sp{sus}.spind;
        indl = indl + length(orig_sp{sus}.spind);
    end

    sp_classes = gt(s{i}); % classes of pixels in current sp
    scores(:,i) = histc(sp_classes, gt_ind)/length(sp_classes);

end % for each superpixel

a = zeros(1, max(gt_ind)); % intersection pixel count
b = zeros(1, max(gt_ind)); % union pixel count
best_cover = [];

for ind = gt_ind % for each object
    q = find(gt == ind);
    gt_seg{ind} = q;
    b(ind) = length(q); % size of gt object, initial size of union
end

for cl = gt_ind % for each object
    [cl_scores, scoreperm] = sort(scores(cl,:), 'descend'); % cl_scores = a*/(a* + b*)
    cl_scoresx = 1./(1./cl_scores - 1); % cl_scoresx = a*/b*

    for i = 1:length(cl_scores)
        % condition for a/b <= (a + a*)/(b + b*) is that a/b <= a*/b*. Since
        % always a <= b, having a* >= b*, i.e., scores >= 0.5 is enough too.
        if cl_scores(i) >= 0.5 || cl_scoresx(i) >= double(a(cl))/b(cl)
            a(cl) = a(cl) + length(intersect(s{scoreperm(i)}, gt_seg{cl})); % size of intersection increases by this value
            b(cl) = b(cl) + length(setdiff(s{scoreperm(i)}, gt_seg{cl})); % size of union increases by this value
        else
            best_cover{cl} = scoreperm(1:i);
            break; % scores from now on are too low to increase the overlap ratio, stop.
        end

    end % for each sp
end % for each object

bounds = double(a)./b;

%toc


function Y = as_m(tr,X)
%
%  Evaluates matrix-matrix products with the real, symmetric matrix A,
%  i.e., Y = A*X.
%
%  A is provided as global data. This data must be generated by calling
%  'as_m_i' before calling this routine!
%
%  If called without input parameters, this routine returns the order of
%  the matrix A.
%
%  Calling sequence:
%
%    Y = as_m(tr,X)
%    n = as_m
%
%  Input:
%
%    tr        is not referenced;
%    X         a matrix of proper size.
%
%  Output:
%
%    Y         the resulting product;
%    n         the order of the matrix A.
%
%
%  LYAPACK 1.0 (Thilo Penzl, May 1999)

ni = nargin;

if ni~=2 & ni~=0
  error('Wrong number of input arguments.');
end

global LP_A

if ~length(LP_A)
  error('This routine needs global data which must be generated by calling ''as_m_i'' first.');
end

if ni==0
  Y = size(LP_A,1);
else
  Y = LP_A*X;
end


classdef DCTBasis < spx.dict.Operator

properties(SetAccess=private)
    % Dimensions
    N
end

methods

function self = DCTBasis(N)
    if nargin < 1
        error('Basis dimensions must be specified');
    end
    self.N = N;
end

function [mm, nn]  = get_size(self)
    mm = self.N;
    nn = self.N;
end

function result = apply(self, vectors)
    if size(vectors, 1) ~= self.N
        error('Dimensions mismatch');
    end
    result = idct(vectors);
end

function result = apply_ctranspose(self, vectors)
    if size(vectors, 1) ~= self.N
        error('Dimensions mismatch');
    end
    result = dct(vectors);
end

function result = double(self)
    % Converts the operator into a MATRIX
    n = self.N;
    result = zeros(n);
    for i=1:n
        x = zeros(n, 1);
        x(i) = 1;
        result(:, i) = self.apply(x);
    end
end

function result = norm(self)
    result = 1;
end

end

end


function [safety, order, union, alg_conn, safety_obs, min_d_obs ] = ...
    compute_swarm_performance(pos_history, vel_history, ...
    p_swarm, dirname)
% Compute swarm performance - This function allows to compute the
% performance from the history of the swarming variables (time, position,
% velocity, acceleration).
%
% Inputs:
%   pos_history: series of agents' positions
%   vel_history: series of agents' velocities
%   p_swarm: structure with swarm parameters
%   dirname: path of the directory where results are saved
%
% Outputs:
%   safety: safety metric. It reflects the number of agent-agent collisions
%   order: order metric. It measures the correlation between velocity
%               vectors
%   union: union metric. It measures the number of connected components of
%               the undirected graph
%   alg_conn: algebraic connectivity metric.
%   safety_obs: safety againt obstacles metric. reflects the number of
%               agent-obstacle collisions
%   min_d_obs: minimum distance agent-obstacle
%

%% Init variables

[t_steps,nx] = size(pos_history);
nb_agents = nx/3;
M = zeros(t_steps,nb_agents,nb_agents);
nb_ag_coll = zeros(t_steps,1);
nb_obs_coll = zeros(t_steps,1);
nb_conn_comp = zeros(t_steps,1);

safety = ones(t_steps,1);
order = zeros(t_steps,1);
union = zeros(t_steps,1);
alg_conn = zeros(t_steps,1);
safety_obs = zeros(t_steps,1);
min_d_obs = zeros(t_steps,nb_agents);

%% Loop over time

for k = 1:t_steps

    %% Safety: reflects the number of collisions among the swarm agents

    pos_k = pos_history(k,:);
    pos_k = reshape(pos_k,3,[]);
    dist_vect_k = pdist(pos_k');
    if ~isempty(dist_vect_k)
        nb_ag_coll(k) = sum(dist_vect_k < 2*p_swarm.r_coll);
        safety(k) = 1 - (sum(nb_ag_coll(k)) / length(dist_vect_k));
    else
        nb_ag_coll(k)=0;
        safety(k) = 1;
    end

    %% Order: reflects the correlation of the velocity vectors
    distance_matrix_k = squareform(dist_vect_k);
    M(k,:,:) =  compute_neighborhood(distance_matrix_k, p_swarm.r, p_swarm.max_neig);
    for agent = 1:nb_agents
        neig = (M(k,:,agent)==true);
        nn = sum(neig);
        vel_k = vel_history(k,:);
        vel_k = reshape(vel_k,3,[]);
        if nn ~= 0
            scalar_prod = vel_k(:, agent)' * vel_k(:, neig);
            speed_agent = norm(vel_k(:, agent));
            speed_neig = sqrt(sum((vel_k(:, neig) .^ 2), 1));
            order(k) = order(k) + ...
                sum (scalar_prod ./ (speed_agent * speed_neig)) / nn;
        else
            order(k) = order(k) + 1;
        end
    end
    order(k) = order(k)/nb_agents;

    %% Union: reflects the number of connected components in the related
    % symeetric graph

    M_k = squeeze(M(k,:,:));
    A = ((M_k + M_k') > 0);
    [nb_conn_comp(k), ~, ~]  = network_components(A);
    union(k) = (nb_agents - nb_conn_comp(k))/(nb_agents-1);

    %% Connectivity: reflects the algebraic connectivity in the related
    % symmetric graph

    alg_conn(k) = compute_alg_connectivity(A)/nb_agents;

    %% Safety with obstacles: reflects the number of collisions between swarm agents and obstacles

    nb_possible_coll = 0;
    if p_swarm.is_active_spheres | p_swarm.is_active_cyl | p_swarm.is_active_arena

        pos_k = pos_history(k,:);
        pos_k = reshape(pos_k,3,[]);

        if p_swarm.is_active_spheres
            c_spheres = p_swarm.spheres(1:3,:);
            r_spheres = p_swarm.spheres(4,:);

            D_spheres = pdist2(pos_k',c_spheres');
            min_d_obs(k,:) = min(pdist2(pos_k(1:2,:)',c_spheres') - repmat(r_spheres, nb_agents, 1),[],2);
            nb_obs_coll(k) = nb_obs_coll(k) + sum(sum(D_spheres < repmat(r_spheres, nb_agents, 1)));
            nb_possible_coll = nb_possible_coll + nb_agents*length(r_spheres);
        end
        if p_swarm.is_active_cyl
            c_cyl = p_swarm.cylinders(1:2,:);
            r_cyl = p_swarm.cylinders(3,:);

            D_cyl = pdist2(pos_k(1:2,:)',c_cyl');
            min_d_obs(k,:) = min(pdist2(pos_k(1:2,:)',c_cyl') - repmat(r_cyl, nb_agents, 1),[],2);
            nb_obs_coll(k) = nb_obs_coll(k) + sum(sum(D_cyl < repmat(r_cyl, nb_agents, 1)));
            nb_possible_coll = nb_possible_coll + nb_agents*length(r_cyl);
        end
        if p_swarm.is_active_arena
            x_coll = sum(pos_k(1,:)< p_swarm.x_arena(1,1) | pos_k(1,:)> p_swarm.x_arena(1,2));
            y_coll = sum(pos_k(2,:)< p_swarm.x_arena(2,1) | pos_k(2,:)> p_swarm.x_arena(2,2));
            z_coll = sum(pos_k(3,:)< p_swarm.x_arena(3,1) | pos_k(3,:)> p_swarm.x_arena(3,2));
            nb_obs_coll(k) = nb_obs_coll(k) + x_coll + y_coll + z_coll;
            nb_possible_coll = nb_possible_coll + nb_agents;
        end

        safety_obs(k) = 1 - (nb_obs_coll(k)/nb_possible_coll);
    end

end

%% Save workspace

if ~isempty(dirname)
    path = strcat(dirname,'/performance');
    save(path);
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function connectivity = compute_alg_connectivity(A)
% Compute alg connectivity - compute algebraic connectivity of the graph
% associated to the swarm at a given time step.

D = diag(sum(A, 2)); % degree matrix
L = D - A;           % laplacian matrix
eigenvalues = eig(L);
eigenvalues = sort(eigenvalues);
connectivity = eigenvalues(2);

end


function results = PTKComputeVolumeFromSegmentation(segmentation_mask, reporting)
    % PTKComputeVolumeFromSegmentation. Computes volume and surface area from masks
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2013.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.

    voxel_size = segmentation_mask.VoxelSize;

    voxel_volume_mm3 = voxel_size(1) * voxel_size(2) * voxel_size(3);
    lung_volume_mm3 = sum(segmentation_mask.RawImage(:) > 0)*voxel_volume_mm3;

    surface = PTKGetSurfaceFromSegmentation(segmentation_mask.RawImage);
    surface_volume_mm3 = sum(surface(:))*voxel_volume_mm3;

    results = PTKMetrics;
    results.AddMetric('VolumeCm3', lung_volume_mm3/1000, 'Volume (cm^3)');
    results.AddMetric('SurfaceVolumeCm3', surface_volume_mm3/1000, 'Volume of surface voxels (cm^3)');
end


% poolType = 'mean' or 'max'
% poolLayer = integer layer number after which we do the pooling
function layer = genNetworkTemporalConvDeep(para)
if isfield(para, 'LastActivation4MSE')==0
    para.LastActivation4MSE = 'linear';
end
inputDim = double(para.inputDim);

layer{1}.name = 'Input';        % this is an input layer
layer{end}.inputIdx = 1;    % specifies the index of GCC in Visible_tr
layer{end}.dim = [1 1]*inputDim;             % [input dim; output dim];

nFilter = para.nFilter;
for i=1:length(nFilter)
    if i==1 % only the first layer will use tconv, the rest uses splice and affine transform
        layer{end+1}.name = 'tconv';
        layer{end}.prev = -1;
        layer{end}.W = []; % to be initialized randomly or by pretraining
        layer{end}.b = [];
        layer{end}.dim = [nFilter(i) inputDim*para.filterLen(i)];
    else
        layer{end+1}.name = 'Splice';
        layer{end}.prev = -1;
        layer{end}.context = para.filterLen(i);
        layer{end}.dim = [layer{end}.context 1]*layer{length(layer)+layer{end}.prev}.dim(1);
        layer{end}.update = 0;

        layer{end+1}.name = 'Affine';
        layer{end}.prev = -1;
        layer{end}.W = []; % to be initialized randomly or by pretraining
        layer{end}.b = [];
        layer{end}.dim = [nFilter(i) layer{length(layer)+layer{end}.prev}.dim(1)];
    end
    layer{end}.update = 1;

    layer{end+1}.name = 'tmaxpool';
    layer{end}.context = para.poolingLen(i);
    layer{end}.stride = para.poolingStride(i);
    layer{end}.prev = -1;
    layer{end}.dim = [1 1]*nFilter(i);

    if isfield(para, 'activation')
        layer{end+1}.name = para.activation;
    else
        layer{end+1}.name = 'sigmoid';
    end
    layer{end}.prev = -1;
    layer{end}.dim = [1 1]*nFilter(i);
end

layer2 = genNetworkFeedForward_v2(nFilter(end), para.hiddenLayerSizeFF, para.outputDim, para.costFn, para.LastActivation4MSE);

layer = [layer layer2(2:end)];
layer = FinishLayer(layer);

end


classdef SumOverDim < dagnn.ElementWise
  % @author: Hakan Bilen
  % SumOverDim is the sum of the elements of inputs{1} over dimension dim
  properties
    dim = 3;
  end

  methods
    function outputs = forward(obj, inputs, params)
      outputs{1} = sum(inputs{1},obj.dim) ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)

      ndims = ones(1,numel(size(inputs{1})));
      ndims(obj.dim) = size(inputs{1},obj.dim);
      derInputs{1} = repmat(derOutputs{1},ndims);

      derParams = {} ;
    end

    function outputSizes = getOutputSizes(obj, inputSizes)
      outputSizes{1} = inputSizes{1} ;
      outputSizes{1}(obj.dim) = 1;
    end

    function obj = SumOverDim(varargin)
      obj.load(varargin) ;
      obj.dim = obj.dim;
    end
  end
end


% motorseed comparison
% Perform regression of motor traces (seed or raw) with all cells.
% Histograms of regression coefficients (red: motorseed; gray: raw traces)
% show that for each of all fish, regression with motor seeds yields higher
% coefficients for the population.
% not surprising, but shows that (even if a bit circular), the motorseeds
% have a better chance of capturing motor related neurons.

%%
hfig = figure;
InitializeAppData(hfig);
ResetDisplayParams(hfig);

%%
binEdges = -1:0.05:1;
nbins = length(binEdges)-1;
N_hist_notseed = zeros(18,nbins,2);
N_hist_seed = zeros(18,nbins,2);
for i_fish = 1:18
    ClusterIDs = [2,1];
    [cIX,gIX,M,stim,behavior,M_0] = LoadSingleFishDefault(i_fish,hfig,ClusterIDs);

    %%
    isMotorseed = 0;
    setappdata(hfig,'isMotorseed',isMotorseed);
    [~,~,behavior] = UpdateTimeIndex(hfig);

    [~,~,regressor_m,names_m] = GetMotorRegressor(behavior,i_fish);
    if isMotorseed
        Reg = regressor_m;
    else
        Reg = regressor_m([1,3],:);
    end
    % regression
    Corr = corr(Reg',M_0');
    for i_reg = 1:2
        R = Corr(i_reg,:);
        [N,~] = histcounts(R,binEdges);
        N_hist_notseed(i_fish,:,i_reg) = N;
    end
    %%
    isMotorseed = 1;
    setappdata(hfig,'isMotorseed',isMotorseed);
    [~,~,behavior] = UpdateTimeIndex(hfig);

    [~,~,regressor_m,names_m] = GetMotorRegressor(behavior,i_fish);
    if isMotorseed
        Reg = regressor_m;
    else
        Reg = regressor_m([1,3],:);
    end
    % regression
    Corr = corr(Reg',M_0');
    for i_reg = 1:2
        R = Corr(i_reg,:);
        [N,~] = histcounts(R,binEdges);
        N_hist_seed(i_fish,:,i_reg) = N;
    end
end

%%
figure;
cmap = [1 0.4 0.4;0.4 0.4 0.4];
range_fish = 1:18;
m = length(range_fish);
N_type = cell(1,2);
N_type{1} = N_hist_seed;
N_type{2} = N_hist_notseed;
% i_plot = 0;

% left half
i_reg = 1;
for i_fish = range_fish
    for i_type = 1:2
        i_plot = 1+2*(i_fish-1);%i_plot+1;
        subplot(m,2,i_plot);
        hold on;

        counts = squeeze(N_type{i_type}(i_fish,:,i_reg));
        histogram('BinEdges',binEdges,'BinCounts',counts,'FaceColor',cmap(i_type,:))

        %     ymax = max(max(N),max(N_shf));
        %     plot([0,0],[0,ymax],'r--');
        ymax = counts(24)+eps;
        xlim([-1,1]);
        ylim([0,ymax]);
        set(gca,'YTick',[])
        ylabel('a.u.')
        title(['Fish ' num2str(i_fish)])
    end
    end

% right half
i_reg = 2;
for i_fish = range_fish
    for i_type = 1:2
        i_plot = 2*i_fish;%i_plot+1;
        subplot(m,2,i_plot);
        hold on;

        counts = squeeze(N_type{i_type}(i_fish,:,i_reg));
        histogram('BinEdges',binEdges,'BinCounts',counts,'FaceColor',cmap(i_type,:))

        %     ymax = max(max(N),max(N_shf));
        %     plot([0,0],[0,ymax],'r--');
        ymax = counts(24)+eps;
        xlim([-1,1]);
        ylim([0,ymax]);
        set(gca,'YTick',[])
        ylabel('a.u.')
        title(['Fish ' num2str(i_fish)])
    end
end


function [ T ] = computeTrans( imgs )
Thresh = 10;
confidence = 0.99;
inlierRatio = 0.3;
epsilon = 1.5;

nImgs = size(imgs, 4);

T = zeros(3, 3, nImgs);
T(:, :, 1) = eye(3);
[f2, d2] = getSIFTFeatures(imgs(:, :, :, 1), Thresh);
for i = 2 : nImgs
    f1 = f2;
    d1 = d2;
    [f2, d2] = getSIFTFeatures(imgs(:, :, :, i), Thresh);
    [matches, ~] = getMatches(f1, d1, f2, d2);
    [T(:, :, i),~] = RANSAC(confidence, inlierRatio, 1, matches, epsilon);
end
end


  function groups = group2d(nx, ny, ngv, mask, chat)
%|function groups = group2d(nx, ny, ngv, mask, chat)
%| build groups for GCA for 2d case
%| ngv [2]	[ngx ngy]
%|
%| Copyright Mar. 1999, Jeff Fessler

if 1 ~= exist('chat'), chat = 1; end
if 1 ~= exist('mask') || isequal(mask, 1)
	mask = ones(nx,ny);
end

	ngx = ngv(1);
	ngy = ngv(2);

	np = nx*ny;

	ng = ngx*ngy;
	groups	= logical(zeros(np, ng));
	for igx = 1:ngx
		for igy = 1:ngy
			ig = igx + (igy-1)*ngx;
			gx = [igx:ngx:nx]';
			gy = [igy:ngy:ny]';
			ggx = gx * ones(1,length(gy));
			ggy = ones(length(gx),1) * gy';
			gg = ggx + (ggy-1) * nx;
			gg = gg(:);
			groups(gg,ig) = ones(size(gg));
		end
	end

	groups = groups(find(mask(:)),:);

	% look at groups
	if chat
		im(groups', 'groups')
		for ig=1:ng
			prompt
			im(embed(groups(:,ig), mask), sprintf('group %d', ig))
		end
		prompt
		im(embed(groups * [1:ng]',mask), 'All Groups')
	end

	if any(sum(groups')) > 1, error 'overlap groups', end


classdef circularGraph < handle
% CIRCULARGRAPH Plot an interactive circular graph to illustrate connections in a network.
%
%% Syntax
% circularGraph(X)
% circularGraph(X,'PropertyName',propertyvalue,...)
% h = circularGraph(...)
%
%% Description
% A 'circular graph' is a visualization of a network of nodes and their
% connections. The nodes are laid out along a circle, and the connections
% are drawn within the circle. Click on a node to make the connections that
% emanate from it more visible or less visible. Click on the 'Show All'
% button to make all nodes and their connections visible. Click on the
% 'Hide All' button to make all nodes and their connections less visible.
%
% Required input arguments.
% X : A symmetric matrix of numeric or logical values.
%
% Optional properties.
% Colormap : A N by 3 matrix of [r g b] triples, where N is the
%            length(adjacenyMatrix).
% Label    : A cell array of N strings.
%%
% Copyright 2014 The MathWorks, Inc.
  properties
    Node = node(0,0); % Array of nodes
    ColorMap;         % Colormap
    Label;            % Cell array of strings
    ShowButton;       % Turn all nodes on
    HideButton;       % Turn all nodes off
  end

  methods
    function this = circularGraph(adjacencyMatrix,varargin)
      % Constructor
      p = inputParser;

      defaultColorMap = parula(length(adjacencyMatrix));
      defaultLabel = cell(length(adjacencyMatrix));
      for i = 1:length(defaultLabel)
        defaultLabel{i} = num2str(i);
      end

      addRequired(p,'adjacencyMatrix',@(x)(isnumeric(x) || islogical(x)));
      addParameter(p,'ColorMap',defaultColorMap,@(colormap)length(colormap) == length(adjacencyMatrix));
      addParameter(p,'Label'   ,defaultLabel   ,@iscell);

      parse(p,adjacencyMatrix,varargin{:});
      this.ColorMap = p.Results.ColorMap;
      this.Label    = p.Results.Label;

      this.ShowButton = uicontrol(...
        'Style','pushbutton',...
        'Position',[0 40 80 40],...
        'String','Show All',...
        'Callback',@circularGraph.showNodes,...
        'UserData',this);

      this.HideButton = uicontrol(...
        'Style','pushbutton',...
        'Position',[0 0 80 40],...
        'String','Hide All',...
        'Callback',@circularGraph.hideNodes,...
        'UserData',this);

      fig = gcf;
      set(fig,...
        'UserData',this,...
        'CloseRequestFcn',@circularGraph.CloseRequestFcn);

      % Draw the nodes
      delete(this.Node);
      t = linspace(-pi,pi,length(adjacencyMatrix) + 1).'; % theta for each node
      extent = zeros(length(adjacencyMatrix),1);
      for i = 1:length(adjacencyMatrix)
        this.Node(i) = node(cos(t(i)),sin(t(i)));
        this.Node(i).Color = this.ColorMap(i,:);
        this.Node(i).Label = this.Label{i};
      end

      % Find non-zero values of s and their indices
      [row,col,v] = find(adjacencyMatrix);

      % Calculate line widths based on values of s (stored in v).
      minLineWidth  = 0.5;
      lineWidthCoef = 5;
      lineWidth = v./max(v);
      if sum(lineWidth) == numel(lineWidth) % all lines are the same width.
        lineWidth = repmat(minLineWidth,numel(lineWidth),1);
      else % lines of variable width.
        lineWidth = lineWidthCoef*lineWidth + minLineWidth;
      end

      % Draw connections on the Poincare hyperbolic disk.
      %
      % Equation of the circles on the disk:
      % x^2 + y^2
      % + 2*(u(2)-v(2))/(u(1)*v(2)-u(2)*v(1))*x
      % - 2*(u(1)-v(1))/(u(1)*v(2)-u(2)*v(1))*y + 1 = 0,
      % where u and v are points on the boundary.
      %
      % Standard form of equation of a circle
      % (x - x0)^2 + (y - y0)^2 = r^2
      %
      % Therefore we can identify
      % x0 = -(u(2)-v(2))/(u(1)*v(2)-u(2)*v(1));
      % y0 = (u(1)-v(1))/(u(1)*v(2)-u(2)*v(1));
      % r^2 = x0^2 + y0^2 - 1

      for i = 1:length(v)
        if row(i) ~= col(i)
          if abs(row(i) - col(i)) - length(adjacencyMatrix)/2 == 0
            % points are diametric, so draw a straight line
            u = [cos(t(row(i)));sin(t(row(i)))];
            v = [cos(t(col(i)));sin(t(col(i)))];
            this.Node(row(i)).Connection(end+1) = line(...
              [u(1);v(1)],...
              [u(2);v(2)],...
              'LineWidth', lineWidth(i),...
              'Color', this.ColorMap(row(i),:),...
              'PickableParts','none');
          else % points are not diametric, so draw an arc
            u  = [cos(t(row(i)));sin(t(row(i)))];
            v  = [cos(t(col(i)));sin(t(col(i)))];
            x0 = -(u(2)-v(2))/(u(1)*v(2)-u(2)*v(1));
            y0 =  (u(1)-v(1))/(u(1)*v(2)-u(2)*v(1));
            r  = sqrt(x0^2 + y0^2 - 1);
            thetaLim(1) = atan2(u(2)-y0,u(1)-x0);
            thetaLim(2) = atan2(v(2)-y0,v(1)-x0);

            if u(1) >= 0 && v(1) >= 0
              % ensure the arc is within the unit disk
              theta = [linspace(max(thetaLim),pi,50),...
                       linspace(-pi,min(thetaLim),50)].';
            else
              theta = linspace(thetaLim(1),thetaLim(2)).';
            end

            this.Node(row(i)).Connection(end+1) = line(...
              r*cos(theta)+x0,...
              r*sin(theta)+y0,...
              'LineWidth', lineWidth(i),...
              'Color', this.ColorMap(row(i),:),...
              'PickableParts','none');
          end
        end
      end

      axis image;
      ax = gca;
      for i = 1:length(adjacencyMatrix)
        extent(i) = this.Node(i).Extent;
      end
      extent = max(extent(:));
      ax.XLim = ax.XLim + extent*[-1 1];
      fudgeFactor = 1.75; % Not sure why this is necessary. Eyeballed it.
      ax.YLim = ax.YLim + fudgeFactor*extent*[-1 1];
      ax.Visible = 'off';
      ax.SortMethod = 'depth';

      fig = gcf;
      fig.Color = [1 1 1];
    end

  end

  methods (Static = true)
    function showNodes(this,~)
      % Callback for 'Show All' button
      n = this.UserData.Node;
      for i = 1:length(n)
        n(i).Visible = true;
      end
    end

    function hideNodes(this,~)
      % Callback for 'Hide All' button
      n = this.UserData.Node;
      for i = 1:length(n)
        n(i).Visible = false;
      end
    end

    function CloseRequestFcn(this,~)
      % Callback for figure CloseRequestFcn
      c = this.UserData;
      for i = 1:length(c.Node)
        delete(c.Node(i));
      end
      delete(gcf);
    end

  end

end


function linpack_c_test17 ( )

%*****************************************************************************80
%
%% TEST17 tests CPBCO.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2009
%
%  Author:
%
%    John Burkardt
%
  m = 1;
  n = 3;
  lda = m + 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST17\n' );
  fprintf ( 1, '  For a single precision complex (C)\n' );
  fprintf ( 1, '  positive definite hermitian band matrix (PB),\n' );
  fprintf ( 1, '  CPBCO estimates the reciprocal condition number.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Set the value of the superdiagonal and diagonal.
%
  a(1,1) = complex ( 0.0000,  0.0000 );
  a(1,2) = complex ( 2.1341, -0.2147 );
  a(1,3) = complex ( 2.0905,  1.1505 );

  a(2,1) = complex ( 4.5281,  0.0000 );
  a(2,2) = complex ( 5.0371,  0.0000 );
  a(2,3) = complex ( 4.7638,  0.0000 );
%
%  Estimate the condition.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimate the condition.\n' );

  [ a, rcond, info ] = cpbco ( a, lda, n, m );

  if ( info ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  CPBCO returned INFO = %d\n', info );
    fprintf ( 1, '  The factorization was not completed.\n' );
    return
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Reciprocal condition  = %f\n', rcond );

  return
end


function [cut, map, short] = vl_aibcut(parents, n)
% VL_AIBCUT  Cut VL_AIB tree
%  CUT = VL_AIBCUT(PARENTS, N) cuts the binary merge tree PARENTS and
%  returns a cut CUT of N nodes. The format of PARENTS is the same
%  used by the VL_AIB() function.
%
%  A cut is a set of N nodes such that no node is a descendant of any
%  other node in the CUT and such that any leaf descend from a node in
%  the cut. CUT lists the nodes of the binary merge tree PARENT that
%  form the cut.
%
%  Nodes with null parent (as defined by PARENTS) are comprised in the
%  cut if the other nodes are not enough to fill a cut of N elements.
%
%  [CUT, MAP] = VL_AIBCUT(...) returns a vector MAP with the same size as
%  PARENTS. MAP assign each node below or in the cut to the
%  corresponding element in the CUT vector (each element above the cut
%  or with null parent is mapped to 0). To get the index of the
%  corresponding cut nodes use CUT(MAP). MAP by itself is useful to
%  quantize the leaves in a sequences of N contiguous indexes,
%  starting from one (see also VL_AIBCUTPUSH()).
%
%  [CUT, MAP, SHORT] = VL_AIBCUT(...) returns also a vector SHORT that
%  represents a merge tree compressed to the cut. This is obtained by
%  mapping all nodes below to the cut element above them. Nodes in or
%  above the cut are mapped to themselves. Null parents are left
%  unchanged, except if the corresponding node is in the cut (in which
%  case the map-to-itself rule has the precedence).
%
%  See also: VL_HELP(), VL_AIB().

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

% --------------------------------------------------------------------
%                                           Determine nodes in the cut
% --------------------------------------------------------------------

if n > 1
  root    = max(parents) ;

  % count number of null nodes
  z = sum(parents(1:root) == 0) ;

  % determine number of leves
  nleaves = (root - z + 1) / 2 ;

  % find first node of the cut
  mu   = root - min(n, nleaves) + 1 ;

  % correction for presence of null nodes
  nz   = find(parents(1:mu) > 0) ;
  mu   = nz(end) ;

  % find node belnoging to the cut
  cut  = find(parents(1:mu) > mu) ;

  % In the presence of null nodes, the cut size might exceed
  % nleaves, which is the maximum cut size we can obtain with the
  % specified tree. The additional nodes have to be picked up from
  % the null nodes.

  if length(cut) < n
    sel_z = find(parents == 0) ;
    cut = [sel_z(1:n-length(cut)) cut] ;
  end

  % aesthetic reasons only
  cut = sort(cut) ;

else
  mu   = max(parents) ;
  cut  = mu ;
end

% --------------------------------------------------------------------
%                                       Short-circuit nodes to the cut
% --------------------------------------------------------------------

stop = [cut find(parents == 0)] ;
short = 1:length(parents) ;

while 1
  [drop,sel] = setdiff(short(1:mu), stop)  ;
  sel = setdiff(sel, stop) ;
  if isempty(sel), break ; end
  short(sel) = parents(short(sel))  ;
end

short(setdiff(find(parents == 0), cut)) = 0 ;

% --------------------------------------------------------------------
%                                                  Build quantizer map
% --------------------------------------------------------------------

map             = 1:numel(parents) ;
map(cut)        = 1:n ;
map(short >  0) = map(short(short > 0)) ;
map(short == 0) = 0 ;
map(mu+1:end)   = 0 ;


function plot_subdivided(V,F,u,k)
%PLOT_SUBDIVIDED Plot the function u on the mesh V,F which has subdivided,
%with Loop subdivision, k times
%
% plot_subdivided(V,F,u,k);
%
% Inputs:
%  V,F  the input mesh to be subdivided
%  u  the input function to be subdivided along with the mesh
%  k  how many times to subdivide
%

Vu = ...
Fu = ...
uu = ...

t = tsurf(Fu,Vu, 'CData',uu);
shading interp;
axis equal;
axis off;
colormap(cbrewer('Blues', 500));
light('Position',[-1.5 1 1],'Style','local');
lights = camlight;
set(t, 'FaceLighting','gouraud', 'FaceColor','interp');
set(t, 'DiffuseStrength',0.5, 'SpecularStrength',0.2, 'AmbientStrength',0.3);
camproj('perspective');
add_shadow([t],lights);

end


%  Figure 7.34      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
%% fig7_34.m
%% SRL for estimator design
clf;
num=[1];
den=conv([1 0 1],[1 0 1]);
rlocus(num,den);
grid;
title('Fig. 7.34: SRL for Estimator Design');
text(0,0.9,'q --> 0');
text(0,-0.9,'q --> 0');
text(-5,4,'q-->\infty');
text(-5,-4,'q-->\infty');


function value = r8_cos ( x )

%*****************************************************************************80
%
%% R8_COS evaluates the cosine of an R8 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the cosine of X.
%
  persistent ntsn
  persistent pi2
  persistent pi2rec
  persistent pihi
  persistent pilo
  persistent pirec
  persistent sincs
  persistent xmax
  persistent xsml
  persistent xwarn
%
%  pihi + pilo = pi.  pihi is exactly representable on all machines
%  with at least 8 bits of precision.  whether it is exactly
%  represented depends on the compiler.  this routine is more
%  accurate if it is exactly represented.
%
  pi2 = 1.57079632679489661923132169163975;
  pi2rec = 0.63661977236758134307553505349006;
  pihi = 3.140625;
  pilo = 9.6765358979323846264338327950288E-04;
  pirec = 0.31830988618379067153776752674503;

  if ( isempty ( ntsn ) )

    sincs = [ ...
      -0.374991154955873175839919279977323464, ...
      -0.181603155237250201863830316158004754, ...
      +0.005804709274598633559427341722857921, ...
      -0.000086954311779340757113212316353178, ...
      +0.000000754370148088851481006839927030, ...
      -0.000000004267129665055961107126829906, ...
      +0.000000000016980422945488168181824792, ...
      -0.000000000000050120578889961870929524, ...
      +0.000000000000000114101026680010675628, ...
      -0.000000000000000000206437504424783134, ...
      +0.000000000000000000000303969595918706, ...
      -0.000000000000000000000000371357734157, ...
      +0.000000000000000000000000000382486123, ...
      -0.000000000000000000000000000000336623, ...
      +0.000000000000000000000000000000000256 ]';

    ntsn = r8_inits ( sincs, 15, 0.1 * r8_mach ( 3 ) );
    xsml = sqrt ( 2.0 * r8_mach ( 3 ) );
    xmax = 1.0 / r8_mach ( 4 );
    xwarn = sqrt ( xmax );

  end

  absx = abs ( x );
  y = absx + pi2;

  if ( xmax < y )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_COS - Warning!\n' );
    fprintf ( 1, '  No precision because |X| is big.\n' );
    value = 0.0
    return
  end

  if ( xwarn < y )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_COS - Warning!\n' );
    fprintf ( 1, '  Answer < half precision because |X| is big.\n' );
  end

  value = 1.0;

  if ( absx < xsml )
    return
  end

  xn = r8_aint ( y * pirec + 0.5 );
  n2 = r8_aint ( mod ( xn, 2.0 ) + 0.5 );
  xn = xn - 0.5;
  f = ( absx - xn * pihi ) - xn * pilo;

  xn = 2.0 * ( f * pi2rec ) * ( f * pi2rec ) - 1.0;

  value = f + f * r8_csevl ( xn, sincs, ntsn );

  if ( n2 ~= 0 )
    value = - value;
  end

  if ( value < -1.0 )
    value = -1.0;
  elseif ( 1.0 < value )
    value = 1.0;
  end

  return
end


function rs = diff(s, nth)

%tstoolbox/@signal/diff
%   Syntax:
%     * diff(s, nth)
%
%   Compute the nth numerical derivative along dimension 1. s has be to
%   sampled equidistantly.
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

narginchk(1,2);

if nargin < 2
	nth = 1;
end

a = getaxis(s,1);
switch  resolution(a)
	case 'linear'
		c = diff(s.core, nth, delta(a)); 		% call real working routine for parent core object
		rs = signal(c, s);				% special constructor calling syntax for working routines
		rs = setyunit(rs, yunit(s)/(unit(a)^nth));
		a = setfirst(a, first(a) + nth*delta(a)/2);
		rs = setaxis(rs,  1, a);
		rs = addhistory(rs,  ['Computed ' num2str(nth) '# numerical derivative along dimension 1'] );
		rs = addcommandlines(rs, 's = diff(s', nth);

        otherwise
	   x=spacing(s)';
	   x2=x(1:end-1);
	   x=x(2:end);

	   y=data(s);
	   y2=y(1:end-1,:);
	   y=y(2:end,:);
	   y=(y-y2);
	   for i=1:length(y(1,:))
	     y(:,i)=y(:,i)./(x-x2);
	   end
	   x=(x-x2)/2+x2;

	   rs=signal(core(y),s);
	   rs=setplothint(rs,'multigraph');
	   rs = setyname(rs, 'd ld N(r)/d lg r');
	   rs = setyunit(rs, yunit(s)/(unit(a)^nth));
	   rs = setaxis(rs,  1, achse(x));
	   rs = addhistory(rs,  ['Computed ' num2str(nth) '# numerical derivative along dimension 1'] );
	   rs = addcommandlines(rs, 's = diff(s', nth);

%	   error('Data values are not sampled equidistantly');

end


function pick = nms_matlab(boxes, overlap)
% top = nms_fast(boxes, overlap)
% Non-maximum suppression. (FAST VERSION)
% Greedily select high-scoring detections and skip detections
% that are significantly covered by a previously selected
% detection.
% NOTE: This is adapted from Pedro Felzenszwalb's version (nms.m),
% but an inner loop has been eliminated to significantly speed it
% up in the case of a large number of boxes
% Tomasz Malisiewicz (tomasz@cmu.edu)

if isempty(boxes)
    pick = [];
    return;
end

x1 = boxes(:,1);
y1 = boxes(:,2);
x2 = boxes(:,3);
y2 = boxes(:,4);
s = boxes(:,end);

area = (x2-x1+1) .* (y2-y1+1);
[vals, I] = sort(s);

pick = s*0;
counter = 1;
while ~isempty(I)

    last = length(I);
    i = I(last);
    pick(counter) = i;
    counter = counter + 1;

    xx1 = max(x1(i), x1(I(1:last-1)));
    yy1 = max(y1(i), y1(I(1:last-1)));
    xx2 = min(x2(i), x2(I(1:last-1)));
    yy2 = min(y2(i), y2(I(1:last-1)));

    w = max(0.0, xx2-xx1+1);
    h = max(0.0, yy2-yy1+1);

    o = w.*h ./ area(I(1:last-1));

    I([last; find(o>overlap)]) = [];
end

pick = pick(1:(counter-1));


function [ptrace, ptrace_n,...
    idx_sort1, idx_sort2, idx_match1, idx_match2] = set_matching_figure(cgk1, cgk2, flag_figure)

% assume cgk1,2 already taken intersection
if ~exist('flag_figure', 'var')
    flag_figure = 1;
end

% set matching/covering index
M = set_match_count(cgk1, cgk2);
[n1, n2] = size(M);
din = sum(M,2);
din(din==0) = 1;
%         dout = sum(M,1);
%         dout(dout==0) = 1;
M1 = M./ repmat(din, [1, n2]);
% M1b = M./ repmat(dout, [n1, 1]);

% iterative sorting...
Msort = M;
idx1 = 1:n1;
idx2 = 1:n2;
flag_converge = 0;
for k = 1: 1000
    [~, idsort2] = sortMn_column(Msort);
    Msort = Msort(:, idsort2);
    idx2 = idx2(idsort2);
    [~, idsort1] = sortMn_column(Msort');
    Msort = Msort(idsort1, :);
    idx1 = idx1(idsort1);

    if sum(abs(idsort2-(1:n2)))<=2 && sum(abs(idsort1-(1:n1))) <= 2
        flag_converge = 1;
        break;
    end
end
if flag_converge == 0
    display(' sorting does not converge');
end

M2 = M1(idx1, idx2);
M2n = M(idx1, idx2);

if flag_figure == 1
    fig1 = figure;
    subplot(121);
    imagesc(M2);
    colormap(bluewhitered);
end

% matching
[assignment, ~] = munkres(-M2n); % based on count
idx3 = 1:n1;
M3 = M2(idx3(assignment>0), assignment(assignment>0));
M3n = M2n(idx3(assignment>0), assignment(assignment>0));

ptrace = trace(M3)/sum(sum(M3));
ptrace_n = trace(M3n)/sum(sum(M3n));

if flag_figure == 1
    figure(fig1);
    subplot(122);
    imagesc(M3);
    colormap(bluewhitered);
    axis equal
    axis tight
    % title([num2str(ptrace), ', ',...
    %     num2str(ptrace_n), ', ', num2str(size(M3,1))])
end

idx_sort1 = idx1;
idx_sort2 = idx2;
idx_match1 = idx1(idx3(assignment>0));
idx_match2 = idx2(assignment(assignment>0));
end


function ierror = perm1_check ( n, p )

%*****************************************************************************80
%
%% PERM1_CHECK checks a permutation of (1,...,N).
%
%  Discussion:
%
%    The routine verifies that each of the integers from 1 to
%    to N occurs among the N entries of the permutation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 May 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries.
%
%    Input, integer P(N), the array to check.
%
%    Output, integer IERROR:
%    0, P is a legal permutation of (1,...,N).
%    1, P is not a legal permutation of (1,...,N).
%
  ierror = 0;

  for value = 1 : n

    ierror = 1;

    for location = 1 : n
      if ( p(location) == value )
        ierror = 0;
        break;
      end
    end

    if ( ierror ~= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'PERM1_CHECK - Warning!\n' );
      fprintf ( 1, '  Permutation is missing the value %d.\n', value );
      break;
    end

  end

  return
end


function evaluate_clustering(name, embedding_dimension, distance)

if nargin < 1
    name = 'liftedstructsim_softmax_pair_m128_multilabel';
end

if nargin < 2
    embedding_dimension = 64;
end

if nargin < 3
    distance = 'euclidean';
end

K = 11316;
filename = sprintf('idx_kmeans_googlenet_%s_embed%d_%s.mat', name, embedding_dimension, distance);
if exist(filename, 'file')
    object = load(filename);
    idx = object.idx;
else
    % compute similarity
    load(sprintf('validation_googlenet_feat_matrix_%s_embed%d_baselr_1E4_gaussian2k.mat', ...
                                        name, embedding_dimension), 'fc_embedding');
    X = double(fc_embedding');

    % kmeans clustering
    fprintf('2d kmeans %d\n', K);
    opts = struct('maxiters', 1000, 'mindelta', eps, 'verbose', 1);
    [center, sse] = vgg_kmeans(X, K, opts);
    [idx_kmeans, d] = vgg_nearest_neighbour(X, center);

    % construct idx
    num = size(X, 2);
    idx = zeros(num, 1);
    for i = 1:K
        index = find(idx_kmeans == i);
        [~, ind] = min(d(index));
        cid = index(ind);
        idx(index) = cid;
    end

    fprintf('Number of clusters: %d\n', length(unique(idx)));
    save(sprintf('idx_kmeans_googlenet_%s_embed%d_%s.mat', name, embedding_dimension, distance), 'idx');
end

% evaluation
num_validation_classes = K;

% load ground truth from filenames
[image_ids, class_ids, superclass_ids, path_list] = ...
    textread('/cvgl/group/Ebay_Dataset/Ebay_test.txt', '%d %d %d %s',...
    'headerlines', 1);

num = numel(class_ids);
item_ids = cell(num,1);
for i = 1:num
    class_id = class_ids(i);
    item_ids{i} = num2str(class_id);
end
assert(length(unique(item_ids)) == num_validation_classes);

% Given cluster assignment and the class names
%   Compute the three clustering metrics.
[NMI, RI, F1] = compute_clutering_metric(idx, item_ids);

fprintf('[method: %s, distance: %s] NMI: %.3f, RI: %.3f, F1: %.3f\n\n', ...
    name, distance, NMI, RI, F1);


% WAVELET_FACTORY_1D Create wavelet cascade
%
% Usage
%    [Wop, filters] = WAVELET_FACTORY_1D(N)
%
%    [Wop, filters] = WAVELET_FACTORY_1D(N, filt_opt)
%
%    [Wop, filters] = WAVELET_FACTORY_1D(N, filt_opt, scat_opt)
%
% Input
%    N (int): The size of the signals to be transformed.
%    filt_opt (struct): The filter options, same as FILTER_BANK.
%    scat_opt (struct): The scattering options, same as WAVELET_LAYER_1D.
%
% Output
%    Wop (cell): A cell array of wavelet layer transforms needed for the
%       scattering transform.
%    filters (cell): A cell array of the filters used in defining the
%       wavelets.
%
% Description
%    In order to calculate the scattering coefficients of a signal, a set of
%    filter banks need to be defined first. Given these filter banks, wavelet
%    transforms can be defined on scattering layers. To obtain the former,
%    WAVELET_FACTORY_1D calls the FILTER_BANK function using the parameters
%    provided in filt_opt, the result of which is returned as filters.
%    Each filter bank is then used to create a layer operator, using the
%    function WAVELET_LAYER_1D. This function takes a layer, a filter bank
%    and a set of parameters as input. WAVELET_FACTORY_1D fixes the filter
%    bank as to the element of filters corresponding to the layer order, while
%    scat_opt is used as the parameters. The result is a cell array Wop,
%    of layer operators which take one layer as input, and return two layers
%    as output, A and V corresponding to the "average" and "variation" of the
%    input layer.
%
% See also
%    WAVELET_LAYER_1D, WAVELET_1D, FILTER_BANK

function [Wop, filters] = wavelet_factory_1d(N, filt_opt, scat_opt)
	if nargin < 2
		filters = filter_bank(N);
    else
        filters = filter_bank(N, filt_opt);
    end

	if nargin < 3
		scat_opt = struct();
    end
    scat_opt = fill_struct(scat_opt, 'M', 2); % M is the scattering order

    Wop = cell(1,scat_opt.M);
	for m = 0:scat_opt.M
		filt_ind = min(numel(filters), 1+m);
		Wop{1+m} = @(X)(wavelet_layer_1d(X, filters{filt_ind}, scat_opt));
	end
end


function out = binsumcol(in, numinbin)
% Rebins image by summing numinbin bins together along columns of the
% 2D data.
% out = binsumcol(in, numinbin)

di=0;
if strcmp(class(in),'dip_image')
    di=1;
    in=double(in);
end
tmp = in;

for ii=2:numinbin
    tmp = tmp + circshift(in,[-1*(ii-1) 0]);
end

nb = floor(size(in,1)/numinbin);
ix=numinbin*(1:nb)-(numinbin-1);
out = tmp(ix,:);
if di
    out=dip_image(out);
end


%% Housekeeping
clear
close all
clc

%% read the models and their calibrations

m0=rise('frwz_nk');

%% solve the model

m=solve(m0,'steady_state_unique',true);

part_list={};

m2=solve(m0,'solve_perturbation_type',{'frwz',part_list});

%% print results
m.print_solution()

%% print solution for a subset of variables only
m.print_solution({'PAI','Y'})

%% Alternative calibrations

cal_2=struct('psi_a_2', 0.7);

m2=set(m,'parameters',cal_2);

%% construct a vector of models

bigm=[m,m2];

%% compute impulse responses for all models simultaneously

myirfs=irf(bigm,'irf_periods',20);

%% plot the impulse responses
close all
var_list=m.endogenous.name;
figure('name','Impulse responses to a monetary policy shock');
for ii=1:numel(var_list)
    subplot(3,1,ii)
    reg1=myirfs.EPS_R.regime_1.(var_list{ii});
    reg2=myirfs.EPS_R.regime_2.(var_list{ii});
    plot([reg1,reg2]);
    title(var_list{ii})
    if ii==1
        legend({'reg1-m1','reg1-m2','reg2-m1','reg2-m2'})
    end
    axis tight
end


function a = kershaw_llt ( a )

%*****************************************************************************80
%
%% KERSHAW_LLT returns the Cholesky factor of the KERSHAW matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real A(4,4), the matrix.
%
  a = [ ...
    1.732050807568877,  0.0,                0.0,                0.0; ...
   -1.154700538379252,  1.290994448735805,  0.0,                0.0; ...
                  0.0, -1.549193338482967,  0.774596669241483,  0.0; ...
    1.154700538379252,  1.032795558988645, -0.516397779494321,  0.577350269189626 ];

  return
end


function [X Z G U histo histo_R] = SPC(T,Q,TV_SV,rho,K,SNR,rate,maxiter,tol,out)

  warning off;

  histo=[];
  histo_R=[];
  N  = ndims(T);
  II = size(T);
  NN = sum(Q(:));
  epsiron = 10^(-SNR/10) * sum_all(T(Q).^2);

  %% initialization
  E  = zeros(II);
  X  = zeros(II);
  Z  = zeros(II);
  ONE= zeros(II);

  R  = 1;

  X(Q) = T(Q);
  X(~Q) = sum(X(:))/NN;
  for n = 1:N
    U{n} = randn(II(n),R);
    U{n} = U{n}/norm(U{n});
    Pp  = eye(II(n)-1,II(n));
    Pm  = eye(II(n)); Pm(1,:) = [];
    P{n} = Pp - Pm;
    PtP{n} = P{n}'*P{n};
    Pr{n} = inv(rho(n)*PtP{n} + eye(II(n)));
  end
  G = tensor_allprod(X,U,1);

  for r = 1:R
    Z = Z + G(r)*outerprod(U,r);
  end
  obj = sum_all((T(Q) - Z(Q)).^2);
  X(~Q) = Z(~Q);
  E = X - Z;

  %% output some figures

  h1 = figure();clf;
  subplot(1,2,1);cla;hold on;
  subplot(1,2,2);
  drawnow;
  if out == 1
    h2 = figure();clf;
    imagesc(uint8(X));drawnow;
    imwrite(uint8(X),['saved/' TV_SV '_iter_0.png']);
  end

  %% start main algorithm
  for iter = 1:maxiter

    [val ID] = sort(G);
    for k = ID(1:min(end,K))

      ONE = G(k)*outerprod(U,k);
      Z = Z - ONE;
      E = E + ONE;

      div = 1;
      for n = 1:N

        if strcmp(TV_SV,'tv')

          u = innerprod_one_exc(E,U,k,n);
          u = u(:);
          a = u(:);
          object = 0.5*G(k)*sum(abs(P{n}*a)) + a'*u + 0.5*G(k);
          for nn = 1:1000
            df = P{n}'*sign(P{n}*a);
            dL = (0.5*G(k)*rho(n)*df - u + G(k)*a)/G(k);
            al = [0 0.1 0.01 0.001 0.0001 0.00001];
            for ai = 1:length(al)
              a2 = a - al(ai)*dL;
              score(ai) = 0.5*G(k)*sum(abs(P{n}*a2)) + a2'*u + 0.5*G(k);
            end
            [object2 ai] = min(score);
            a2 = a - al(ai)*dL;
            if abs(object2 - object)/II(n) < 1e-3
              break;
            else
              a = a2;
              object = object2;
            end
          end
          lam = norm(a);
          u = a/lam;
          U{n}(:,k) = u;
          v{n} = u;
          div  = div + rho(n)*sum(abs(P{n}*u));

        elseif strcmp(TV_SV,'sv')

          u = innerprod_one_exc(E,U,k,n);
          u = Pr{n} * u(:);
          lam = norm(u);
          u = u/lam;
          U{n}(:,k) = u;
          v{n} = u;
          div  = div + rho(n)*u'*PtP{n}*u;

        else
          error('2rd input is ''tv'' or ''qc'' ');
        end

      end
      G(k) = tensor_allprod(E,v,1)/div;
      if G(k) < 0
        G(k) = -G(k);
        U{1}(:,k) = - U{1}(:,k);
      end

      ONE = G(k)*outerprod(U,k);
      E = E - ONE;
      E(~Q) = 0;
      Z = Z + ONE;
      X(~Q) = Z(~Q);

    end

    %% calculate MSE
    obj2 = sum_all(E.^2);

    %% convergence speed
    speed = abs(obj2 - obj)/abs(epsiron - obj2);

    %% step for R <= R + 1
    if speed < rate || abs(obj2-obj)/NN < tol

      R = R + 1;
      for n = 1:N
        u = randn(II(n),1);
        u = u/norm(u);
        U{n}(:,R) = u;
        v{n} = u;
      end
      G(R) = tensor_allprod(E,v,1);
      E = E - G(R)*outerprod(U,R);
      E(~Q) = 0;

    end

    %% checking convergence
    if obj2 < epsiron
      break;
    else
      obj = obj2;
      if mod(iter,5)==0
        fprintf('%d:  %f :: %f :: %f :: Pid %d \n',iter,obj2/NN,epsiron/NN,speed,R);
      end
      histo(iter) = obj;
      histo_R(iter,:) = size(G);
    end

    %% output figures
    set(0,'CurrentFigure',h1);
    subplot(1,2,1);cla;hold on;
    plot((histo));
    plot((epsiron)*ones(1,length(histo)))
    grid on;
    set(gca,'YScale','log')
    title('MSE')
    subplot(1,2,2);
    plot(histo_R(:,2));
    title('number of components R')

    drawnow;
    if out == 1
      set(0,'CurrentFigure',h2);
      imagesc(uint8(X));
      title(['Number of R = ' num2str(R)]);
      drawnow;
    end
    if mod(iter,10) == 0 & out == 1
      imwrite(uint8(Z),['saved/' TV_SV '_iter_' num2str(iter) '.png']);
    end

    if mod(iter,100) == 0
      pack;
    end

  end


% Fig. 5.8   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami

clf
n9=1;
d9=[1 8 32 0];
rlocus(n9,d9)
title('Fig.5.08 The root locus for L(s)=1/s(s^2+8s+32)')
axis([-14 2 -6 6])
z=0:.1:.9;
 wn= 1:6;
 sgrid(z, wn)
hold on
x=[-2.67 2 -2.67 2];
y=[0 4.67*sqrt(3) 0 -4.67*sqrt(3)];
plot(x,y)
r=roots([1 0 32]);
 plot(r,'*')
 hold off