function [traces, rel, cSize] = calcTraces(grains, clusterId, varargin)
% traces of subsets of grains
%
% Syntax
%
%   clusterId = [grainId,variantId];
%   [traces, rel, cSize] = calcTraces(grains, clusterId,'shape')
%
% Input
%  grains - @grain2d
%  cId  - cluster Id
%
% Output
%  traces - @vector3d, size max(clusterId(:,1)) x max(clusterId(:,2)) ....
%  rel    - relyability index, same size as traces
%  cSize  - cluster size, , same size as traces
%
% Options
%  minClusterSize - minimum grainSize required for trace computation (default: 100)
%  shape - characteristic shape based algorithm
%  calliper - use shortest calliper instead of eigenvectors
%  hist  - circular histogram based algorithm
%
% References
%
% * <https://arxiv.org/abs/2201.02103 Determination of child phase habit
% planes from two-dimensional reconstructed parent phase orientation maps>,
% arXiv, 2022
%

% generate clusters
if nargin == 1 || ~isnumeric(clusterId), clusterId = ones(length(grains),1); end
notZero = all(clusterId>0,2);
cSize = accumarray(clusterId(notZero,:),grains.grainSize(notZero));
ic = find(cSize > get_option(varargin,'minClusterSize',100));

% prepare output
sz = [max(clusterId),1];
omega = nan(sz);
rel = zeros(sz);

% extract grain geometry
V = grains.boundary.V;
F = grains.boundary.F;
I_BG = grains.boundary.I_FG;
grainId = grains.id;

% get method to use
useHist = check_option(varargin,'hist');
useCalliper = check_option(varargin,'calliper');

% loop over all relevant clusters
for i = 1:length(ic)

  % combine grains according to clusterIndex
  [sub{1:size(clusterId,2)}]  = ind2sub(sz,ic(i));
  ind = all(clusterId == [sub{:}],2);

  % the boundary segments
  lF = F(any(I_BG(:,grainId(ind)),2),:);

  % shifted into the origin
  lS = V(lF(:,1),:) - V(lF(:,2),:);

  if isempty(lF), continue; end

  if useHist % density function method

    rho = atan2(lS(:,2),lS(:,1));

    %fun = calcDensity(rho, 'weights',sqrt(sum(lS.^2,2)),'periodic','sigma',10*degree);
    fun = calcDensity(rho, 'weights',vecnorm(lS,2,2),'periodic','sigma',10*degree);
    fun.antipodal = true;

    phi = linspace(0,2*pi,360);
    [m,ind] = max(real(fun.eval(phi)));

    omega(ic(i)) = phi(ind);
    rel(ic(i)) = (m-1)/m;

  else % characteristic shape method

    %cS = shape2d.byFV(F(any(I_BG(:,ind),2),:),V,'noSimplify');
    %[omega(ic(i)),a,b] = principalComponents(cS);
    %traces(ic(i)) = cS.caliper('shortestPerp'); % this is a bit more precise but slower

    % the following lines are from shape2d.byFV
    % just consider one direction
    fcond = lS(:,2)<0;
    lS(fcond,:)=lS(fcond,:).*-1;
    dxy = [lS; -lS];

    % sort segments according to angle
    [~,id]= sort(atan2(dxy(:,2),dxy(:,1)));
    dxy = dxy(id,:);

    % sum up
    xyn = cumsum(dxy);

    % shift again
    xyn = [xyn(:,1) - mean(xyn(:,1)) xyn(:,2) - mean(xyn(:,2))];

    if useCalliper

      mid = round(size(xyn,1)/2);
      dxyn = xyn - [xyn(1+mid:end,:);xyn(1:mid,:)];
      delta = vecnorm(dxyn,2,2);

      % minimum and maximum Ferret diameter
      a = max(delta); [b,ib] = min(delta);

      % use vector perpendicular to short axis
      omega(ic(i)) = pi/2 + atan2(dxyn(ib,2),dxyn(ib,1));

    else

      % the following lines are taken from grain2d/principleComponent
      % compute length of line segments
      dist = sqrt(sum((xyn(1:end-1,:) - xyn(2:end,:)).^2,2));
      dist = 0.5*(dist(1:end) + [dist(end);dist(1:end-1)]);

      % weight vertices according to half the length of the adjacent faces
      xyn = xyn(1:end-1,:) .* [dist,dist] .* sum(xyn(1:end-1,:).^2,2).^(0.25);

      % compute eigen values and vectors
      [ew, omega(ic(i))] = eig2(xyn' * xyn);

      % halfaxes are square roots of the eigenvalues
      b = sqrt(ew(1)); a = sqrt(ew(2));
    end
    rel(ic(i)) = (a-b)./a;
  end

end

traces = vector3d.byPolar(pi/2,omega,'antipodal');


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


%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 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 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


%%**********************************************************************
%% 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 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


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 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 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 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


% 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


function [xopt,fval,exitflag,output] = spcompsearch(z, xbox, options)
% SPCOMPSEARCH  Modified compass (coordinate) search algorithm
%    performing a local search of the sparse grid inpterpolant.
%    X = SPCOMPSEARCH(Z)  Finds a local optimizer X for the given
%    sparse grid interpolant Z using a modified compass search
%    algorithm starting from the best available sparse grid point.
%    The entire range of the sparse grid interplant is searched.
%
%    X = SPCOMPSEARCH(Z, XBOX) Uses the search box XBOX, X = [a1,
%    b1; a2, b2; ...]. The size of search box XBOX must be smaller
%    than or equal to the range of the interpolant.
%
%    X = SPCOMPSEARCH(Z, XBOX, OPTIONS)  Additionally, an OPTIONS
%    structure can be provided, see SPOPTIMSET for further details.
%
%    [X,FVAL] = SPCOMPSEARCH(...)  returns the value of the
%    sparse grid interpolant at X.
%
%    [X,FVAL,EXITFLAG] = SPCOMPSEARCH(...)  returns an EXITFLAG
%    that describes the exit condition of SPCOMPSEARCH. Possible
%    values of EXITFLAG and the corresponding exit conditions are
%
%     1  SPCOMPSEARCH converged to a solution X.
%     0  Maximum number of iterations reached.
%
%    [X,FVAL,EXITFLAG,OUTPUT] = SPCOMPSEARCH(...) returns a
%    structure OUTPUT with the number of function evaluations in
%    OUTPUT.nFevals and the computing time in .time.
%
%    Example: (minimizing the three-hump camel-back function)
%       f = inline('12*x.^2-6.3*x.^4+x.^6+6*y*(y-x)');
%       range = [-3 3; -3 3];
%       options = spset('keepFunctionValues','on');
%       z = spvals(f, 2, range, options);
%       [xopt, fval] = spcompsearch(z)
%
%    See also SPOPTIMSET.

% Author : Andreas Klimke
% Version: 2.1
% Date   : September 1, 2007

% Change log:
% V2.1 : September 1, 2007
%        Changed calling syntax to match other optimization
%        methods. Added maximum number of iterations.
% V2.0 : January 23, 2006
%        Added TolFun break criterium.
% V1.9 : June 9, 2005
%        Removed multiple start processing from V1.5; moved to
%        separate routine SPMULTISTART. Removed start point
%        computation; moved to separate routine.
% V1.8 : April 15, 2005
%        Corrected bug concerning testCorners option; Modified
%        slightly to cope with new dimension-adaptive format.
% V1.7 : January 14, 2005
%        Corrected bug that lead to wrong rescaling of parts of the
%        grid data if previous results were used.
% V1.6 : January 11, 2005
%        Added capability for handling polynomial sparse grid
%        interpolants. Added tolerance on X as options.
% V1.5 : November 13, 2004
%        Added feature for performing multiple searches at a time.
% V1.4 : September 6, 2004
%        Added capability for handling dimension-adaptive data.
% V1.3 : September 6, 2004
%      : Added check if the spvals structure contains a valid
%        range; otherwise, set it to [0,1]^d.
% V1.2 : March 9, 2004
%        Added random start point option
% V1.1 : February 18, 2004
%        Moved search for optimum of sparse grid points to separate
%        routine (spfindopt.m).
% V1.0 : January 7, 2004
%        Initial version

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

t0 = clock;

if nargin < 2, xbox = []; end
if nargin < 3, options = []; end

d = z.d;

% In case that no range has been provided to spvals -> set it to
% [0,1]^d.
if isempty(z.range)
	z.range = [zeros(d,1) ones(d,1)];
end
if isempty(xbox)
	xbox = z.range;
end

if isfield(z, 'dimAdapt')
	n = z.maxLevel';
else
	n = z.maxLevel*ones(d,1,'uint8');
end

% savepoints = [];

% Determine optimization start point(s)
[xopt, fval] = spgetstartpoint(z, xbox, options);

minimize = spoptimget(options, 'Minimize', 'on');
maximize = spoptimget(options, 'Maximize', 'off');
if strcmpi(minimize, 'on'), isminimize = 1; else isminimize = 0; end
if strcmpi(maximize, 'on'), ismaximize = 1; else ismaximize = 0; end

ymin = []; ymax = [];
if isminimize
	ymin = fval(1);
	if ismaximize
		ymax = fval(2);
	end
else
	ymax = fval(1);
end

switch(lower(z.gridType))
 case {'maximum', 'noboundary', 'clenshaw-curtis'}
	linear = 1;
 case {'chebyshev', 'gauss-patterson'}
	linear = 0;
 otherwise
		error('MATLAB:spinterp:badopt',['Unknown grid type ''' gridtype '''.']);
end

maxiter = spoptimget(options, 'MaxIter', 100);

tolx = spoptimget(options, 'TolX', []);
if isempty(tolx)
	if linear
		tolx = inf;
	else
		tolx = max(z.estRelError * 1e-2, 10*eps);
	end
end

tolfun = spoptimget(options, 'TolFun', 1e-6);
dispopt = spoptimget(options, 'Display', 'off');

nfevals = 0;

x = zeros(2*d,d);

exitflag = zeros(size(xopt,2));

for k = 1:size(xopt,2)

	% Compute the step length parameters
	dx = 1./2.^(floor(double(n)/double(d))).*(z.range(:,2)-z.range(:,1));
	dxunit = 1./2.^(floor(double(n)/double(d)));
	dxmin = 1./2.^double(n);
	dxminvec = 1./2.^double(n).*(z.range(:,2)-z.range(:,1));

	% correct the start values to lie on a full grid point
	if linear
		updated = 0;
		for l = 1:d
			xtemp = floor((xopt(l,k) - z.range(l,1)) / ...
										dxminvec(l))*dxminvec(l) + z.range(l,1);
			while xtemp < xbox(l,1)
				xtemp = xtemp + dxminvec(l);
			end
			if xtemp > xbox(l,2)
				% this can only be true if the search box is smaller than the
				% step width dx. In this case, take the center of the search
				% box as the starting point.
				xtemp = (xbox(l,1) + xbox(l,2))/2;
			end
			if xtemp ~= xopt(l,k)
				xopt(l,k) = xtemp;
				updated = 1;
			end
		end
		if updated
			xoptcell = num2cell(xopt(:,k));
			nfevals = nfevals + 1;
      if k == 1 && isminimize
        ymin = spinterp(z, xoptcell{:});
      else
        ymax = spinterp(z, xoptcell{:});
      end
    end
	end

  [isdispiter, iterstr] = initoptidisp(dispopt);
  if isdispiter
	  if k == 1 && isminimize
		  disp(sprintf(iterstr, 0, nfevals, 0, ymin, 'start point'));
	  else
		  disp(sprintf(iterstr, 0, nfevals, 0, ymax, 'start point'));
		end
	end
	if k == 2
		isminimize = 0;
	end

	exitflag(k) = 0;
	for kit = 1:maxiter

		% save the points for later processing, if requested by the
    % user
		%if nargout == 3
		%	if isminimize
		%		savepoints = [savepoints; [xopt(:,k)' ymin]];
		%	else
		%		savepoints = [savepoints; [xopt(:,k)' ymax]];
		%	end
		%end

		nfevals = nfevals + uint32(d)*2;

		% Generate search points
		id = uint32(1);
		for l = 1:d
			for l2 = 1:d
				if l == l2
					x(id,l) = xopt(l,k) - dx(l);
					x(id+1,l) = xopt(l,k) + dx(l);
					if x(id,l) < xbox(l,1)
            x(id,l) = xbox(l,1);
          end
          if x(id+1,l) > xbox(l,2)
            x(id+1,l) = xbox(l,2);
          end
				else
					x(id,l2) = xopt(l2,k);
					x(id+1,l2) = xopt(l2,k);
				end
			end
			id = id + 2;
		end

		% Perform sparse grid interpolation
		xcell = num2cell(x,1);
		ytemp = spinterp(z, xcell{:});

		if isminimize
			[ymintemp, id] = min(ytemp);
			if ymintemp < ymin
				xopt(:,k) = x(id,:)';
      	if isdispiter, disp(sprintf(iterstr, kit, nfevals, ...
	                          0, ymintemp, 'coordinate step')); end
        % Terminate if function value change is below tolerance
        if abs(ymin-ymintemp) < tolfun
				  ymin = ymintemp;
					exitflag(k) = 1;
          break;
        end
        ymin = ymintemp;
			else
      	if isdispiter, disp(sprintf(iterstr, kit, nfevals, ...
	                          0, ymin, 'contract step')); end
				if all(dxunit <= dxmin) && all(dxunit <= tolx)
					exitflag(k) = 1;
					break;
				end
				for l = 1:d
					if dxunit(l) > dxmin(l) || dxunit(l) > tolx
						dxunit(l) = dxunit(l) / 2;
						dx(l) = dx(l) / 2;
					end
				end
			end
		else
			[ymaxtemp, id] = max(ytemp);
			if ymaxtemp > ymax
				xopt(:,k) = x(id,:)';
      	if isdispiter, disp(sprintf(iterstr, kit, nfevals, ...
	                          0, ymaxtemp, 'coordinate step')); end
        % Terminate if function value change is below tolerance
        if abs(ymax-ymaxtemp) < tolfun
				  ymax = ymaxtemp;
					exitflag(k) = 1;
          break;
        end
				ymax = ymaxtemp;
			else
      	if isdispiter, disp(sprintf(iterstr, kit, nfevals, ...
	                          0, ymax, 'contract step')); end
				if all(dxunit <= dxmin) && all(dxunit <= tolx)
					exitflag(k) = 1;
					break;
				end
				for l = 1:d
					if dxunit(l) > dxmin(l) || dxunit(l) > tolx
						dxunit = dxunit / 2;
						dx = dx / 2;
					end
				end
			end
		end
	end
end

fval = [ymin ymax];

if nargout == 4
	output.nFEvals = nfevals;
	output.time = etime(clock, t0);
	% output.points = savepoints;
end


function [M1,MW,MW1] = perform_wavelet_matching(M1,M,options)

% perform_wavelet_matching - match multiscale histograms
%
% M1 = perform_wavelet_matching(M1,M,options);
%
%   M1 is the image to synthesize.
%   M is the exemplar image.
%
%   This function match the histogram of the image and the histogram
%   of each sub-band of a wavelet-like pyramid.
%
%   To do texture synthesis, one should apply several time this function.
%   You can do it by setting the value of options.niter_synthesis.
%   This leads to the synthesis as described in
%
%       Pyramid-Based Texture Analysis/Synthesis
%       D. Heeger, J. Bergen,
%       Siggraph 1995
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
niter_synthesis = getoptions(options, 'niter_synthesis', 1);

if not(isfield(options, 'color_mode'))
    options.color_mode = 'pca';
end
if isfield(options, 'color_mode') && strcmp(options.color_mode, 'pca') && ~isfield(options, 'ColorP') && size(M,3)==3
    [tmp,options.ColorP] = change_color_mode(M,+1,options);
end
rgb_postmatching = getoptions(options, 'rgb_postmatching', 0);

if size(M,3)==3
    options.niter_synthesis = 1;
    for iter=1:niter_synthesis
        % color images
        M  = change_color_mode(M, +1,options);
        M1 = change_color_mode(M1,+1,options);
        for i=1:size(M,3)
            M1(:,:,i) = perform_wavelet_matching(M1(:,:,i),M(:,:,i), options);
        end
        M  = change_color_mode(M, -1,options);
        M1 = change_color_mode(M1,-1,options);
        if rgb_postmatching
            for i=1:size(M,3)
                M1(:,:,i) = perform_histogram_equalization(M1(:,:,i),M(:,:,i));
            end
        end
    end
    return;
end

if size(M,3)>1
    for i=1:size(M,3)
        [M1(:,:,i),MW,MW1] = perform_wavelet_matching(M1(:,:,i),M(:,:,i),options);
    end
    return;
end

n = size(M,1);
n1 = size(M1,1);

synthesis_method = getoptions(options, 'synthesis_method', 'steerable');

m = 2^( ceil(log2(n)) );
m1 = 2^( ceil(log2(n1)) );
M = perform_image_extension(M,m);
M1 = perform_image_extension(M1,m1);

% precompute input
MW = my_transform(M, +1, options);

for iter=1:niter_synthesis
    % spatial equalization
    M1 = my_equalization(M1,M);
    % forward transforms
    MW1 = my_transform(M1, +1, options);
    % wavelet domain equalization
    MW1 = my_equalization(MW1,MW);
    % backward transform
    M1 = my_transform(MW1, -1, options);
    % spatial equalization
    M1 = my_equalization(M1,M);
end

M1 = M1(1:n1,1:n1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function M = my_equalization(M,M0)

options.absval = 0;
options.rows = 0;
options.cols = 0;
options.dim3 = 1;

if iscell(M)
    for i=1:min(length(M),length(M0))
        M{i} = my_equalization(M{i},M0{i});
    end
    return;
end
if size(M,3)>1
    for i=1:min(size(M,3),size(M0,3))
        M(:,:,i) = my_equalization(M(:,:,i),M0(:,:,i));
    end
    return;
end
M = perform_histogram_equalization(M,M0);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function M = my_transform(M, dir, options)

n = size(M,1);
deltaJ = 5;
Jmax = log2(n)-1;
Jmin = max(Jmax-deltaJ+1,3);

synthesis_method = getoptions(options, 'synthesis_method', 'steerable');
% steerable options
if not(isfield(options, 'nb_orientations'))
    options.nb_orientations = 4;
end
% wave ortho options
options.wavelet_type = 'biorthogonal_swapped';
options.wavelet_vm = 4;

switch synthesis_method
    case 'steerable'
        M = perform_steerable_transform(M, Jmin, options);
    case 'wavelets-ortho'
        if dir==-1
            M = convert_wavelets2list(M, Jmin);
        end
        M = perform_wavelet_transform(M, Jmin, dir, options);
        if dir==1
            M = convert_wavelets2list(M, Jmin);
        end
    case 'quincunx-ti'
        M = perform_quicunx_wavelet_transform_ti(M,Jmin,options);
    case 'wavelets-ti'
        options.wavelet_type = 'biorthogonal';
        options.wavelet_vm = 3;
        M = perform_atrou_transform(M,Jmin,options);
    otherwise
        error('Unknown transform.');
end


function x = line_ccvt_lloyd ( n, a, b, it_num, header, x )

%*****************************************************************************80
%
%% LINE_CCVT_LLOYD carries out the constrained Lloyd algorithm.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 July 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of generators.
%
%    Input, real A, B, the left and right endpoints.
%
%    Input, integer IT_NUM, the number of iterations to take.
%
%    Input, string HEADER, an identifying string.
%
%    Input, real X(N), the initial point locations.
%
%    Output, real X(N), the final point locations.
%
  x = x(:);
%
%  Print the initial generators.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial generators:\n' );
  fprintf ( 1, '\n' );
  for k = 1 : n
    fprintf ( 1, '  %4d  %f\n', k, x(k,1) );
  end
%
%  Initialize the plotting arrays.
%
  step = 1 : it_num;
  e = nan ( it_num, 1 );
  xm = nan ( it_num, 1 );

  for it = 1 : it_num

    x_plot ( 1:n, it ) = x(1:n,1);

    x_new = line_ccvt_lloyd_step ( n, a, b, x );

    e(it) = line_cvt_energy ( n, a, b, x );
    e(it) = max ( e(it), eps );
%
%  Display the energy.
%
    figure ( 1 )
    plot ( step, log ( e ), 'm-*' )
    title ( 'Log (Energy)' )
    xlabel ( 'Step' )
    ylabel ( 'Energy' )
    grid
%
%  Compute the generator motion.
%
    xm(it,1) = sum ( ( x_new(:) - x(:) ).^2 ) / n;
%
%  Display the generator motion.
%
    figure ( 2 )
    plot ( step, log ( xm ), 'm-*' )
    title ( 'Log (Average generator motion)' )
    xlabel ( 'Step' )
    ylabel ( 'Motion' )
    grid
%
%  Update the generators.
%
    x(1:n,1) = x_new(1:n,1);

  end

  x_plot(1:n,it_num+1) = x(1:n,1);
%
%  Print the current generators.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Current generators:\n' );
  fprintf ( 1, '\n' );
  for k = 1 : n
    fprintf ( 1, '  %4d  %f\n', k, x(k,1) );
  end
%
%  Plot the evolution of the locations of the generators.
%
  figure ( 3 )

  y = ( 0 : it_num );
  for k = 1 : n
    plot ( x_plot(k,1:it_num+1), y, 'LineWidth', 1 )
    hold on;
  end
  grid on
  hold off;

  title ( 'Generator evolution.' );
  xlabel ( 'Generator positions' );
  ylabel ( 'Iterations' );
%
%  Save the plots.
%
  figure ( 1 )
  filename = strcat ( header, '_energy.png' );
  print ( '-dpng', filename );
  figure ( 2 )
  filename = strcat ( header, '_motion.png' );
  print ( '-dpng', filename );
  figure ( 3 )
  filename = strcat ( header, '_evolution.png' );
  print ( '-dpng', filename );

  return
end


function [dat, W, A, lambda]= proc_ssd(dat, freqs, varargin)
% PROC_SSD - Spatio-Spectral Decomposition for a given frequency band of
% interest
%
%Synopsis:
% [DAT, W, A, LAMBDA]= PROC_SSD(DAT, FREQS, <OPT>)
%
% INPUT:
%     DAT -   data structure of continous data
%     FREQS - 3 x 2 matrix with the cut-off frequencies for the following
%             bands:
%             - first row: pass-band for the signal of interest
%             - second row: pass-band for the noise
%             - third row: stop-band for the noise
%             See the example below.
% OPT - struct or property/value list of optional properties:
%     .filterOrder  -     filter order used for butterworth bandpass and
%                         bandstop filtering. Default: 2
%     .epochIndices -     a matrix of size N x 2, where N is the number of
%                         good (i.e. artifact free) data segments. Each row of
%                         this matrix contains the first and the last sample index of
%                         a data segment, i.e. epoch_indices(n,:) = [1000, 5000]
%                         means that the n'th segment starts at sample 1000 and
%                         ends at sample 5000.
%
% OUTPUT:
% DAT       - updated data structure
% W         - SSD projection matrix (filters are in the columns)
% A         - estimated mixing matrix (spatial patterns are in the columns)
% LAMBDA    - generalized eigenvalue score of SSD objective function
%
%
%
% Description:
% This is a function for the extraction of neuronal oscillations
% with optimized signal-to-noise ratio. The algorithm maximizes
% the power at the center frequency (signal of interest) while simultaneously suppressing it
% at the flanking frequency bins (noise band).
%
% Example for FREQS:
% Let us consider that we want to extract oscillations in the 10-12 Hz
% frequency range. Then we define:
%   freqs = [10 12; 8 14; 9 13].
% The first row defines the frequency band of interest, here 10-12 Hz. The
% second and third row define the pass-pand and stop-band for the noise, respectively.
% Here we have a passband of 8-14 Hz and a stop-band of 9-13 Hz in order
% to get the noise activity just below and just above the band of interest.
%
%
%
% References:
%
% Nikulin VV, Nolte G, Curio G. A novel method for reliable and fast extraction
% of neuronal EEG/MEG oscillations on the basis of spatio-spectral decomposition.
% NeuroImage, 2011, 55: 1528-1535.
%
% Haufe, S., Dahne, S., & Nikulin, V. V. Dimensionality reduction for the
% analysis of brain oscillations. NeuroImage, 2014 (accepted for publication)
% DOI: 10.1016/j.neuroimage.2014.06.073

props= {'filterOrder'   3           'INT'
        'epochIndices'  []          'DOUBLE[- -2]'};

if nargin==0,
  dat = props; return
end

dat = misc_history(dat);
misc_checkType(dat, 'STRUCT(x clab fs)');
opt = opt_proplistToStruct(varargin{:});
opt = opt_setDefaults(opt, props);
opt_checkProplist(opt, props);

%% check if data is segmented or continous
is_epoched = ndims(dat.x) == 3;
if is_epoched
    % if the data is segmented (i.e. epoched), then concatenate epochs
    [Te, Nc, Ne] = size(dat.x);
    dat.x = reshape(permute(dat.x, [1,3,2]), [Te*Ne, Nc]);
end

%% compute SSD
[W, A, lambda, ~, X_ssd] = ssd(dat.x, freqs, dat.fs, opt.filterOrder, opt.epochIndices);

%% store the bandpass-filtered data, projected onto the SSD filters

% make sure to put the data in the correct format
if is_epoched
    X_ssd = permute(reshape(X_ssd, [Te, Ne, size(X_ssd,2)]), [1,3,2]);
end
% store the transformed data
dat.x = X_ssd;

%% rename channel labels and save old channel labels
dat.origClab= dat.clab;
dat.clab=cell(1,size(dat.x,2));
for k=1:size(dat.x,2)
    dat.clab{k} = sprintf('ssd %d',k);
end


% Multi-Frame Analysis based on Discrete Cepstral Envelope (DCE-MFA)
%
% Input
%  af            : Array of structures with matrices containing sinusoidal
%                  parameters (as the output of sin_analysis.m).
%                  Each matrix is made of:
%                   The 1st line for the frequency of each sinusoid [Hz]
%                   The 2nd line for their amplitude (linear scale)
%                  The DC has to be included (in the first column)
%  fs            : [Hz] Signal's sampling frequency
%  order         : Cepstral order
%  [extrap_dcny] : If true (default), alleviate stability problems of the DCE
%                     by replacing the DCE value and extrapolating sinusoidal
%                     components up to Nyquist.
%                  If false, use the sinusoidal components as they are.
%  [scale]       : empty  : Frequency linear scale (default)
%                 'mel'  : see frq2mel (for MFCC computation)
%                 'bark' : see frq2bark
%                 'erb'  : see frq2erb
%  [Bw]          : [Hz] Standard-deviation of the Gaussian used as weighting
%                       function (for emphasizing the importance of the low
%                       frequencies in the solution).
%                       Bw As to be big enough to have the weights still
%                       significant up to Nyquist.
%  [lr]          : Regularization parameter (as in [2]) (def. 0)
%  [dftlen]      : DFT's length, if the 4th output argument is requested.
%
% Output
%  cc             : Cepstral coefficients
%  Dk             : [log] Log energy corrections
%  af             : As in input, plus the aligned amplitudes values in the
%                   'a' field.
%  E              : The amplitude cepstral envelope
%
% References
%  [1] Y. Shiga and S. King, "Estimation of voice source and vocal tract
%      characteristics based on multi-frame analysis," EUROSPEECH, 2003.
%  [2] M. Campedel-Oudot, O. Cappe and E. Moulines, "Estimation of the Spectral
%      Envelope of Voiced Sounds Using a Penalized Likelihood Approach"
%
% Copyright (c) Yannis Stylianou, 2011, Bilbao
%
% License
%  This file is part of libphoni. libphoni 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. libphoni 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.
%
% This function is part of the COVAREP project: http://covarep.github.io/covarep
%
% Authors
%  Yannis Stylianou <yannis@csd.uoc.gr>
%  Gilles Degottex <degottex@csd.uoc.gr> (regularization term)
%

function [cc Dk af E] = env_dce_mfa(af, fs, order, extrap_dcny, scale, Bw, lr, dftlen)

    debug = 0; % 0:Do nothing; 1:Plot iterations info; 2:Plot results

    % Input parameters
    if nargin<4; extrap_dcny=true; end
    if nargin<5; scale = []; end
    if nargin<6 || isempty(Bw); Bw = 2000*(fs/16000); end% 2kHz with 16kHz[1]
    if nargin<7 || isempty(lr); lr = 0; end
    if nargin<8; dftlen=4096; end

    if ~isempty(scale)
        Bw = 100*fs; % To make it as similar as possible to the DCE-SFA
        lr = 3.5e-2; % To make it as similar as possible to the DCE-SFA [2]
        eval(['fnscale=@frq2' scale ';']);
        for n=1:numel(af)
            af(n).f = 0.5*fs*fnscale(af(n).f)/fnscale(fs/2);
        end
    end

    % If asked, extrapolate components at DC and up to Nyquist
    if extrap_dcny; af = env_extrap_sins_dcny(af, fs); end

    for n=1:numel(af)
        af(n).f = af(n).sins(1,:);
        af(n).a = log(af(n).sins(2,:));
    end

    if debug>1; subplot(211); hold off; end

    M = length(af);

    BWB = 0;  % [1](9)
    erI = 0;  % Error of previous step (initialization error)
    iter = 0; % Iteration number
    cc = zeros(order+1,1);  % initialization for ceps
    rt = 0;

    for k=1:M
        fk = af(k).f;
        ak = af(k).a;
        Nk = length(fk);
        fk = fk(:);
        ak = ak(:);
        Bk = [ones(Nk,1) 2*cos(2*pi*fk/fs*(1:order))];

        wk = exp(-fk.^2 / (2 * Bw * Bw))'; % Bottom-right paragraph p.3
        Wk = diag(wk)/Nk; % (8)

        % keep the main matrices
        BWB = BWB+(Bk'*Wk*Bk); % order by order
        af(k).Bk = Bk;        % keep Bk
        af(k).Wk = Wk;        % keep Wk
        uk = ones(Nk,1);

        dk = uk'*Wk*(ak)/(uk'*Wk*uk); % (10) with c=0
        erI = erI + ((ak-dk*uk)'*Wk*(ak-dk*uk));% (7) with c=0
        rt =  rt + (Bk'*Wk*(ak-dk*uk)); % Right-hand term of (9)
    end

    if debug>0; disp(['iter:' num2str(iter) ' error=' num2str(erI) 'log']); end

    % first estimation of ceps
    if lr==0
        cc = BWB\rt; % Solution of (9)
    else
        cc = (BWB+lr*diag(ones(size(BWB,1),1)))\rt; % Solution of (9) + Regul term
    end
    cc(1) = 0;

    iter = 1;
    while(1)
        er = 0;
        rt = 0; % Right-hand term of (9)
        Dk = zeros(M,1); % log energy corrections

        for k=1:M
            ak = af(k).a;ak= ak(:);
            Bk = af(k).Bk;
            Wk = af(k).Wk;

            Nk = length(ak);
            uk = ones(Nk,1);

            dk = uk'*Wk*(ak-Bk*cc)/(uk'*Wk*uk);% (10)
            h = (ak-dk*uk-Bk*cc);
            er = er + (h'*Wk*h);% (7)
            rt =  rt + (Bk'*Wk*(ak-dk*uk)); % Right-hand term of (9)

            Dk(k) = dk;
        end
        if lr==0
            cc = BWB\rt; % Solution of (9)
        else
            cc = (BWB+lr*diag(ones(size(BWB,1),1)))\rt; % Solution of (9) + Regul term
        end
        cc(1) = 0;

        if debug>0; disp(['iter:' num2str(iter) ' error=' num2str(er) 'log']); end
    %      disp(['reldiff=' num2str(abs((erI-er)/er))]);

        if debug>1
            % plot
            subplot(211);
                plot(iter-1,log(erI), 'o');
                hold on
                plot(iter,log(er), 'x');
                title(num2str(iter));
            subplot(212);
                hold off;
                for k=1:M
                    plot(af(k).f,af(k).a-Dk(k),'+');
                    hold on;
                end
                dftlen = 2048;
                fv = fs*(0:dftlen/2)/dftlen;
                lF = 2*cos(2*pi*fv'/fs*(1:order))*cc(2:order+1);
                plot(fv,lF,'r', 'LineWidth', 2);
            keyboard
        end

        if( abs((erI-er)/er)<0.001 ) % Stop if error doesn't improve more than 0.1%
            break;
        else
            erI = er;
            iter = iter+1;
        end
    end

    % Align the gain corrections with respect to the central frame
    ci = floor((numel(af)-1)/2)+1;
    cc(1) = cc(1)+Dk(ci);
    Dk = Dk - Dk(ci);
    cc(2:end) = 2*cc(2:end);

    % Include the log energy corrections into the output
    for fi=1:numel(af)
        af(fi).a = af(fi).a - Dk(fi);
    end

    % If asked, compute the envelope
    if nargout>2
        if isempty(scale)
            E = exp(fft(cc, dftlen));
            E = E(1:end/2+1);
        else
            if strcmp(scale,'bark')
                E = barkcc2spec(cc, fs, dftlen);
                E = E(1:end/2+1);
            elseif strcmp(scale,'mel')
                E = exp(fft(cc, dftlen));
                E = E(1:dftlen/2+1);
                E = cc2hspec(cc, fs, dftlen);
                E = fwcep2hspec(cc, fs, dftlen);
            end
        end
    end

return


% WAVELET_3D_PYRAMID Compute the roto-translation wavelet transfrom
%
% Usage
%   [y_Phi, y_Psi, meta_Phi, meta_Psi] = wavelet_3d_pyramid(y, filters, filters_rot, options)
%
% Input
%   y (numeric): a 3d matrix whose first two dimension corresponds to spatial
%       postion and third dimension corresponds to orientation.
%   filters (struct): a 2d pyramid filter bank (applied along spatial variable)
%   filters_rot (struct): a 1d filter bank (applied along orientation)
%   options (struct): containing the following optional fields
%       x_resolution (int): the log of spatial resolution
%       psi_mask (bool array): a mask for determining which filter to apply
%       oversampling (int): the log of spatial oversampling
%       oversampling_rot (int): the log of orientation oversampling
%
% Output
%   y_Phi (numeric): the roto-translation convolution y * Phi
%   y_Psi (cell): containing all the roto-translation convolution y * Psi
%   meta_phi (struct): meta associated to y_Phi
%   meta_psi (struct): meta associated to y_Psi
%
% Description
%   This is a pyramid implementation of the roto-translation wavelet
%   transform. Another (slower, FFT-based) implementation is available as
%   WAVELET_3D.
%	This function computes the roto-translation convolution of a three dimensional
%	signal y, with roto-translation wavelets defined as the separable product
%	low pass :
%		PHI(U,V) * PHI(THETA)
%	high pass :
%		PHI(U,V) * PHI(THETA)
%		PSI(U,V) * PHI(THETA)
%		PSI(U,V) * PSI(THETA)
%
% Reference
%	Rotation, Scaling and Deformation Invariant Scattering for Texture
%	Discrimination, Laurent Sifre, Stephane Mallat
%	Proc of CVPR 2013
%	http://www.cmapx.polytechnique.fr/~sifre/research/cvpr_13_sifre_mallat_final.pdf
%
% See also
%   WAVELET_LAYER_3D, WAVELET_FACTORY_3D

function [y_Phi, y_Psi, meta_Phi, meta_Psi] = wavelet_3d_pyramid(y,...
        filters, filters_rot, options)

    % retrieve info on input
    Q = filters.meta.Q;
    L = filters_rot.meta.size_filter / 2;
    % do not compute any convolution with high pass if y_Psi is not
    % outputed
    calculate_psi = (nargout>=2);
    [N, M, T] = size(y);

    % retrieve options
    white_list = {'J', 'angular_range', 'oversampling_rot', 'j_min', 'q_mask'};
    check_options_white_list(options, white_list);
    options = fill_struct(options, 'J', 4);
    options = fill_struct(options, 'angular_range', 'non_specified');
    options = fill_struct(options, 'oversampling_rot', -1);
    options = fill_struct(options, 'j_min', 0);
    options = fill_struct(options, 'q_mask', ones(1, Q));

    % angular resolution
    switch (options.angular_range)
        case 'zero_pi'
            angular_res = floor(log2(2*L/(2*T)));
        case 'zero_2pi'
            angular_res = floor(log2(2*L/T));
        otherwise
            error('unspecified angular range');
    end

    if (isa(filters.h.filter.coefft, 'single'))
        cast = @single;
    else
        cast = @double;
    end

    %%%%%%%%%%%%%%%%%%
    %%% low passes %%%
    %%%%%%%%%%%%%%%%%%

    %%%%%%%%%%%%%%%%%%
    %%% phi - phi %%%%
    %%%%%%%%%%%%%%%%%%

    %% low pass filter spatial : cascade with h for every slice
    % initialize structure
    hy.signal{1} = y;
    hy.meta.j(1) = 0;

    % low pass spatial using a pyramid
    h = filters.h.filter;
    for j = 1:options.J
        for theta = 1:T % filter each slice
            if (theta == 1) % allocate when size is known
                tmp_slice = pad_conv_sub_unpad(hy.signal{j}(:,:,theta), h);
                tmp = cast(zeros([size(tmp_slice), T]));
                tmp(:,:,theta) = tmp_slice;
            else
                clear tmp_slice;
                tmp(:,:,theta) = ...
                    pad_conv_sub_unpad(hy.signal{j}(:,:,theta), h);
            end
        end
        hy.signal{j+1} = tmp;
        hy.meta.j(j+1) = j;
    end
    y_phi.signal{1} = hy.signal{options.J+1};
    y_phi.meta.j(1) = hy.meta.j(options.J+1);

    %% low pass filter anguler : fft along anguler
    % initialize structure (recopy if range is zero_pi)
    ds = max(filters_rot.meta.J/filters_rot.meta.Q - options.oversampling_rot, 0);
    if (2^ds == 2*L)
        % in this case it is faster to compute the sum along the angle
        y_Phi = sum(y_phi.signal{1},3) / 2^(ds/2);
    else
        if (strcmp(options.angular_range, 'zero_pi'))
            % divide by sqrt(2) for energy preservation
            y_phi.signal{1} = repmat(y_phi.signal{1}, [1 1 2]) / sqrt(2);
        end
        y_phi_f = fft(y_phi.signal{1}, [], 3);
        phi_angle = filters_rot.phi.filter;
        y_Phi = real(...
            sub_conv_1d_along_third_dim_simple(y_phi_f, phi_angle, ds));
    end
    meta_Phi.j2 = options.J;
    meta_Phi.k2 = filters_rot.meta.J;

    %%

    %%%%%%%%%%%%%%%%%%%
    %%% high passes %%%
    %%%%%%%%%%%%%%%%%%%

    if (calculate_psi)
        p = 1;
        %%%%%%%%%%%%%%%%%%
        %%% phi - psi %%%%
        %%%%%%%%%%%%%%%%%%

        %% low pass spatial - already computed
        %% high pass angular
        % NOTE : y_phi_f
        % (the (angular) fourier transform of y * (spatial) phi)
        % might have already been computed
        if ~exist('y_phi_f', 'var')
            % fourier angle
            if (strcmp(options.angular_range, 'zero_pi'))
                % divide by sqrt(2) for energy preservation
                y_phi.signal{1} = repmat(y_phi.signal{1}, [1 1 2]) / sqrt(2);
            end
            y_phi_f = fft(y_phi.signal{1}, [], 3);
        end
        for k2 = 0:numel(filters_rot.psi.filter)-1
            psi_angle = filters_rot.psi.filter{k2+1};
            ds = max(k2/filters_rot.meta.Q - options.oversampling_rot, 0);
            y_Psi{p} = ...
                sub_conv_1d_along_third_dim_simple(y_phi_f, psi_angle, ds);
            meta_Psi.j2(p) = options.J;
            meta_Psi.q2(p) = 0;
            meta_Psi.theta2(p) = 0;
            meta_Psi.k2(p) = k2;
            p = p + 1;
        end

        %%%%%%%%%%%%%%%%%%
        %%% psi - phi %%%%
        %%%%%%%%%%%%%%%%%%
        %      AND       %
        %%%%%%%%%%%%%%%%%%
        %%% psi - phi %%%%
        %%%%%%%%%%%%%%%%%%

        %% high pass spatial - with pyramid
        if (angular_res == 0)
            g = filters.g.filter;

            for j2 = options.j_min:options.J-1
                for q = find(options.q_mask==1)-1
                    for theta2 = 1:L
                        if (strcmp(options.angular_range, 'zero_pi'))
                            for theta = 1:L
                                % convolution with psi_{j1, theta + theta2}
                                theta_sum_mod2L =  1 + mod(theta + theta2 - 2, 2*L);
                                theta_sum_modL =  1 + mod(theta + theta2 - 2, L);
                                tmp_slice = pad_conv_unpad(hy.signal{j2+1}(:,:,theta), g{theta_sum_modL + L*q});
                                if (theta == 1) % allocate
                                    tmp = cast(zeros([size(tmp_slice), 2*L]));
                                end
                                if (theta_sum_mod2L <= L)
                                    tmp(:,:,theta) = tmp_slice;
                                    tmp(:,:,theta+L) = conj(tmp_slice);
                                else
                                    tmp(:,:,theta) = conj(tmp_slice);
                                    tmp(:,:,theta+L) = tmp_slice;
                                end
                            end
                        else % options.angular_range is 'zero_2pi'
                            for theta = 1:2*L
                                % convolution with psi_{j1, theta + theta2}
                                theta_sum_mod2L =  1 + mod(theta + theta2 - 2, 2*L);
                                theta_sum_modL =  1 + mod(theta + theta2 - 2, L);
                                if (theta_sum_mod2L <= L)
                                    curr_g =  g{theta_sum_modL + L*q};
                                else
                                    curr_g =  conjugate_filter(g{theta_sum_modL + L*q});
                                end
                                tmp_slice = pad_conv_unpad(hy.signal{j2+1}(:,:,theta), curr_g);
                                if (theta == 1) % allocate
                                    tmp = cast(zeros([size(tmp_slice), 2*L]));
                                end
                                tmp(:,:,theta) = tmp_slice;

                            end
                        end

                        % tmp can now be filtered along the orientation
                        tmp_f = fft(tmp, [], 3);
                        %% low pass angle
                        ds = min(2*L, max(filters_rot.meta.J/filters_rot.meta.Q - options.oversampling_rot, 0));
                        if (2^ds == 2*L)
                            % faster to compute the sum along the angle
                            y_Psi{p} = sum(tmp, 3) / 2^(ds/2);
                        else
                            y_Psi{p} = ...
                                sub_conv_1d_along_third_dim_simple(tmp_f, phi_angle, ds);
                        end
                        meta_Psi.j2(p) = j2;
                        meta_Psi.theta2(p) = theta2;
                        meta_Psi.k2(p) = filters_rot.meta.J;
                        meta_Psi.q2(p) = q;
                        p = p + 1;

                        %% high pass angle
                        for k2 = 0:numel(filters_rot.psi.filter)-1
                            psi_angle = filters_rot.psi.filter{k2+1};
                            ds = max(k2/filters_rot.meta.Q - options.oversampling_rot, 0);
                            y_Psi{p} = ...
                                sub_conv_1d_along_third_dim_simple(tmp_f, psi_angle, ds);
                            meta_Psi.j2(p) = j2;
                            meta_Psi.theta2(p) = theta2;
                            meta_Psi.k2(p) = k2;
                            meta_Psi.q2(p) = q;
                            p = p + 1;
                        end

                    end
                end
            end
        else
            error('not yet supported');
        end

    end

    function out = pad_conv_sub_unpad(in, filter)
        Npad = size(in) + filters.meta.size_filter - 1;
        out = pad_signal(in, Npad, 'symm', 1);
        out = conv_sub_2d(out, filter, 1);
        out = unpad_signal(out, 1, size(in), filters.meta.offset);
    end

    function out = pad_conv_unpad(in, filter)
        Npad = size(in) + filters.meta.size_filter - 1;
        out = pad_signal(in, Npad, 'symm', 1);
        out = conv_sub_2d(out, filter, 0);
        out = unpad_signal(out, 0, size(in), filters.meta.offset);
    end

    function conj_filter = conjugate_filter(filter)
        conj_filter = filter;
        conj_filter.coefft = conj(filter.coefft);
    end
end


function [w, infos] = subsamp_tr(problem, in_options)

% Trust Region Method
% Authors: Jonas Kohler and Aurelien Lucchi, 2017

% Minimize a continous, unconstrained function using the Trust Region method.
%
% References
% ----------
% Conn, A. R., Gould, N. I., & Toint, P. L. (2000). Trust region methods. Society for Industrial and Applied Mathematics.
%
%
% Original Python code was created by J. M. Kohler and A. Lucchi (https://github.com/dalab/subsampled_cubic_regularization)
%
%
% This file is part of SGDLibrary.
%
% Ported to MATLAB code by K.Yoshikawa and H.Kasai on March, 2018.
% Modified by H.Kasai on Apr. 17, 2018

    % set dimensions and samples
    d = problem.dim();
    n = problem.samples();

    %% set local options
    local_options.sampling_scheme = 'exponential'; % 'linear', 'exponential'
    local_options.penalty_increase_multiplier = 2;  % multiply by..
    local_options.penalty_derease_multiplier = 2;  % divide by..
    local_options.initial_tr_radius = 1;
    local_options.successful_treshold = 0.1;
    local_options.very_successful_treshold = 0.9;
    local_options.grad_tol = 1e-9;
    local_options.max_trust_radius = 1e4;

    % sampling
    local_options.Hessian_sampling = 1;
    local_options.gradient_sampling = 0;
    local_options.initial_sample_size_Hessian = 0.025;
    local_options.initial_sample_size_gradient = 0.25;

    % subproblem
    local_options.subproblem_solver = 'GLTR';

    local_options.krylov_tol = 1e-1;
    local_options.exact_tol = 1e-2;

    % merge options
    options = mergeOptions(get_default_options(d), local_options);
    options = mergeOptions(options, in_options);

    if options.verbose > 1
        fprintf('- Subproblem_solver:%s\n', options.subproblem_solver)
        fprintf('- Hessian_sampling:%d\n', options.Hessian_sampling)
        fprintf('- Gradient_sampling:%d\n', options.gradient_sampling)
        fprintf('- Sampling_scheme:%s\n\n', options.sampling_scheme)
    end

    % decide mode
    if options.gradient_sampling == 0 && options.Hessian_sampling == 0
        mode = 'TR';
    else
        mode = 'Subsamp TR';
    end

    % initialize
    iter = 0;
    grad_calc_count = 0;
    w = options.w_init;
    lambda_k = 0;
    successful_flag = false;
    tr_radius = options.initial_tr_radius;  % intial tr radius

    grad = problem.full_grad(w);

    % compute exponential growth constant such that full sample size is reached in n_iterations
    if strcmp(options.sampling_scheme, 'exponential')
        exp_growth_constant = ((1-options.initial_sample_size_Hessian)*n)^(1/options.max_iter);
    end

    % store first infos
    clear infos;
    [infos, f_val, optgap] = store_infos(problem, w, options, [], iter, grad_calc_count, 0);

    % display infos
    if options.verbose > 0
        fprintf('%s:        : Iter = %03d, cost = %.16e, optgap = %.4e\n', mode, iter, f_val, optgap);
    end

    % set start time
    start_time = tic();

    for i = 0 : options.max_iter-1

        %% I: Subsampling
        % a) determine batchsize
        if strcmp(options.sampling_scheme, 'exponential')

            sample_size_Hessian = options.Hessian_sampling*(fix(min([n, n*options.initial_sample_size_Hessian + exp_growth_constant^(i+1)]))+1) + (1-options.Hessian_sampling)*n;
            sample_size_gradient = options.gradient_sampling*(fix(min([n, n*options.initial_sample_size_gradient + exp_growth_constant^(i+1)]))+1) + (1-options.gradient_sampling)*n;

        elseif strcmp(options.sampling_scheme, 'linear')

            sample_size_Hessian = options.Hessian_sampling*fix(min([n, max([n*options.initial_sample_size_Hessian, n/options.max_iter*(i+1)])]))+(1-options.Hessian_sampling)*n;
            sample_size_gradient = options.gradient_sampling*fix(min([n, max([n*options.initial_sample_size_gradient, n/options.max_iter*(i+1)])]))+(1-options.gradient_sampling)*n;

        elseif strcmp(options.sampling_scheme, 'fix') % Added by HK

            sample_size_Hessian = options.Hessian_sampling*fix(min(n, n*options.initial_sample_size_Hessian))+(1-options.Hessian_sampling)*n;
            sample_size_gradient = options.gradient_sampling*fix(min(n, n*options.initial_sample_size_gradient))+(1-options.gradient_sampling)*n;

        else

            sample_size_Hessian = n;
            sample_size_gradient = n;

        end

        % b) draw batches
        if sample_size_Hessian < n
            int_idx_Hessian = randi([1, n], sample_size_Hessian,1); % KY
            bool_idx_Hessian = false(n,1);
            bool_idx_Hessian(int_idx_Hessian) = true;
            sub_hess_indices = find(bool_idx_Hessian);
        else
            sub_hess_indices = 1:n;
        end

        if sample_size_gradient < n
            int_idx_gradient = randi([1, n], sample_size_gradient,1);
            bool_idx_gradient = false(n,1);
            bool_idx_gradient(int_idx_gradient) = true;
            sub_grad_indices = find(bool_idx_gradient);
        else
            sub_grad_indices = 1:n;
        end

        n_samples_per_step = sample_size_Hessian+sample_size_gradient;

        % recompute gradient either because of accepted step or because of re-sampling
        if options.gradient_sampling == 1 || successful_flag == 1
            %grad = gradient_f(w, new_X2, new_Y2, alpha);
            grad = problem.grad(w, sub_grad_indices');
            grad_norm = norm(grad);
            if grad_norm < options.grad_tol
                fprintf('Norm of gradient (%e) reached: grad_tol = %g\n', grad_norm, options.grad_tol);
                break;
            end
        end

        %% II: Step computation
        % b) call subproblem solver
        [s, lambda_k] = tr_subsolver(problem, w, grad, tr_radius, sub_hess_indices, successful_flag, lambda_k, options.subproblem_solver,...
                                           options.exact_tol, options.krylov_tol);

        %sn = norm(s);

        %% III: Regularization Update
        f_prev = problem.cost(w);
        f_curr = problem.cost(w+s);
        function_decrease = f_prev - f_curr; % f(w) - f(w+w)

        %Hs = Hv_f(w, new_X, new_Y, s, alpha);
        Hs = problem.hess_vec(w, s, sub_hess_indices);

        model_decrease = -((grad'*s) + 0.5 * (s'*Hs));

        rho = function_decrease / model_decrease;

        if model_decrease >=0
            if options.verbose > 2
                fprintf('\tNegative model decrease (%e). This should not have happened\n', model_decrease);
            end
        end

        % update w if step s is successful
        if rho >= options.successful_treshold
            w = w + s;
            loss = f_curr;
            successful_flag = true;
            accstr = 'ACC';
        else
            loss = f_prev;
            accstr = 'REJ';
        end

        % Update trust region radius
        tr_radius_ = tr_radius;
        if rho < options.successful_treshold
            tr_radius = tr_radius * (1/options.penalty_increase_multiplier);
            %fprintf('unscuccesful iteration\n');
            successful_flag = false;
            trstr = 'TR-';
        elseif rho > options.very_successful_treshold && ((norm(s) - tr_radius) < 1e-10)
            tr_radius = min([options.penalty_derease_multiplier * tr_radius, options.max_trust_radius]);
            trstr = 'TR+';
        else
            trstr = '   ';
        end

%         % recompute gradient either because of accepted step or because of re-sampling
%         if options.gradient_sampling == 1 || successful_flag == 1
%             %grad = gradient_f(w, new_X2, new_Y2, alpha);
%             grad = problem.grad(w, sub_grad_indices');
%             grad_norm = norm(grad);
%             if grad_norm < options.grad_tol
%                 break;
%             end
%         end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % count gradient evaluations
        grad_calc_count = grad_calc_count + n_samples_per_step;
        iter = iter + 1;

        % store infos
        [infos, f_val, optgap] = store_infos(problem, w, options, infos, iter, grad_calc_count, elapsed_time);

        % display infos
        if options.verbose > 0
            fprintf('%s: %s %s: Iter = %03d, cost = %.16e, optgap = %.4e', mode, accstr, trstr, iter, f_val, optgap);
            if options.verbose > 1
                fprintf(', sample (H,G)= (%d, %d)\n', sample_size_Hessian, sample_size_gradient)
            else
                fprintf('\n');
            end
        end

        if optgap < options.tol_optgap
            fprintf('Optimality gap tolerance reached: tol_optgap = %g\n', options.tol_optgap);
            break;
        end
    end

    if iter == options.max_iter
        fprintf('Max epoch reached: max_iter = %g\n', options.max_iter);
    end

end


function gram_vector = find_gram_vector(samplesf, new_sample, num_training_samples, params)
% Find the inner product of the new sample with the existing samples. TO be
% used for distance calculation

% Note: Since getting the 'exact' distance between the samples is not important,
% the inner product computation is done by using only the half spectrum for
% efficiency. In practice the error incurred by this is negligible. Also
% since we wannt to merge samples that are similar, and finding the best
% match is not important, small error in the distance computation doesn't
% matter

gram_vector = inf(params.nSamples,1,'like', params.data_type);

num_feature_blocks = numel(new_sample);

if num_training_samples == params.nSamples
    % This if statement is only for speed
    ip = zeros(1,'like', params.data_type);
    for k = 1:num_feature_blocks
        ip_block = 2*reshape(samplesf{k}, num_training_samples, []) * conj(new_sample{k}(:));
        ip = ip + real(ip_block);
    end

    gram_vector = ip;
elseif num_training_samples > 0
    ip = zeros(1,'like', params.data_type);
    for k = 1:num_feature_blocks
        ip_block = 2*reshape(samplesf{k}(1:num_training_samples,:,:,:),num_training_samples, []) * conj(new_sample{k}(:));
        ip = ip + real(ip_block);
    end

    gram_vector(1:num_training_samples) = ip;
end
end


function [K] = kv1u1(x, y, xp, yp, hyp, i)

logsigma = hyp(1);
logthetax = hyp(2);
logthetay = hyp(3);

n_x = size(x,1);
n_y = size(y,1);
n_xp = size(xp,1);
n_yp = size(yp,1);

x = repmat(x,1,n_xp);
y = repmat(y,1,n_yp);
xp = repmat(xp',n_x,1);
yp = repmat(yp',n_y,1);

switch i

case 0

K=exp(1).^(logsigma+(-1).*logthetax+(-1).*logthetay+(-1/2).*exp(1).^((-1) ...
  .*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*(x+(-1).*xp).*(y+(-1).*yp);

case 1 % logsigma

K=exp(1).^(logsigma+(-1).*logthetax+(-1).*logthetay+(-1/2).*exp(1).^((-1) ...
  .*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*(x+(-1).*xp).*(y+(-1).*yp);

case 2 % logthetax

K=exp(1).^(logsigma+(-1).*logthetax+(-1).*logthetay+(-1/2).*exp(1).^((-1) ...
  .*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*((-1)+(1/2).*exp(1).^((-1).*logthetax).*(x+(-1).*xp).^2).*(x+ ...
  (-1).*xp).*(y+(-1).*yp);

case 3 % logthetay

K=exp(1).^(logsigma+(-1).*logthetax+(-1).*logthetay+(-1/2).*exp(1).^((-1) ...
  .*logthetax).*(x+(-1).*xp).^2+(-1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*(x+(-1).*xp).*((-1)+(1/2).*exp(1).^((-1).*logthetay).*(y+(-1) ...
  .*yp).^2).*(y+(-1).*yp);

otherwise

        K = zeros(n_x, n_xp);
end

end


function [rVect, vVect] = getStateAtTime_alg(time, sma, ecc, inc, raan, arg, mean, epoch, gmu)
%getStateAtTime Summary of this function goes here
%   Detailed explanation goes here
    numTimes = length(time);

    oneArray = (zeros(1, numTimes)+1);

    sma = sma * oneArray;
    ecc = ecc * oneArray;
    inc = AngleZero2Pi(deg2rad(inc)) * oneArray;
    raan = AngleZero2Pi(deg2rad(raan)) * oneArray;
    arg = AngleZero2Pi(deg2rad(arg)) * oneArray;
    M0 = deg2rad(mean) * oneArray;

    n = computeMeanMotion(sma, gmu);
    deltaT = time - epoch;
    M = (M0(:) + n(:).*deltaT(:))';
    tru = computeTrueAnomFromMean(M, ecc);

    if(length(tru) > 1)
        [rVect,vVect] = vect_getStatefromKepler(sma, ecc, inc, raan, arg, tru, gmu, false);
    else
        [rVect, vVect] = getStatefromKepler_Alg(sma, ecc, inc, raan, arg, tru, gmu);
    end

    rVect(isnan(rVect)) = 0;
    vVect(isnan(vVect)) = 0;
end


function [R,eff] = randmio_und_connected(R, ITER)
%RANDMIO_UND_CONNECTED     Random graph with preserved degree distribution
%
%   R = randmio_und_connected(W,ITER);
%   [R eff] = randmio_und_connected(W, ITER);
%
%   This function randomizes an undirected network, while preserving the
%   degree distribution. The function does not preserve the strength
%   distribution in weighted networks. The function also ensures that the
%   randomized network maintains connectedness, the ability for every node
%   to reach every other node in the network. The input network for this
%   function must be connected.
%
%   Input:      W,      undirected (binary/weighted) connection matrix
%               ITER,   rewiring parameter
%                       (each edge is rewired approximately ITER times)
%
%   Output:     R,      randomized network
%               eff,    number of actual rewirings carried out
%
%   References: Maslov and Sneppen (2002) Science 296:910
%
%
%   2007-2012
%   Mika Rubinov, UNSW
%   Jonathan Power, WUSTL
%   Olaf Sporns, IU

%   Modification History:
%   Jun 2007: Original (Mika Rubinov)
%   Apr 2008: Edge c-d is flipped with 50% probability, allowing to explore
%             all potential rewirings (Jonathan Power)
%   Mar 2012: Limit number of rewiring attempts, count number of successful
%             rewirings (Olaf Sporns)

n=size(R,1);
[i,j]=find(tril(R));
K=length(i);
ITER=K*ITER;

% maximal number of rewiring attempts per 'iter'
maxAttempts= round(n*K/(n*(n-1)));
% actual number of successful rewirings
eff = 0;

for iter=1:ITER
    att=0;
    while (att<=maxAttempts)                                     %while not rewired
        rewire=1;
        while 1
            e1=ceil(K*rand);
            e2=ceil(K*rand);
            while (e2==e1),
                e2=ceil(K*rand);
            end
            a=i(e1); b=j(e1);
            c=i(e2); d=j(e2);

            if all(a~=[c d]) && all(b~=[c d]);
                break           %all four vertices must be different
            end
        end

        if rand>0.5
            i(e2)=d; j(e2)=c; 	%flip edge c-d with 50% probability
            c=i(e2); d=j(e2); 	%to explore all potential rewirings
        end

        %rewiring condition
        if ~(R(a,d) || R(c,b))
            %connectedness condition
            if ~(R(a,c) || R(b,d))
                P=R([a d],:);
                P(1,b)=0; P(2,c)=0;
                PN=P;
                PN(:,d)=1; PN(:,a)=1;

                while 1
                    P(1,:)=any(R(P(1,:)~=0,:),1);
                    P(2,:)=any(R(P(2,:)~=0,:),1);
                    P=P.*(~PN);
                    if ~all(any(P,2))
                        rewire=0;
                        break
                    elseif any(any(P(:,[b c])))
                        break
                    end
                    PN=PN+P;
                end
            end %connectedness testing

            if rewire               %reassign edges
                R(a,d)=R(a,b); R(a,b)=0;
                R(d,a)=R(b,a); R(b,a)=0;
                R(c,b)=R(c,d); R(c,d)=0;
                R(b,c)=R(d,c); R(d,c)=0;

                j(e1) = d;          %reassign edge indices
                j(e2) = b;
                eff = eff+1;
                break;
            end %edge reassignment
        end %rewiring condition
        att=att+1;
    end %while not rewired
end %iterations


function [out] = idv2idv(n)
% Outputs a transformation matrix for changing from forward to reverse order.
%
% order 1 (Jennie's)
%
% 000, 001, 010, 011, 100, 101, 110, 111
%
% order 2 (mine)
%
% 000, 100, 010, 110, 001, 101, 011, 111
%
% USAGE:
%
%    [out] = idv2idv(n)
%
% INPUT:
%    n:      matrix, size of matrix `(2^n x 2^n)`
%
% OUTPUT:
%    out:    transforamtion matrix

warning('are you sure you want to call this function?');

if n <= 0
    out = 1;
    return;
end

out = sparse(2^n,2^n);
for i = 0:(2^n-1)
    t = dec2bin(i,n);
    t2 = t(end:-1:1);
    i2 = bin2dec(t2);
    out(i+1,i2+1) = 1;
end