function [P,model] = chrrParseModel(model)
% Parse a COBRA model into the right format for the CHRR sampler
%
% USAGE:
%
%      [P,model] = chrrParseModel(model);
%
% We are trying to sample uniformly at random from the points v that satisfy:
%
% .. math::
%                     Sv = b\\
%             ~~ l_b \leq v \leq u_b
%
% INPUTS:
%    model:    COBRA model structure with fields:
%
%               * .S - The `m x n` stoichiometric matrix
%               * .lb - `n x 1` lower bounds on fluxes
%               * .ub - `n x 1` upper bounds on fluxes
%
% OPTIONAL INPUTS:
%               * .C - 'k x n' matrix of additional inequality constraints
%               * .d - 'k x 1' rhs of the above constraints
%               * .dsense - 'k x 1' the sense of the above constraints ('L' or 'G')
%
% OUTPUTS:
%    P:        A structure with fields:
%
%               * .A_eq - Equality constraint matrix (`model.S`)
%               * .b_eq - Right hand side of equality constraints (`model.b`)
%               * .A - Inequality constraint matrix (`[I_n 0; 0 -I_n]`)
%               * .b - Right hand side of inequality constraints (`[lb; -ub]`)
%
% .. Authors:
%       - Ben Cousins and Hulda Haraldsdóttir, 10/2016
%       - Ben Cousins, 12/2017, Moved objective function handling to preprocess function
%       - Ben Cousins, 05/2019, Added support for C,d inequalities.

dim = length(model.lb);

P.A = [eye(dim); -eye(dim)];
P.b = [model.ub; -model.lb];

if isfield(model,'C') && isfield(model,'d')
   for i=1:size(model.C,1)
      if model.dsense(i)=='G'
          % convert constraint to <=
          model.C(i,:) = model.C(i,:)*-1;
          model.d(i) = model.d(i)*-1;
      elseif model.dsense{i}=='E'
          error('Equality constraints not supported in C,d fields.');
      end
   end
   P.A = [P.A; model.C];
   P.b = [P.b; model.d];
end

P.A_eq = model.S;
P.b_eq = model.b;

end


function [yu,dyudtheta] = u_GaussianBumps(Theta,u,inF)

% input function for free-form deterministic DCM

spread = exp(Theta(1));
scale = exp(Theta(inF.indscale));
centres = exp(Theta(inF.indcentres));
n = length(centres);
yu = 0;
dyudtheta = zeros(size(Theta));
for i=1:n
   dt = centres(i)-u;
   bump = scale(i)*exp(-0.5*dt.^2./spread);
   dyudtheta(1) = ...
       dyudtheta(1) + scale(i)*dt^2./(2*spread*exp(dt^2/(2*spread)));
   dyudtheta(inF.indscale(i)) = bump;
   dyudtheta(inF.indcentres(i)) = ...
       -scale(i)*centres(i)*dt./(spread*exp(dt^2/(2*spread)));
   yu = yu + bump;
end


function runSimSCFDMA()

SP.FFTsize = 512;
SP.inputBlockSize = 16;
SP.CPsize = 20;
%SP.subband = 15;
SP.subband = 0;

SP.SNR = [0:2:20];
SP.numRun = 10^5;

% TS 25.104
pedAchannel = [1 10^(-9.7/20) 10^(-22.8/20)];
pedAchannel = pedAchannel/sqrt(sum(pedAchannel.^2));
vehAchannel = [1 0 10^(-1/20) 0 10^(-9/20) 10^(-10/20) 0 0 0 10^(-15/20) 0 0 0 10^(-20/20)];
vehAchannel = vehAchannel/sqrt(sum(vehAchannel.^2));
idenChannel = 1;

SP.channel = idenChannel;
%SP.channel = pedAchannel;
%SP.channel = vehAchannel;

SP.equalizerType ='ZERO';
%SP.equalizerType ='MMSE';

[SER_ifdma SER_lfdma] = scfdma(SP);

save scfdma_awgn


function W = SimGraph_NearestNeighbors(M, k, Type, sigma)
% SIMGRAPH_NEARESTNEIGHBORS Returns kNN similarity graph
%   Returns adjacency matrix for an k-Nearest Neighbors
%   similarity graph
%
%   'M' - A d-by-n matrix containing n d-dimensional data points
%   'k' - Number of neighbors
%   'Type' - Type if kNN Graph
%      1 - Normal
%      2 - Mutual
%   'sigma' - Parameter for Gaussian similarity function. Set
%      this to 0 for an unweighted graph. Default is 1.
%
%   Author: Ingo Buerk
%   Year  : 2011/2012
%   Bachelor Thesis

if nargin < 3
   ME = MException('InvalidCall:NotEnoughArguments', ...
       'Function called with too few arguments');
   throw(ME);
end

if ~any(Type == (1:2))
   ME = MException('InvalidCall:UnknownType', ...
       'Unknown similarity graph type');
   throw(ME);
end

n = size(M, 2);

% Preallocate memory
indi = zeros(1, k * n);
indj = zeros(1, k * n);
inds = zeros(1, k * n);

for ii = 1:n
    % Compute i-th column of distance matrix
    dist = distEuclidean(repmat(M(:, ii), 1, n), M);

    % Sort row by distance
    [s, O] = sort(dist, 'ascend');

    % Save indices and value of the k
    indi(1, (ii-1)*k+1:ii*k) = ii;
    indj(1, (ii-1)*k+1:ii*k) = O(1:k);
    inds(1, (ii-1)*k+1:ii*k) = s(1:k);
end

% Create sparse matrix
W = sparse(indi, indj, inds, n, n);

clear indi indj inds dist s O;

% Construct either normal or mutual graph
if Type == 1
    % Normal
    W = max(W, W');
else
    % Mutual
    W = min(W, W');
end

if nargin < 4 || isempty(sigma)
    sigma = 1;
end

% Unweighted graph
if sigma == 0
    W = (W ~= 0);

% Gaussian similarity function
elseif isnumeric(sigma)
    W = spfun(@(W) (simGaussian(W, sigma)), W);

else
    ME = MException('InvalidArgument:NotANumber', ...
        'Parameter epsilon is not numeric');
    throw(ME);
end

end


function vscl = vscale(f)
%VSCALE   Vertical scale of a CHEBFUN3.
%   VSCL = VSCALE(F) returns the vertical scale of a CHEBFUN3 object F as
%   determined by evaluating F on a coarse tensor-product grid.

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

% [TODO]: Should this also be taking the maximum along the edges when we are
% evaluating at 1st kind grids.

% If f is an empty CHEBFUN3, VSCL = 0:
if ( isempty(f) )
    vscl = 0;
    return
end

techCol = get(f.cols.funs{1}, 'tech');

% Get the degree of the CHEBFUN3:
[m, n, p] = length(f);

% If F is of low degree, then oversample:
m = min(max(m, 9), 41);
n = min(max(n, 9), 41);
p = min(max(p, 9), 41); % cannot afford to go over 41x41x41.

% Calculate values on a tensor grid:
dom = f.domain;
x = mypoints(m, dom(1:2), techCol);
y = mypoints(n, dom(3:4), techCol);
z = mypoints(p, dom(5:6), techCol);
[xx, yy, zz] = ndgrid(x, y, z);
vals = feval(f, xx, yy, zz);

% Take the absolute maximum:
vscl = max(abs(vals(:)));

end

%%
function x = mypoints(n, dom, tech)
% Get the sample points that correspond to the right grid for a particular
% technology.

% What tech am I based on?:
if ( isa(tech(), 'chebtech2') )
    x = chebpts(n, dom, 2);
elseif ( isa(tech(), 'chebtech1') )
    x = chebpts(n, dom, 1);
elseif ( isa(tech(), 'trigtech') )
    x = trigpts(n, dom);
else
    error('CHEBFUN:CHEBFUN3:vscale:mypoints:techType', ...
        'Unrecognized technology');
end

end


function [f,J] = spm_fx_lfp(x,u,P,M)
% state equations for a neural mass model of erps
% FORMAT [f,J] = spm_fx_lfp(x,u,P,M)
% x      - state vector
%   x(:,1)  - voltage (spiny stellate cells)
%   x(:,2)  - voltage (pyramidal cells)         +ve
%   x(:,3)  - voltage (pyramidal cells)         -ve
%   x(:,4)  - current (spiny stellate cells)    +ve
%   x(:,5)  - current (pyramidal cells)         +ve
%   x(:,6)  - current (pyramidal cells)         -ve
%   x(:,7)  - voltage (inhibitory interneurons) +ve
%   x(:,8)  - current (inhibitory interneurons) +ve
%   x(:,9)  - voltage (pyramidal cells)
%   x(:,10) - voltage (inhibitory interneurons) -ve
%   x(:,11) - current (inhibitory interneurons) -ve
%   x(:,12) - voltage (inhibitory interneurons)
%
%   x(:,13) - slow potassium conductance
%
% f    = dx(t)/dt  = f(x(t))
% J    = df/dx
%
% Fixed parameter scaling [Defaults]
%
%  E = [32 16 4];             % extrinsic rates (forward, backward, lateral)
%  G = [1 1 1/2 1/2 1/8]*128; % intrinsic rates (g1, g2, g3, g4, g5)
%  D = [2 16];                % propagation delays (intrinsic, extrinsic)
%  H = [4 32];                % receptor densities (excitatory, inhibitory)
%  T = [4 16];                % synaptic constants (excitatory, inhibitory)
%  R = [2 1];                 % parameters of static nonlinearity
%
%__________________________________________________________________________
%
% This is a simplified version of spm_fx_erp
%
% David O, Friston KJ (2003) A neural mass model for MEG/EEG: coupling and
% neuronal dynamics. NeuroImage 20: 1743-1755
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_fx_lfp.m 5369 2013-03-28 20:09:27Z karl $

% check if intrinsic connections are free parameters
%--------------------------------------------------------------------------
try, P.H; catch, P.H = 0; end

% get dimensions and configure state variables
%--------------------------------------------------------------------------
x    = spm_unvec(x,M.x);       % neuronal states
n    = size(x,1);              % number of sources
s    = size(x,2);              % number of states

% [default] fixed parameters
%--------------------------------------------------------------------------
E    = [32 16 4];              % extrinsic rates (forward, backward, lateral)
G    = [1 1 1/2 1/2 1/32]*128; % intrinsic rates (g1, g2 g3, g4)
D    = [2 4];                  % propagation delays (intrinsic, extrinsic)
H    = [8 32];                 % receptor densities (excitatory, inhibitory)
T    = [4 16];                 % synaptic constants (excitatory, inhibitory)
R    = [1 2];                  % parameters of static nonlinearity

% [specified] fixed parameters
%--------------------------------------------------------------------------
if isfield(M,'pF')
    try, E  = M.pF.E; end
    try, G  = M.pF.H; end
    try, D  = M.pF.D; end
    try, H  = M.pF.G; end
    try, T  = M.pF.T; end
    try, R  = M.pF.R; end
end

% exponential transform to ensure positivity constraints
%--------------------------------------------------------------------------
A{1} = exp(P.A{1})*E(1);
A{2} = exp(P.A{2})*E(2);
A{3} = exp(P.A{3})*E(3);
C    = exp(P.C);
G    = exp(P.H)*diag(G);

% intrinsic connectivity and parameters
%--------------------------------------------------------------------------
Te   = T(1)/1000*exp(P.T(:,1));      % excitatory time constants
Ti   = T(2)/1000*exp(P.T(:,2));      % inhibitory time constants
Tk   = 512/1000;                     % slow potassium
He   = H(1)*exp(P.G);                % excitatory receptor density
Hi   = H(2);                         % inhibitory receptor density

% pre-synaptic inputs: s(V) with threshold adaptation
%--------------------------------------------------------------------------
R      = R.*exp(P.R);
x      = x';
X      = x;
X(1,:) = X(1,:) - X(13,:);
S      = 1./(1 + exp(-R(1)*(X - R(2)))) - 1./(1 + exp(R(1)*R(2)));
dSdx   = R(1)*exp(-R(1)*(max(X,-128) - R(2)))./(1 + exp(-R(1)*(X - R(2)))).^2;

% input
%==========================================================================
if isfield(M,'u')

    % endogenous input
    %----------------------------------------------------------------------
    U = u(:)*32;

else
    % exogenous input
    %----------------------------------------------------------------------
    U = C*u(:);
end

% State: f(x) and Jacobian dfdx
%==========================================================================

% NB: activity-dependent reduction in inhibitory effective time-constant
%--------------------------------------------------------------------------
Ti    = 4/1000 + Ti;

% intrinsic coupling
%--------------------------------------------------------------------------
for i = 1:n

    % synaptic dynamics - dfdx
    %----------------------------------------------------------------------
    ke    = -2/Te(i);
    ki    = -2/Ti(i);
    Ke    = -1/(Te(i)^2);
    Ki    = -1/(Ti(i)^2);
    dfdx  = [0   0   0   1   0   0   0   0   0   0   0   0   0
             0   0   0   0   1   0   0   0   0   0   0   0   0
             0   0   0   0   0   1   0   0   0   0   0   0   0
             Ke  0   0   ke  0   0   0   0   0   0   0   0   0
             0   Ke  0   0   ke  0   0   0   0   0   0   0   0
             0   0   Ki  0   0   ki  0   0   0   0   0   0   0
             0   0   0   0   0   0   0   1   0   0   0   0   0
             0   0   0   0   0   0  Ke   ke  0   0   0   0   0
             0   0   0   0   1   -1  0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   1   0   0
             0   0   0   0   0   0   0   0   0  Ki   ki  0   0
             0   0   0   0   0   0   0   1   0   0  -1   0   0
             0   0   0   0   0   0   0   0   0   0   0   0  -1/Tk];

    % intrinsic afferents - dfdS
    %----------------------------------------------------------------------
    Ke    = He(i)/Te(i);
    Ki    = Hi/Ti(i);

    j1    = Ke*G(i,1);
    j2    = Ke*G(i,2);
    j3    = Ke*G(i,3);
    j4    = Ki*G(i,4);
    j5    = Ki*G(i,5);
    dfdS  = [0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   j1  0   0   0   0
             j2  0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   j4  0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   j3  0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0  j5   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
            4/Tk 0   0   0   0   0   0   0   0   0   0   0   0];

    % motion and Jacobian
    %----------------------------------------------------------------------
    dsdx       = diag(dSdx(:,i));
    dsdx(1,13) = -dsdx(1,1);
    dfdu       = sparse(4,1,Ke,s,1);

    F{i}       = dfdx*x(:,i) + dfdS*S(:,i) + dfdu*U(i);
    J{i,i}     = dfdx + dfdS*dsdx;

    % extrinsic afferents
    %----------------------------------------------------------------------
    for j = 1:n, if i ~= j

    k1    = Ke*(A{1}(i,j) + A{3}(i,j));
    k2    = Ke*(A{2}(i,j) + A{3}(i,j));
    k3    = Ki*(A{2}(i,j) + A{3}(i,j));
    dfdS  = [0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   k1  0   0   0   0
             0   0   0   0   0   0   0   0   k2  0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   k3  0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0
             0   0   0   0   0   0   0   0   0   0   0   0   0];

    % motion and Jacobian
    %----------------------------------------------------------------------
    F{i}   = F{i} + dfdS*S(:,j);
    J{i,j} = dfdS*diag(dSdx(:,j));

    end, end
end

% construct motion and Jacobian
%--------------------------------------------------------------------------
for i = 1:n
    k      = (1:n:s*n) + (i - 1);
    f(k,1) = F{i};
    for j  = 1:n
        l         = [1:n:s*n] + (j - 1);
        dfdx(k,l) = J{i,j};
    end
end

% extrinsic and intrinsic delays
%--------------------------------------------------------------------------
De = D(2).*exp(P.D)/1000;
Di = D(1).*exp(P.I)/1000;
De = (eye(n,n) - 1).*De;
Di = (eye(s,s) - 1)*Di;
De = kron(ones(s,s),De);
Di = kron(Di,eye(n,n));

D  = Di + De;

% Implement: dx(t)/dt = f(x(t + d)) = inv(1 - D.*dfdx)*f(x(t))
%--------------------------------------------------------------------------
D  = spm_inv(speye(n*s,n*s) - D.*dfdx);
f  = D*f;
J  = D*dfdx;

return

% Equations of motion
%==========================================================================

% Supragranular layer (inhibitory interneurons): depolarizing current
%--------------------------------------------------------------------------
f(:,7)  = x(:,8);
f(:,8)  = (He.*((A{2} + A{3})*S(:,9) + G(:,3).*S(:,9)) ...
           - 2*x(:,8) - x(:,7)./Te)./Te;

% Supragranular layer (inhibitory interneurons): hyperpolarizing current
%--------------------------------------------------------------------------
f(:,10) = x(:,11);
f(:,11) = (Hi*G(:,5).*S(:,12) ...
           - 2*x(:,11) - x(:,10)./Ti)./Ti;

% Granular layer (spiny stellate cells): depolarizing current
%--------------------------------------------------------------------------
f(:,1)  = x(:,4);
f(:,4)  = (He.*((A{1} + A{3})*S(:,9) + G(:,1).*S(:,9) + U) ...
           - 2*x(:,4) - x(:,1)./Te)./Te;

% Infra-granular layer (pyramidal cells): depolarizing current
%--------------------------------------------------------------------------
f(:,2)  = x(:,5);
f(:,5)  = (He.*((A{2} + A{3})*S(:,9) + G(:,2).*S(:,1)) ...
           - 2*x(:,5) - x(:,2)./Te)./Te;

% Infra-granular layer (pyramidal cells): hyperpolarizing current
%--------------------------------------------------------------------------
f(:,3)  = x(:,6);
f(:,6)  = (Hi*G(:,4).*S(:,12) ...
           - 2*x(:,6) - x(:,3)./Ti)./Ti;

% Surpa and Infra-granular layer (pyramidal cells): Voltage
%--------------------------------------------------------------------------
f(:,9)  = x(:,5) - x(:,6);
f(:,12) = x(:,8) - x(:,11);

% Granular layer (spiny stellate cells): hyperpolarizing current
%--------------------------------------------------------------------------
f(:,13) = (4*S(:,1) - x(:,13))./Tk;

% Jacobian for delays (evaluate numerically for simplicity)
%==========================================================================
dfdx = spm_diff('spm_fx_lfp',x,u,P,1,1);


function check = exponential_check ( a, b )

%*****************************************************************************80
%
%% EXPONENTIAL_CHECK checks the parameters of the Exponential CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, the parameter of the PDF.
%    0.0 < B.
%
  if ( b <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'EXPONENTIAL_CHECK - Fatal error!\n' );
    fprintf ( 1, '  B <= 0.0\n' );
    check = 0;
    return
  end

  check = 1;

  return
end


function [u,s,U_r,U_s,U_pk,U_pd,U_l]  = projAhmPntIntoPinHoleOnRob(Rf, Sf, Spk, Spd, l)

% PROJAHMPNTINTOPINHOLEONROB Project Ahm pnt into pinhole on robot.
%    [U,S] = PROJAHMPNTINTOPINHOLEONROB(RF, SF, SPK, SPD, L) projects 3D
%    anchored homogeneous points into a pin-hole camera mounted on a robot,
%    providing also the non-measurable depth. The input parameters are:
%       RF : robot frame
%       SF : pin-hole sensor frame in robot
%       SPK: pin-hole intrinsic parameters [u0 v0 au av]'
%       SPD: radial distortion parameters [K2 K4 K6 ...]'
%       L  : 3D anchored homog. point [x y z vx vy vz rho]'
%    The output parameters are:
%       U  : 2D pixel [u v]'
%       S  : non-measurable depth
%
%    The function accepts an ahm points matrix L = [L1 ... Ln] as input.
%    In this case, it returns a pixels matrix U = [U1 ... Un] and a depths
%    row-vector S = [S1 ... Sn].
%
%    [U,S,U_R,U_S,U_K,U_D,U_L] = ... gives also the jacobians of the
%    observation U wrt all input parameters. Note that this only works for
%    single points.
%
%    See also PINHOLE, TOFRAME, PROJEUCPNTINTOPINHOLEONROB.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if nargout <= 2  % No Jacobians requested

    p     = ahm2euc(l);
    [u,s] = projEucPntIntoPinHoleOnRob(Rf, Sf, Spk, Spd, p);

else            % Jacobians requested

    if size(l,2) == 1

        % function calls
        [p,P_l]                     = ahm2euc(l);
        [u,s,U_r,U_s,U_pk,U_pd,U_p] = projEucPntIntoPinHoleOnRob(Rf, Sf, Spk, Spd, p);

        % chain rule
        U_l = U_p*P_l;

    else
        error('??? Jacobians not available for multiple AHM points.')

    end

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


classdef CI_HS < PROBLEM
% <single> <real> <large/none> <multitask>
% Multitasking problem (Griewank function + Rastrigin function)
% SubD --- 50,50 --- Number of decision variables of each task

%------------------------------- Reference --------------------------------
% K. K. Bali, Y. Ong, A. Gupta, and P. S. Tan, Multifactorial evolutionary
% algorithm with online transfer parameter estimation: MFEA-II, IEEE
% Transactions on Evolutionary Computation, 2020, 24(1): 69-83.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        SubD;   % Number of decision variables of each task
        L1;   	% Low bounds of the first task
        L2;   	% Low bounds of the second task
        U1;   	% Upper bounds of the first task
        U2;   	% Upper bounds of the second task
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.SubD     = obj.ParameterSet([50,50]);
            obj.M        = 1;
            obj.D        = max(obj.SubD) + 1;
            obj.L1       = zeros(1,obj.SubD(1)) - 600;
            obj.U1       = zeros(1,obj.SubD(1)) + 600;
            obj.L2       = zeros(1,obj.SubD(2)) - 5.12;
            obj.U2       = zeros(1,obj.SubD(2)) + 5.12;
            obj.lower    = [zeros(1,obj.D-1),1];
            obj.upper    = [ones(1,obj.D-1),length(obj.SubD)];
            obj.encoding = [ones(1,obj.D-1),2];
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            PopObj = zeros(size(PopDec,1),1);
            for i = 1 : size(PopDec,1)
                if PopDec(i,end) == 1       % Task 1
                    x1 = obj.L1 + PopDec(i,1:obj.SubD(1)).*(obj.U1-obj.L1);
                    PopObj(i) = 1/4000*sum(x1.^2,2) - prod(cos(x1./sqrt(repmat(1:size(x1,2),size(x1,1),1))),2) + 1;
                elseif PopDec(i,end) == 2   % Task 2
                    x2 = obj.L2 + PopDec(i,1:obj.SubD(2)).*(obj.U2-obj.L2);
                    PopObj(i) = sum(x2.^2-10*cos(2*pi*x2)+10,2);
                end
            end
        end
    end
end


% read a g2o data file describing a 2D SLAM instance
function graph = read_graph(filename)

fid = fopen(filename, 'r');

graph = struct (
  'x', [],
  'edges', [],
  'idLookup', struct
);

disp('Parsing File');
while true
  ln = fgetl(fid);
  if (ln == -1)
    break;
  end
  tokens = strsplit(ln, ' ', true);
  double_tokens = str2double(tokens);

  tk = 2;
  if (strcmp(tokens(1), 'VERTEX_SE2') != 0)
    id = int32(double_tokens(tk++));
    values = double_tokens(tk:tk+2)'; tk += 3;
    graph.idLookup = setfield(graph.idLookup, num2str(id), struct('offset', length(graph.x), 'dimension', length(values)));
    graph.x = [graph.x; values];
  elseif (strcmp(tokens(1), 'VERTEX_XY') != 0)
    id = int32(double_tokens(tk++));
    values = double_tokens(tk:tk+1)'; tk += 2;
    graph.idLookup = setfield(graph.idLookup, num2str(id), struct('offset', length(graph.x), 'dimension', length(values)));
    graph.x = [graph.x; values];
  elseif (strcmp(tokens(1), 'EDGE_SE2') != 0)
    fromId = int32(double_tokens(tk++));
    toId = int32(double_tokens(tk++));
    measurement = double_tokens(tk:tk+2)'; tk += 3;
    uppertri = double_tokens(tk:tk+5)'; tk += 6;
    information = [uppertri(1), uppertri(2), uppertri(3);
                   uppertri(2), uppertri(4), uppertri(5);
                   uppertri(3), uppertri(5), uppertri(6)];
    graph.edges = [graph.edges; struct(
      'type', 'P',
      'from', fromId,
      'to', toId,
      'measurement', measurement,
      'information', information)];
  elseif (strcmp(tokens(1), 'EDGE_SE2_XY') != 0)
    fromId = int32(double_tokens(tk++));
    toId = int32(double_tokens(tk++));
    measurement = double_tokens(tk:tk+1)'; tk += 2;
    uppertri = double_tokens(tk:tk+2)'; tk += 3;
    information = [uppertri(1), uppertri(2); uppertri(2), uppertri(3)];
    graph.edges = [graph.edges; struct(
      'type', 'L',
      'from', fromId,
      'to', toId,
      'measurement', measurement,
      'information', information)];
  end

end

% setup the index into the state vector
disp('Preparing helper structs');
for eid = 1:length(graph.edges)
  graph.edges(eid).fromIdx = getfield(graph.idLookup, num2str(graph.edges(eid).from)).offset + 1;
  graph.edges(eid).toIdx = getfield(graph.idLookup, num2str(graph.edges(eid).to)).offset + 1;
end

end


function d = NormL1NN_dual(x,weights)
% Dual of non-negative L1 gauge function

x(x < 0) = 0;
d = norm(x./weights,inf);


function F  = obj_actuator_freq_resp_param(x,amplitude_40, frequency_40, amplitude_100, frequency_100)
% Computes objective function for determination of basic parameters of
% the proportional valve actuator that match the required frequency
% response. The frequency response of a nonlinear system is obtained by
% using frestimate.  The required and actual frequency responses are
% compared.
% Copyright 2010 MathWorks, Inc.

% Manufacturer's characterstic
%RefFR_100_Frq =     [7 10 20 30 40 43 50 54];
%RefFR_100_Phs = -1*[25 35 59 75 87 90 96 100];
%RefFR_40_Frq =     [7 10 20 30 40 50 57 70];
%RefFR_40_Phs = -1*[21 30 50 63 75 85 90 100];

model = 'actuator_freq_resp';
load_system(model);

assignin('base','act_gain', x(1));
assignin('base','time_const', x(2));
assignin('base','act_saturation', x(3));

% Computing phase angle at current values of variable parameters

% Determine inputs and outputs
ios = getlinio(model);

% Generate input at test frequencies
in100 = frest.Sinestream(...
   'Frequency',frequency_100, ...
   'Amplitude', amplitude_100*ones(size(frequency_100)), ...
   'SimulationOrder', 'Sequential', ...
   'FreqUnits', 'Hz');

in40 = frest.Sinestream(...
   'Frequency',frequency_40, ...
   'Amplitude', amplitude_40*ones(size(frequency_40)), ...
   'SimulationOrder', 'Sequential', ...
   'FreqUnits', 'Hz');

% Test model, determine phase
sys100 = frestimate(model,ios,in100);
R100 = sys100.ResponseData(:);
Lag = angle(R100)/pi*180;
phase_100_20 = Lag(1); phase_100_43 = Lag(2);

% Test model, determine phase
sys40 = frestimate(model,ios,in40);
R40 = sys40.ResponseData(:);
Lag = angle(R40)/pi*180;
phase_40_20 = Lag(1); phase_40_57 = Lag(2);

% Calculate difference from manufacturer's characteristic
F = (phase_100_20 + 59)^2 + (phase_100_43 + 90)^2 + ...
    (phase_40_20 + 50)^2 + (phase_40_57 + 90)^2;

end


function [D,D1ols,q,C]=irfiv_ols_for_bootstrap_GK(txt,names,IVrotate,betadraw,n,Xdraw,Ydraw,k,p,enddate,startdate,cut1,cut2,cut3,cut4)
%% Copyright Ben Schumann
% function [D, gamma]=bear.irfiv_ols(names, betahat,sigmahat, n,X,Y,k,p,enddate,startdate)
% instrumental variable identification in an OLS setting
% inputs:  - matrix 'betahat': vec(OLS estimates of the reduced form)
%          - matrix 'sigmahat': vec(OLS estimates of sigma)
%          - matrix 'X': Independend Variable
%          - matrix 'Y': Dependend Variable
%          - matrix 'IV': Rotated IV from wild bootstrap
%          - integer 'IRFperiods': number of periods for IRFs
%          - integer 'n': number of endogenous variables in the VAR model (defined p 7 of technical guide)
%          - integer 'm': number of exogenous variables in the VAR model (defined p 7 of technical guide)
%          - integer 'p': number of lags included in the model (defined p 7 of technical guide)
%          - integer 'k': number of coefficients to estimate for each equation in the BVAR model (defined p 7 of technical guide)
%          - integer 'T': number of observations
%          - string  'stardate': VAR startdate
%          - string  'enddate': VAR enddate
% outputs: - matrix 'D': record of the accepted draws for the structural matrix D
%          - matrix 'gamma': record of the draws for the structural disturbances variance-covariance matrix gamma
IV = IVrotate;
%% Preparation for first stage regression
%get reduced form residuals
beta = betadraw;
B    = reshape(beta,k,n);
EPS  = Ydraw-Xdraw*B;

[EPSIV,IVcut] = bear.cut_EPS_IV_GK_new(txt, names, EPS, IV, cut1, cut2, cut3, cut4, startdate, enddate, p);

%% Imposing the covariance restrictions
%E_1 = EPSIV'*IVcut/length(IVcut);
%E11 = E_1(1,:);
%E21 = E_1(2:end,:);
%Mu = E21*E11^(-1); %relative impulse vector
%% normalize to a one standard deviation shock
sigmahatIV=(1/(length(EPSIV)-k))*(EPSIV'*EPSIV);

%get the gamma vector
%partition the reduced form VCV
%Sigma11 = sigmahatIV(1,1);
%Sigma12 = sigmahatIV(1,2:end);
%Sigma21 = sigmahatIV(2:end,1);
%Sigma22 = sigmahatIV(2:end,2:end);

%Gamma = Sigma22 + Mu*Sigma11*Mu' - Sigma21*Mu' - Mu*Sigma21'; %%%%%Gamma ouput is not used after this
%get b12 as in Michelle Piffers notes
%b12b12t = (Sigma21-Mu*Sigma11)'*Gamma^(-1)*(Sigma21-Mu*Sigma11);
%b11b11t = Sigma11 - b12b12t;
% b11 = chol(bear.nspd(b11b11t)); %%this is the scaling vector

%% first stage regression (this results in the same vector as Mu)

%step 2: Regress the first reduced form shock on the instrument
Shock = EPSIV(:,1);
[nobs , ~] = size(IVcut);
XX = [ones(nobs,1) IVcut];
[~, nvar] = size(XX);
%get OLS estimate
XpXi = (XX'*XX)\eye(nvar);
betaIV=XpXi*(XX'*Shock);
%get predicted value
IVpred = XX*betaIV;

%% second stage regression
%step 3: Regress the other reduced form shocks on the predicted value
ImpactIRFIV = zeros(n,1);
ImpactIRFIV(1,1) = 1;

for hh=2:n
Shock = EPSIV(:,hh);
[nobs,~]= size(IVpred);
IVpredtemp = [ones(nobs,1) IVpred];
[~,nvar] = size(IVpredtemp);
IVpIVi = (IVpredtemp'*IVpredtemp)\eye(nvar);
betaIV2=IVpIVi*(IVpredtemp'*Shock);
ImpactIRFIV(hh,1) = betaIV2(2,1); %should be equal to Mu from 2:end
end

% step 5: Create the structural matrix and only fill the first column as
% this is the only one identified
D=zeros(n,n);
% Step 6: Replace the first Column in the Cholesky Decomposition by
%the structural impact matrix computed above
%%another way to retrieve b11 (the scalar that scales the IRF to be a 1sdt Shock) is simply
C=chol(bear.nspd(sigmahatIV),'lower');
b=ImpactIRFIV;
%%Recover the vector q that maps the first column of C into b such that Cq=b;
q = C\b;
%%b11 is the euclidian length of q
b11q = 1/norm(q);

%
D(1:end,1) = ImpactIRFIV*b11q;
D1ols = ImpactIRFIV*b11q; % for the IRFt6 TakeOLS option


function [ f,qualMeasOut] = PCSD(proj,geo,angles,maxiter,varargin)
%PCSD solves the reconstruction problem using projection-controlled steepest descent method
%
%   PCSD(PROJ,GEO,ALPHA,NITER) solves the reconstruction problem using
%   the projection data PROJ taken over ALPHA angles, corresponding to the
%   geometry described in GEO, using NITER iterations.
%
%   PCSD(PROJ,GEO,ALPHA,NITER,OPT,VAL,...) uses options and values for solving. The
%   possible options in OPT are:
%
%
%   'lambda':      Sets the value of the hyperparameter for the SART iterations.
%                  Default is 1
%
%   'lambdared':   Reduction of lambda.Every iteration
%                  lambda=lambdared*lambda. Default is 0.99
%
%       'init':    Describes diferent initialization techniques.
%                   •  'none'     : Initializes the image to zeros (default)
%                   •  'FDK'      : intializes image to FDK reconstrucition
%
%   'TViter':      Defines the amount of TV iterations performed per SART
%                  iteration. Default is 20
%
%   'maxL2err'     Maximum L2 error to accept an image as valid. This
%                  parameter is crucial for the algorithm, determines at
%                  what point an image should not be updated further.
%                  Default is 20% of the FDK L2 norm.
%   'Verbose'      1 or 0. Default is 1. Gives information about the
%                  progress of the algorithm.
% 'redundancy_weighting': true or false. Default is true. Applies data
%                         redundancy weighting to projections in the update step
%                         (relevant for offset detector geometry)
%  'groundTruth'  an image as grounf truth, to be used if quality measures
%                 are requested, to plot their change w.r.t. this known
%                 data.
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Ander Biguri and Manasavee Lohvithee
%--------------------------------------------------------------------------

%% parse inputs
[beta,beta_red,f,ng,verbose,epsilon,QualMeasOpts,nonneg,gpuids,redundancy_weights,gt]=parse_inputs(proj,geo,angles,varargin);

measurequality=~isempty(QualMeasOpts) | ~any(isnan(gt(:)));
if ~any(isnan(gt(:)))
    QualMeasOpts{end+1}='error_norm';
    res_prev=gt;
    clear gt
end
if nargout<2 && measurequality
    warning("Image metrics requested but none catched as output. Call the algorithm with 3 outputs to store them")
    measurequality=false;
end
qualMeasOut=zeros(length(QualMeasOpts),niter);

% does detector rotation exists?
if ~isfield(geo,'rotDetector')
    geo.rotDetector=[0;0;0];
end

%% Create weigthing matrices for the SART step
% the reason we do this, instead of calling the SART fucntion is not to
% recompute the weigths every AwASD-POCS iteration, thus effectively doubling
% the computational time
% Projection weigth, W

geoaux=geo;
geoaux.sVoxel([1 2])=geo.sVoxel([1 2])*1.1; % a Bit bigger, to avoid numerical division by zero (small number)
geoaux.sVoxel(3)=max(geo.sDetector(2),geo.sVoxel(3)); % make sure lines are not cropped. One is for when image is bigger than detector and viceversa
geoaux.nVoxel=[2,2,2]'; % accurate enough?
geoaux.dVoxel=geoaux.sVoxel./geoaux.nVoxel;
W=Ax(ones(geoaux.nVoxel','single'),geoaux,angles,'Siddon','gpuids',gpuids);
W(W<min(geo.dVoxel)/4)=Inf;
W=1./W;

% Compute V
V=computeV(geo,angles,num2cell(angles),num2cell(1:length(angles)),'gpuids',gpuids);

if redundancy_weights
    % Data redundancy weighting, W_r implemented using Wang weighting
    % reference: https://iopscience.iop.org/article/10.1088/1361-6560/ac16bc

    num_frames = size(proj,3);
    W_r = redundancy_weighting(geo);
    W_r = repmat(W_r,[1,1,num_frames]);
    % disp('Size of redundancy weighting matrix');
    % disp(size(W_r));
    W = W.*W_r; % include redundancy weighting in W
end

%Initialize image.
%f=zeros(geo.nVoxel','single');

iter=0;
offOrigin=geo.offOrigin;
offDetector=geo.offDetector;
rotDetector=geo.rotDetector;
stop_criteria=0;
DSD=geo.DSD;
DSO=geo.DSO;
%%
while ~stop_criteria %POCS
    % If quality is going to be measured, then we need to save previous image
    if measurequality && ~strcmp(QualMeasOpts,'error_norm')
        res_prev = f; % only store if necesary
    end
    if (iter==0 && verbose==1);tic;end
    iter=iter+1;

    %Estimation error in the projection domain
    est_proj=Ax(f,geo,angles,'interpolated','gpuids',gpuids);
    delta_p=im3Dnorm(est_proj-proj,'L2');

    %Enforcing ART along all projections if squared delta_p > epsilon
    if (delta_p^2)>epsilon
        for jj=1:size(angles,2)
            if size(offOrigin,2)==size(angles,2)
                geo.offOrigin=offOrigin(:,jj);
            end
            if size(offDetector,2)==size(angles,2)
                geo.offDetector=offDetector(:,jj);
            end
            if size(rotDetector,2)==size(angles,2)
                geo.rotDetector=rotDetector(:,jj);
            end
            if size(DSD,2)==size(angles,2)
                geo.DSD=DSD(jj);
            end
            if size(DSO,2)==size(angles,2)
                geo.DSO=DSO(jj);
            end
            f=f+beta* bsxfun(@times,1./V(:,:,jj),Atb(W(:,:,jj).*(proj(:,:,jj)-Ax(f,geo,angles(:,jj),'gpuids',gpuids)),geo,angles(:,jj),'gpuids',gpuids));

        end
    end

    %Non-negativity projection on all pixels
    if nonneg
        f=max(f,0);
    end

    geo.offDetector=offDetector;
    geo.offOrigin=offOrigin;
    geo.DSD=DSD;
    geo.DSO=DSO;
    geo.rotDetector=rotDetector;
    if measurequality
        qualMeasOut(:,iter)=Measure_Quality(res_prev,f,QualMeasOpts);
    end

    % Compute L2 error of actual image. Ax-b
    dd=im3Dnorm(Ax(f,geo,angles,'gpuids',gpuids)-proj,'L2');
    % Compute change in the image after last SART iteration
    dp_vec=(f-f0);

    if iter==1
        step=1;
    else
        step=delta_p/delta_p_first;
    end
    f0=f;
    %  TV MINIMIZATION
    % =========================================================================
    %  Call GPU to minimize TV
    f=minimizeTV(f0,step,ng,'gpuids',gpuids);    %   This is the MATLAB CODE, the functions are sill in the library, but CUDA is used nowadays
    %                                             for ii=1:ng
    %                                                 %delta=-0.00038 for thorax phantom
    %                                                 df=weighted_gradientTVnorm(f,delta);
    %                                                 df=df./im3Dnorm(df,'L2');
    %                                                 f=f-(step.*df);
    %                                             end

    % Compute change by TV min
    dg_vec=(f-f0);

    if iter==1
        delta_p_first=im3Dnorm((Ax(f0,geo,angles,'interpolated','gpuids',gpuids))-proj,'L2');
    end

    % Reduce SART step
    beta=beta*beta_red;

    % Check convergence criteria
    % ==========================================================================

    %Define c_alpha as in equation 21 in the journal
    c=dot(dg_vec(:),dp_vec(:))/(norm(dg_vec(:),2)*norm(dp_vec(:),2));
    %This c is examined to see if it is close to -1.0

    if (c<-0.99 && dd<=epsilon) || beta<0.005|| iter>maxiter
        if verbose
            disp(['Stopping criteria met']);
            disp(['   c    = ' num2str(c)]);
            disp(['   beta = ' num2str(beta)]);
            disp(['   iter = ' num2str(iter)]);
        end
        stop_criteria=true;
    end

    if (iter==1 && verbose==1)
        expected_time=toc*maxiter;
        disp('PCSD');
        disp(['Expected duration  :    ',secs2hms(expected_time)]);
        disp(['Expected finish time:    ',datestr(datetime('now')+seconds(expected_time))]);
        disp('');
    end

end
end

function [beta,beta_red,f0,ng,verbose,epsilon,QualMeasOpts,nonneg,gpuids,redundancy_weights]=parse_inputs(proj,geo,angles,argin)
opts=     {'lambda','lambda_red','init','tviter','verbose','maxl2err','qualmeas','nonneg','gpuids','redundancy_weighting'};
defaults=ones(length(opts),1);
% Check inputs
nVarargs = length(argin);
if mod(nVarargs,2)
    error('CBCT:PCSD:InvalidInput','Invalid number of inputs')
end

% check if option has been passed as input
for ii=1:2:nVarargs
    ind=find(ismember(opts,lower(argin{ii})));
    if ~isempty(ind)
        defaults(ind)=0;
    else
        error('CBCT:PCSD:InvalidInput',['Optional parameter "' argin{ii} '" does not exist' ]);
    end
end

for ii=1:length(opts)
    opt=opts{ii};
    default=defaults(ii);
    % if one option isnot default, then extract value from input
    if default==0
        ind=double.empty(0,1);jj=1;
        while isempty(ind)
            ind=find(isequal(opt,lower(argin{jj})));
            jj=jj+1;
        end
        if isempty(ind)
            error('CBCT:PCSD:InvalidInput',['Optional parameter "' argin{jj} '" does not exist' ]);
        end
        val=argin{jj};
    end
    % parse inputs
    switch opt
        % Verbose
        %  =========================================================================
        case 'verbose'
            if default
                verbose=1;
            else
                verbose=val;
            end
            if ~is2014bOrNewer
                warning('Verbose mode not available for older versions than MATLAB R2014b');
                verbose=false;
            end
        % Lambda
        %  =========================================================================
        case 'lambda'
            if default
                beta=1;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:PCSD:InvalidInput','Invalid lambda')
                end
                beta=val;
            end
        % Lambda reduction
        %  =========================================================================
        case 'lambda_red'
            if default
                beta_red=0.99;
            else
                if length(val)>1 || ~isnumeric( val)
                    error('TIGRE:PCSD:InvalidInput','Invalid lambda')
                end
                beta_red=val;
            end
        % Initial image
        %  =========================================================================
        case 'init'
            if default || strcmp(val,'none')
                f0=zeros(geo.nVoxel','single');

            else
                if strcmp(val,'FDK')
                    f0=FDK(proj, geo, angles);
                else
                    error('TIGRE:PCSD:InvalidInput','Invalid init')
                end
            end
        % Number of iterations of TV
        %  =========================================================================
        case 'tviter'
            if default
                ng=20;
            else
                ng=val;
            end
        %  Maximum L2 error to have a "good image"
        %  =========================================================================
        case 'maxl2err'
            if default
                epsilon=im3Dnorm(FDK(proj,geo,angles))*0.2; %heuristic
            else
                epsilon=val;
            end
        %Image Quality Measure
        %  =========================================================================
        case 'qualmeas'
            if default
                QualMeasOpts={};
            else
                if iscellstr(val)
                    QualMeasOpts=val;
                else
                    error('TIGRE:PCSD:InvalidInput','Invalid quality measurement parameters');
                end
            end
        %  Non negative
        %  =========================================================================
        case 'nonneg'
            if default
                nonneg=true;
            else
                nonneg=val;
            end
        %  GPU Ids
        %  =========================================================================
        case 'gpuids'
            if default
                gpuids = GpuIds();
            else
                gpuids = val;
            end
        case 'redundancy_weighting'
            if default
                redundancy_weights = true;
            else
                redundancy_weights = val;
            end
        otherwise
            error('TIGRE:PCSD:InvalidInput',['Invalid input name:', num2str(opt),'\n No such option in PCSD()']);

    end
end

end


function s = imMin(img, varargin)
%IMMIN Minimum value of a grayscale image, or of each color component
%
%   S = imMin(IMG)
%   Computes the minimum value of pixels in image IMG. If image is grayscale
%   image, the result is a scalar. If image is a color image, the result is
%   1-by-3 row vector, each componenent corresponding to one color of the
%   image.
%
%   S = imMin(IMG, MASK)
%   Computes the minimum value only in the area specified by MASK.
%
%   S = imMin(..., 'color', COL)
%   Forces the function to consider the image as color (if COL is TRUE) or
%   as grascale (if COL is FALSE). This can be useful for vector image with
%   more than 3 color components.
%
%
%   Example
%   % apply to cameraman image
%   img = imread('cameraman.tif');
%   imMin(img)
%   ans =
%       253
%
%   % apply to a RGB image
%   img = imread('peppers.png');
%   imMin(img)
%   ans =
%       255   255   255
%
%   See also
%   imMax, imMedian, imMean
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-07-30,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

%% Process input arguments

% detect if image is color
color = isColorImage(img);

% check if user specified 'color' option
if length(varargin)>1
    var = varargin{end-1};
    if ischar(var)
        if strcmpi(var, 'color')
            color = varargin{end};
            varargin(end-1:end) = [];
        end
    end
end

%% Process color image

if color
    % If image is color, process each band separately

    % compute image size and dimension (including color dimension)
    dim = size(img);
    nd = length(dim);

    % create idnexing structure
    inds = cell(1, nd);
    for i=1:nd
        inds{i} = 1:dim(i);
    end

    % iterate on colors
    nc = dim(3);
    s = zeros(1, nc);
    for i=1:nc
        % modify the indexing structure to work on the i-th component
        inds{3} = i;
        s(i) = imMin(img(inds{:}), varargin{:});
    end

    return;
end

%% process grayscale image

if isempty(varargin)
    % compute min over all image
    s = min(img(:));
else
    % use first argument as mask
    s = min(img(varargin{1}));
end


function	W = get_estimated_weight2(Model,parm,wmode);
% Return Full Weight matrix from 'Model'
%  W = get_estimated_weight2(Model,parm);
%  W = get_estimated_weight2(Model,parm,wmode);
% --- input
% Model.W : Weight matrix for active input [Ydim x Nactive]
% Model.ix_act : active input index
% parm.Dtau
% parm.M_all
% parm.xnorm
% parm.ynorm
%  In the default mode, normalization constants, xnorm and ynorm
%  are scaled back into the weight matrix 'W' : weight for original input
%  If wmode is given and wmode = 0,
%  no scale normalization is done: weight for normalized input
%
% --- output
% W : Weight matrix : 3D-array [Ydim x Xdim x Dtau)]
% W(n,m,:) : temporal weight for n-th output & m-th input data
% Dtau : time embedding dim
% Ydim : Output space dim
% Xdim : Input space dim
%
% 2008-5-20 Masa-aki Sato

if isfield(parm,'Dtau')
	Dtau  = parm.Dtau;
else
	Dtau  = 1;
end

M_all = Model.M_all;
Ydim  = size(Model.W,1);
Xdim  = M_all/Dtau;

if isfield(Model,'ix_act')
	% Active index
	ix_act = Model.ix_act;

	W = zeros(Ydim ,M_all);
	W(:,ix_act) = Model.W;
else
	W =  Model.W;
end

if ~exist('wmode','var') || wmode~=0,
	if length(parm.xmean) == Xdim,
		parm.xmean = repmat(parm.xmean ,[Dtau 1]);
		parm.xnorm = repmat(parm.xnorm ,[Dtau 1]);
	end

	% Scale back by normalization factor
	if isfield(parm,'xnorm') & isfield(parm,'ynorm')
		W = (parm.ynorm(:)*(1./parm.xnorm(:)')) .* W;
	end
end

W = reshape( W, [Ydim, Xdim, Dtau]);


function [cst,cstJac] = autoGen_cst_footVel(q1p,q2p,q4p,q5p,q1m,q2m,q4m,q5m,dq1p,dq2p,dq4p,dq5p,dq1m,dq2m,dq4m,dq5m,l1,l2,l4,l5)
%AUTOGEN_CST_FOOTVEL
%    [CST,CSTJAC] = AUTOGEN_CST_FOOTVEL(Q1P,Q2P,Q4P,Q5P,Q1M,Q2M,Q4M,Q5M,DQ1P,DQ2P,DQ4P,DQ5P,DQ1M,DQ2M,DQ4M,DQ5M,L1,L2,L4,L5)

%    This function was generated by the Symbolic Math Toolbox version 6.2.
%    22-Oct-2015 19:14:34

t2 = sin(q1p);
t3 = sin(q2p);
t4 = sin(q4p);
t5 = sin(q5p);
t6 = sin(q1m);
t7 = sin(q2m);
t8 = sin(q4m);
t9 = sin(q5m);
cst = [dq1p.*l1.*t2+dq2p.*l2.*t3-dq4p.*l4.*t4-dq5p.*l5.*t5;-dq1m.*l1.*t6-dq2m.*l2.*t7+dq4m.*l4.*t8+dq5m.*l5.*t9];
if nargout > 1
    cstJac = reshape([0.0,0.0,dq1p.*l1.*cos(q1p),0.0,dq2p.*l2.*cos(q2p),0.0,0.0,0.0,-dq4p.*l4.*cos(q4p),0.0,-dq5p.*l5.*cos(q5p),0.0,l1.*t2,0.0,l2.*t3,0.0,0.0,0.0,-l4.*t4,0.0,-l5.*t5,0.0,0.0,0.0,0.0,-dq1m.*l1.*cos(q1m),0.0,-dq2m.*l2.*cos(q2m),0.0,0.0,0.0,dq4m.*l4.*cos(q4m),0.0,dq5m.*l5.*cos(q5m),0.0,-l1.*t6,0.0,-l2.*t7,0.0,0.0,0.0,l4.*t8,0.0,l5.*t9],[2, 22]);
end


function sol = gsp_regression_tik(G ,M, y , tau, param )
%GSP_REGRESSION_TIK Regression using graph and Tikhonov
%   Usage: sol = gsp_regression_tik(G ,M, y , tau );
%          sol = gsp_regression_tik(G ,M, y , tau, param );
%
%   Input parameters:
%       G   : Graph
%       M   : Mask (to determine with label is known)
%       y   : label (total size of the problem)
%       tau : regularization parameter (weight for tv)
%       param : optional structure of parameters
%
%   Output parameters:
%       sol : Solution of the problem
%
%   This function solve the following problem
%
%   .. argmin_x  || M x - y ||_2^2 + tau || nabla_G x ||_2^2
%
%   If tau is set to zero, then the following problem is solved
%
%   ..  argmin_x   || nabla_G x ||_2^2    s. t.  M x - y = 0
%
%   For the las problem, this function can compute an exact solution if
%   *param.exact* is activated. It will be efficient if the number of
%   unlabelled points is low.
%
%   Additional parameters
%   ---------------------
%
%   * *param.verbose* : Verbosity of the iterative algorithm
%   * *param.maxit* : maximum number of iteration for PCG
%
%   This function uses the UNLocBoX.
%
%   See also: gsp_classification_tik gsp_regression_tv
%

% Author: Nathanael Perraudin
% Date  : 24 July 2015
% Testing: test_graph_ml

%% Optional parameters

if nargin<5
    param = struct;
end

if nargin<4
    tau = 0;
end

if ~isfield(param,'verbose'), param.verbose = 1; end
if ~isfield(param,'tol'), param.tol = 1e-6; end
if ~isfield(param,'maxit'), param.maxit = 200; end
% if ~isfield(param,'direct'), param.direct = (tau==0); end
% if ~isfield(param,'exact'), param.exact = (numel(M)-nnz(M))<1000; end

if tau==0
    if param.verbose
        fprintf('Using direct solution \n')
    end
    if (numel(M) == size(M,1)) || (numel(M) == size(M,2))
        indl = find(M);
        indu = find(1-M);
    else
        error('I cannot handle this case yet');
    end
    Luu = (G.L(indu,indu));
    Wul = - G.L(indu,indl);
    tmp = (Wul * y(indl,:));
%     if ~param.exact
%         if ~isfield(param,'order'), param.order = 30; end
%         Gtemp.L = Luu;
%         Gtemp.N = size(Luu,1);
% %        Gtemp = gsp_estimate_lmax(Gtemp);
% %         cheb_coeffs = gsp_cheby_coeff(Gtemp, @(x) pinv_n(x,1e-8),...
% %         param.order, param.order +1);
% %         solt = gsp_cheby_op(Gtemp, cheb_coeffs, tmp);
%         paramt.method = 'lanczos';
%         paramt.order = param.order;
%         solt = gsp_filter_analysis(Gtemp, @(x) pinv_n(x,1e-8),tmp,paramt);
%
%     else
%        solt = pinv(full(Luu)) * tmp;
         solt = Luu \ tmp;
%     end
    sol = y;
    sol(indu,:) = solt;
    return
else

    %% prepare the graph
    if ~isfield(G,'lmax')
        G = gsp_estimate_lmax(G);
    end

    %% set the
    % setting the function f2 (see unlocbox for help)

    Mop =@(x) bsxfun(@times,M,x);

%     fg.grad = @(x) 2*Mop(Mop(x)-y);
%     fg.eval = @(x) norm(Mop(x)-y)^2;
%     fg.beta = 2;
% %     paramtik.verbose = param.verbose -1;
% %     ftik.prox = @(x,T) gsp_prox_tik(x,tau * T,G,paramtik);
%     ftik.eval = @(x) tau* sum(gsp_norm_tik(G,x));
%     ftik.grad = @(x) 2 * G.L * x;
%     ftik.beta = 2 * G.lmax;

%
%
% else
% %     param_b2.verbose = param.verbose -1;
% %     param_b2.y = y;
% %     param_b2.A = @(x) M.*x;
% %     param_b2.At = @(x) M.*x;
% %     param_b2.tight = param.tight;
% %     param_b2.epsilon = 0;
% %     fproj.prox = @(x,T) proj_b2(x,T,param_b2);
% %     fproj.eval = @(x) eps;
%
%     fproj.prox = @(x,T) x - Mop(x) + Mop(y);
%     fproj.eval = @(x) eps;
%     ftik.eval = @(x) sum(gsp_norm_tik(G,x));
%     Ltmp = G.L + G.L';
%     ftik.grad = @(x) Ltmp*x;
%     ftik.beta = 2*G.lmax;
% end

% %% solve the problem
%
% % setting different parameter for the simulation
% paramsolver = param;
%
% if tau > 0
%     sol = forward_backward(y,ftik,fg,paramsolver);
% else
%     sol = forward_backward(y,fproj,ftik,paramsolver);
% end

% sol = sol(logical(1-M));
%
% sol = reshape(sol,[],size(M,2));

    A = @(z)  vec ( Mop(reshape(z,[],size(y,2))) ...
        +  tau * G.L * reshape(z,[],size(y,2)));
    b =  vec(Mop(y));
    sol = pcg(A,b,param.tol,param.maxit,[],[],b);
    sol = reshape(sol,[],size(y,2));
end

end

% function r =  pinv_n(x,t)
%
% r = double(abs(x)>t) .* 1./x;
%
% end

%   * *param.direct* : Direct computation of the exact solution (only for
%     $\tau = 0$). (Default tau==0)
%   * *param.exact* : Exact computation of the exact solution (only for
%     $\tau = 0$ and param.direct = 0). (Default: (numel(M)-nnz(M))<1000 )


function[lstms,all_h_t,all_c_t]=Forward(batch,parameter,isTraining)%Forward
    N=size(batch.Word,1);
    zeroState=zeroMatrix([parameter.hidden,N]);
    if isTraining==1
        T=batch.MaxLen;
    else
        T=batch.MaxLenSource;
    end
    all_h_t=cell(parameter.layer_num,T);
    all_c_t=cell(parameter.layer_num,T);
    lstms = cell(parameter.layer_num,T);

    for ll=1:parameter.layer_num
        for tt=1:T
            all_h_t{ll,tt}=zeroMatrix([parameter.hidden,N]);
            all_c_t{ll,tt}=zeroMatrix([parameter.hidden,N]);
        end
    end
    for t=1:T
        for ll=1:parameter.layer_num
            if t<batch.MaxLenSource+1;
                W=parameter.W_S{ll};
            else
                W=parameter.W_T{ll};
            end
            if t==1
                h_t_1=zeroState;
                c_t_1 =zeroState;
            else
                c_t_1 = all_c_t{ll, t-1};
                h_t_1 = all_h_t{ll, t-1};
            end
            if ll==1
                x_t=parameter.vect(:,batch.Word(:,t));
            else
                x_t=all_h_t{ll-1,t};
            end
            x_t(:,batch.Delete{t})=0;
            h_t_1(:,batch.Delete{t})=0;
            c_t_1(:,batch.Delete{t})=0;
            [lstms{ll, t},all_h_t{ll, t},all_c_t{ll, t}]=lstmUnit(W,parameter,x_t,h_t_1,c_t_1,ll,t,isTraining);%LSTM unit calculation
        end
    end
end


%% FUNCTION Least_TGL
% L21 Joint Feature Learning with Least Squares Loss.
%
%% OBJECTIVE
% argmin_W { sum_i^t (0.5 * norm (Y{i} - X{i}' * W(:, i))^2)
%            + opts.rho_L2 * \|W\|_2^2 + rho1 * \|W\|_{2,1} }
%
%% INPUT
% X: {n * d} * t - input matrix
% Y: {n * 1} * t - output matrix
% rho1: L2,1-norm group Lasso parameter.
% optional:
%   opts.rho_L2: L2-norm parameter (default = 0).
%
%% OUTPUT
% W: model: d * t
% funcVal: function value vector.
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% RELATED PAPERS
%
%   [1] Evgeniou, A. and Pontil, M. Multi-task feature learning, NIPS 2007.
%   [2] Liu, J. and Ye, J. Efficient L1/Lq Norm Regularization, Technical
%       Report, 2010.
%
%% RELATED FUNCTIONS
%  Least_L21, init_opts

%% Code starts here
function [W, funcVal] = Least_L21(X, Y, rho1, opts)

if nargin <3
    error('\n Inputs: X, Y, rho1, should be specified!\n');
end
X = multi_transpose(X);

if nargin <4
    opts = [];
end

% initialize options.
opts=init_opts(opts);

if isfield(opts, 'rho_L2')
    rho_L2 = opts.rho_L2;
else
    rho_L2 = 0;
end

task_num  = length (X);
dimension = size(X{1}, 1);
funcVal = [];

% initialize a starting point
if isfield(opts,'W0')
    W0=opts.W0;
    if (nnz(size(W0)-[dimension, task_num]))
        error('\n Check the input .W0');
    end
elseif opts.init==2
    W0 = zeros(dimension, task_num);
elseif opts.init == 0
    XY = cell(task_num, 1);
    W0_prep = [];
    for t_idx = 1: task_num
        XY{t_idx} = X{t_idx}*Y{t_idx};
        W0_prep = cat(2, W0_prep, XY{t_idx});
    end
    W0 = W0_prep;
end

bFlag=0; % this flag tests whether the gradient step only changes a little

Wz= W0;
Wz_old = W0;

t = 1;
t_old = 0;

iter = 0;
gamma = 1;
gamma_inc = 2;

while iter < opts.maxIter
    alpha = (t_old - 1) /t;

    Ws = (1 + alpha) * Wz - alpha * Wz_old;

    % compute function value and gradients of the search point
    gWs  = gradVal_eval(Ws);
    Fs   = funVal_eval (Ws);

    while true
        Wzp = FGLasso_projection(Ws - gWs/gamma, rho1 / gamma);
        Fzp = funVal_eval  (Wzp);

        delta_Wzp = Wzp - Ws;
        r_sum = norm(delta_Wzp, 'fro')^2;
        %         Fzp_gamma = Fs + trace(delta_Wzp' * gWs)...
        %             + gamma/2 * norm(delta_Wzp, 'fro')^2;
        Fzp_gamma = Fs + sum(sum(delta_Wzp.* gWs))...
            + gamma/2 * norm(delta_Wzp, 'fro')^2;

        if (r_sum <=1e-20)
            bFlag=1; % this shows that, the gradient step makes little improvement
            break;
        end

        if (Fzp <= Fzp_gamma)
            break;
        else
            gamma = gamma * gamma_inc;
        end
    end

    Wz_old = Wz;
    Wz = Wzp;

    funcVal = cat(1, funcVal, Fzp + nonsmooth_eval(Wz, rho1));

    if (bFlag)
        % fprintf('\n The program terminates as the gradient step changes the solution very small.');
        break;
    end

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    iter = iter + 1;
    t_old = t;
    t = 0.5 * (1 + (1+ 4 * t^2)^0.5);

end

W = Wzp;

% private functions

    function [X] = FGLasso_projection (D, lambda )
        % l2.1 norm projection.
        X = repmat(max(0, 1 - lambda./sqrt(sum(D.^2,2))),1,size(D,2)).*D;
    end

% smooth part gradient.
    function [grad_W] = gradVal_eval(W)
        if opts.pFlag
            grad_W = zeros(zeros(W));
            parfor i = 1:task_num
                grad_W (i, :) = X{i}*(X{i}' * W(:,i)-Y{i});
            end
        else
            grad_W = [];
            for i = 1:task_num
                grad_W = cat(2, grad_W, X{i}*(X{i}' * W(:,i)-Y{i}) );
            end
        end
        grad_W = grad_W+ rho_L2 * 2 * W;
    end

% smooth part function value.
    function [funcVal] = funVal_eval (W)
        funcVal = 0;
        if opts.pFlag
            parfor i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        else
            for i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        end
        funcVal = funcVal + rho_L2 * norm(W,'fro')^2;
    end

    function [non_smooth_value] = nonsmooth_eval(W, rho_1)
        non_smooth_value = 0;
        if opts.pFlag
            parfor i = 1 : size(W, 1)
                w = W(i, :);
                non_smooth_value = non_smooth_value ...
                    + rho_1 * norm(w, 2);
            end
        else
            for i = 1 : size(W, 1)
                w = W(i, :);
                non_smooth_value = non_smooth_value ...
                    + rho_1 * norm(w, 2);
            end
        end
    end
end


function [ uvcoord ] = uv2coords( uv, width, height, planeID )
%UV2COORDS Summary of this function goes here
%   Detailed explanation goes here
if ~exist('planeID','var')
    planeID = 1;
end
if planeID~=1
    uv = xyz2uvN(uv2xyzN(uv, planeID), 1);
end

uvcoord = zeros(size(uv,1),2);
uvcoord(:,1) = min(round((uv(:,1)+pi)/2/pi*width+0.5), width);
uvcoord(:,2) = min(round((pi/2-uv(:,2))/pi*height+0.5), height);
end


f=[-2; -3; 5];
a=[-2,5,-1;1,3,1]; b=[-10;12];
aeq=[1,1,1];
beq=7;
[x,y]=linprog(f,a,b,aeq,beq,zeros(3,1));
x, y=-y


addpath tensorIO_matlab

clear

clf

tensors = readTensors('outTSDF.tensor');

tsdf = tensors(1).value;

%% visualization
%{

for i=1:size(tsdf,3)
    imagesc((tsdf(end:-1:1,end:-1:1,i))',[0 1]); axis equal; axis tight; colorbar
    title(i);
    pause(0.1);
end

%return;

for i=1:size(tsdf,2)
    imagesc((reshape(tsdf(:,i,:), [size(tsdf,1) size(tsdf,3)])),[0 1]); axis equal; axis tight; colorbar
    pause(0.1);
    title(i);
end

for i=1:size(tsdf,1)
    imagesc((reshape(tsdf(i,end:-1:1,:), [size(tsdf,2) size(tsdf,3)])),[0 1]); axis equal; axis tight; colorbar
    title(i);
    pause(0.1);
end
%}

%% meshing
disp('isosurfacing...');
tic;
fv = isosurface(tsdf,0);
toc;
% visualizaiton
%{
figure(3);
p = patch(fv);
p.FaceColor = 'red';
p.EdgeColor = 'none';
daspect([1,1,1])
view(3); axis tight
camlight
lighting gouraud
%}

unit = 0.0005;

fv.vertices = fv.vertices * unit;

fv.vertices(:,1) = fv.vertices(:,1) - mean(fv.vertices(:,1));
fv.vertices(:,2) = fv.vertices(:,2) - mean(fv.vertices(:,2));
fv.vertices(:,3) = fv.vertices(:,3) - mean(fv.vertices(:,3));

mesh2off('isosurface.off', fv.faces,fv.vertices);


function pl = plot(sT,varargin)

% find all edges given by vertex i and vertex j
[i,j] = find(sT.A_V);

% we have each edge twice -> take only that one with vi > vj
ind = i>j; i = i(ind); j = j(ind);

% interpolate between the vertices
% and add a nan at the end
N = 20; % number of interpolation points

% the interpolation matrix
interpM = [linspace(0,1,N).',linspace(1,0,N).';nan,nan];

% interpolate the vertices
V = sT.vertices(:);
pl = interpM * [V(i),V(j)].';

%
line(pl)

if check_option(varargin,'labeled') && size(sT.T,1)<500
  id = 1:size(sT.T,1);
  hold on
  text(sT.midPoints,cellfun(@int2str,vec2cell(id),'UniformOutput',false));
  hold off
end

if check_option(varargin,'labelV') && size(sT.T,1)<500
  id = 1:length(sT.vertices);
  hold on
  text(sT.vertices,cellfun(@int2str,vec2cell(id),'UniformOutput',false),'color','red');
  hold off
end

end


function line_cvt_lloyd_test04 ( )

%*****************************************************************************80
%
%% LINE_CVT_LLOYD_TEST04 tests the constrained computation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 July 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LINE_CVT_LLOYD_TEST04:\n' );
  fprintf ( 1, '  Test the constrained computation.\n' );
  fprintf ( 1, '  SORT the initial points before use.\n' );

  n = 25;
  a = 0.0;
  b = 1.0;
  it_num = 200;
  x = a + ( b - a ) * rand ( n, 1 );
  x = sort ( x );
  header = 'test04';

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use %d points in the interval [%g,%g]\n', n, a, b );
  fprintf ( 1, '  Take %d iterations.\n', it_num );
  fprintf ( 1, '  Call this calculation "%s"\n', header );
  fprintf ( 1, '  Expect a uniform spacing of %g\n', ( b - a ) / n );

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

  return
end


function tests = test_dictionary_comparison
  tests = functiontests(localfunctions);
end

function test_1(testCase)
    d = spx.dict.simple.dirac_fourier_mtx(16);
    ratio = spx.dict.comparison.matching_atoms_ratio(d, d);
    verifyEqual(testCase, ratio, 1);
    % Let's mess up with one of the atoms
    d2 = d;
    d2(1:4, 1) = 1/2;
    ratio = spx.dict.comparison.matching_atoms_ratio(d, d2);
    verifyEqual(testCase, ratio, 31/32);
    d2(2:5, 2) = 1/2;
    ratio = spx.dict.comparison.matching_atoms_ratio(d, d2);
    verifyEqual(testCase, ratio, 30/32);
end


function [costunit, selectedf] = choosefactory(factorycap,factholdcost,transcost,setupcost,factory,trkcap,salespoint,selectedsp);

    costPerUnitValues = inf(1,length(factorycap));

    for i = 1:length(factorycap)
        if (factory{i}(1,2)>=salespoint{selectedsp}(1,2))
            costPerUnitValues(1,i) = transcost(i,selectedsp) / trkcap + ( ( factorycap(i) - salespoint{selectedsp}(1,2) ) / factorycap(i) ) * factholdcost(i);
        elseif(factory{i}(1,1)==0 && (factorycap(i)+factory{i}(1,2))>=salespoint{selectedsp}(1,2))
            costPerUnitValues(1,i) = (setupcost(i) * (1 - factory{i}(1,1))) / salespoint{selectedsp}(1,2) + transcost(i,selectedsp) / trkcap + ( ( factorycap(i) - salespoint{selectedsp}(1,2) ) / factorycap(i) ) * factholdcost(i);
        end

    end;

    [costunit,selectedf] = min(costPerUnitValues);


function [node,elem] = interfaceUniformrefine(node,elem,interfaceEdge,phi)

%% Construct data structure
totalEdge = uint32(sort([elem(:,[2,3]); elem(:,[3,1]); elem(:,[1,2])],2));
[edge, ~, j] = unique(totalEdge,'rows');
N = size(node,1); NT = size(elem,1); NE = size(edge,1);
elem2edge = uint32(reshape(j,NT,3));

isInterfaceNode = false(N,1);
isInterfaceNode(interfaceEdge(:)) = true;

%% Add new nodes: middle points of all edges
tmpNode = (node(edge(:,1),:)+node(edge(:,2),:))/2;
isNewInterfaceNode = isInterfaceNode(edge(:,1)) & isInterfaceNode(edge(:,2));
a = node(edge(isNewInterfaceNode,1),:) - tmpNode(isNewInterfaceNode,:);
b = node(edge(isNewInterfaceNode,2),:) - tmpNode(isNewInterfaceNode,:);
a = tmpNode(isNewInterfaceNode,:) + [-a(:,2), a(:,1)];
b = tmpNode(isNewInterfaceNode,:) + [-b(:,2), b(:,1)];
tmpNode(isNewInterfaceNode,:) = findintersectbisect(phi,a,b);
node(N+1:N+NE,:) = tmpNode;

edge2newNode = uint32((N+1:N+NE)');

%% Refine each triangle into four triangles as follows
%     3
%    / \
%   5 - 4
%  / \ / \
% 1 - 6 - 2
t = 1:NT;
p(t,1:3) = elem(t,1:3);
p(t,4:6) = edge2newNode(elem2edge(t,1:3));
elem(t,:) = [p(t,1), p(t,6), p(t,5)];
elem(NT+1:2*NT,:) = [p(t,6), p(t,2), p(t,4)];
elem(2*NT+1:3*NT,:) = [p(t,5), p(t,4), p(t,3)];
elem(3*NT+1:4*NT,:) = [p(t,4), p(t,5), p(t,6)];


function p = subsasgn(p,s,r)
%SUBSASGN     Implements subscripted assignment for polynomials
%
%  p(i) = r
%
%For univariate polynomials p, p(i) is the coefficient of x^i, 0<=i<=degree(p).
%  For i>degree(p), p(i):=0. Similarly, p(i:j) is the vector of coefficients
%  [ p(i) p(i+1) ... p(j) ], or, p(:) is the (row) vector of all coefficients
%  of p, the same as vector(p) for univariate polynomials. Especially,
%  p(i) = [] of p(i:j) = [] cancels the i-th or i..j-th coefficient, respectively.
%
%For multivariate polynomials p in k variables x_1..x_k, p(i) is the
%  coefficient of x_1^i, i.e. a polynomial in k-1 variables.
%  Similarly, p(i,[],j) or p(i,:,j) is the coefficient polynomial of
%  x1^i*x3^j. Indices i,j,... must be single indices, no range.
%Note that access to coefficients refers to the current order p.v of
%  variables of p. To change this order, see permvars.
%For example, for a polynomial in three variables p.v={'x','y','z'},
%  p(1,0,3) is the coefficient of x*z^3 (a constant), where p(1,[],3)
%  if the coefficient of x*z^3, a univariate polynomial in y.
%
%Polynomial evaluation is denoted by p{x}, computing the value of p at x.
%  This is the same as polyval(p,x).
%For univariate polynomials, x may be a vector or matrix yielding the vector
%  or matrix of polynomial values evaluated at the corresponding coefficients.
%For multivariate polynomials, x is a vector of values of the variables.
%  For x being a matrix, the result is the (column) vector of p{x(i,:)}.
%
%Moreover, p.mid, p.rad, p.inf, p.sup give access to the midpoint, radius,
%  infimum and supremum of p, respectively.
%
%Finally, p.e, p.c and p.v give access to the arrays of exponents, coefficients
%  and variables of p, such that polynom(p.e,p.c,p.v) is again p. Single variables
%  are accessed by p.v{i}.
%
%In the univariate case,  p == polynom(p.c,p.v)  [ degree is length(p.c)+1 ].
%In the multivariate case,  p == polynom(p.e,p.c,p.v)  [ p.e is (sparse) exponent set ].
%

% written  11/20/97     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  while 1
    if ~isa(p,'polynom')
      p = subsasgn(p,s(1),r);
    elseif strcmp(s(1).type,'()')     % index reference p(i)
      if size(p.e,2)==1               % univariate polynomial
        if length(s(1).subs)>1
          error('invalid call: more than one index')
        end
        if isequal(s(1).subs{1},':')
          p.c(:) = typeadj(r,typeof(p.c));
        else
          n1 = length(p.c);
          index = n1-s(1).subs{1};     % index vector, p(0) constant term, etc.
          if ~isreal(index) | ~isequal(index,round(index))
            error('index must be integer')
          end
          if any(index>n1)             % index negative
            error('index out of range')
          end
          if ( length(r)>1 ) & ( length(index)~=length(r) )
            error('length of arguments do not match, invalid assignment')
          end
          if isa(r,'intval') & ~isa(p.c,'intval')
            p.c = intval(p.c);
          end
          m = min(index);
          if isempty(r)
            r = 0;
          end
          if m<=0                      % index greater than degree
            p.c = [zeros(1,-m+1) p.c];
            p.c(index-m+1) = r;
          else
            p.c(index) = r;
          end
        end
        m = min(find(p.c~=0));
        if isempty(m)                  % zero polynomial
          p.c = typeadj(0,typeof(p.c));
        elseif m>1                     % leading coefficients zero
          p.c = p.c(m:length(p.c));    % Matlab bug: ..end does not work for user-defined data types
        end
        p.e = length(p.c)-1;
      else                             % multivariate polynomial
        k = size(p.e,2);               % number of variables
        if length(s(1).subs)>k
          error('too many indices')
        end
        exponents = zeros(1,k);
        for i=1:length(s(1).subs)
          if isempty(s(1).subs{i}) | isequal(s(1).subs{i},':')
            error('Only integer indices allowed in multivariate polynomial assignment')
          end
          exponents(i) = s(1).subs{i};
        end
        I = all( ( p.e == repmat(exponents,size(p.e,1),1) ) , 2 );
        if any(I)                     % coefficient does occur
          if r==0
            p.e(I,:) = [];
            p.c(I) = [];
          else
            p.c(I) = r;
          end
        else                          % coefficient does not occur
          if r~=0
            p.e = [p.e ; exponents];
            if isa(r,'intval')
              p.c = intval(p.c);
            end
            p.c = [p.c ; r];
          end
        end
        p = normalize(p);
      end
    elseif strcmp(s(1).type,'.')      % polynomial access to .v
      if strcmp(s(1).subs,'v')
        if ischar(p.v)                % univariate p
          if ~ischar(r)
            error('variable of univariate polynomial must be string')
          else
            p.v = r;
          end
        else                          % multivariate p
          if ( length(s)==2 ) & ( s(2).type=='{}' )
            if s(2).subs{1}>length(p.v)
              error('index of variable too large')
            end
            if ~ischar(r)
              error('variable name must be string')
            end
            I = find(strcmp(p.v,r));
            if ~isempty(I) & ( I~=s(2).subs{1} )
              error('duplicate variable name not allowed')
            end
            p.v{s(2).subs{1}} = r;
            if rndold
              setround(rndold)
            end
            return
          elseif ~iscell(r)
            error('variables of multivariate polynomials must be cell array of strings')
          else
            if length(r)~=length(p.v)
              error('number of variables does not match')
            else
              p.v = r;
            end
          end
        end
      else
        error('invalid reference for polynomial')
      end
    else
      error('invalid index reference for polynomial')
    end
    if length(s)==1
      if rndold
        setround(rndold)
      end
      return
    end
    error('invalid call of polynom/subsasgn')
  end


function hh = quiver5(varargin)
% Slightly modified version of quiver3 function that plot arrows with
% true 3D arrow heads
% Bertrand Dano 05-12-08
%QUIVER5 3-D quiver plot.
%   QUIVER5(X,Y,Z,U,V,W) plots velocity vectors as arrows with components
%   (u,v,w) at the points (x,y,z).  The matrices X,Y,Z,U,V,W must all be
%   the same size and contain the corresponding position and velocity
%   components.  QUIVER3 automatically scales the arrows to fit.
%
%   QUIVER5(Z,U,V,W) plots velocity vectors at the equally spaced
%   surface points specified by the matrix Z.
%
%   QUIVER5(Z,U,V,W,S) or QUIVER3(X,Y,Z,U,V,W,S) automatically
%   scales the arrows to fit and then stretches them by S.
%   Use S=0 to plot the arrows without the automatic scaling.
%
%   QUIVER5(...,LINESPEC) uses the plot linestyle specified for
%   the velocity vectors.  Any marker in LINESPEC is drawn at the base
%   instead of an arrow on the tip.  Use a marker of '.' to specify
%   no marker at all.  See PLOT for other possibilities.
%
%   QUIVER5(...,'filled') fills any markers specified.
%
%   H = QUIVER3(...) returns a vector of line handles.
%
%   Example:
%       [x,y] = meshgrid(-2:.2:2,-1:.15:1);
%       z = x .* exp(-x.^2 - y.^2);
%       [u,v,w] = surfnorm(x,y,z);
%       quiver5(x,y,z,u,v,w);
%       axis vis3d; rotate3d on
%
%   See also QUIVER, PLOT, PLOT3, SCATTER.
%   Clay M. Thompson 3-3-94
%   Copyright 1984-2002 The MathWorks, Inc.
%   $Revision: 1.23 $  $Date: 2002/06/05 20:05:16 $
% Arrow head parameters
alpha = 0.33; % Size of arrow head relative to the length of the vector
beta = 0.33;  % Width of the base of the arrow head relative to the length
autoscale = 1; % Autoscale if ~= 0 then scale by this.
plotarrows = 1;
filled = 0;
ls = '-';
ms = '';
col = '';
nin = nargin;
% Parse the string inputs
while isstr(varargin{nin}),
  vv = varargin{nin};
  if ~isempty(vv) & strcmp(lower(vv(1)),'f')
    filled = 1;
    nin = nin-1;
  else
    [l,c,m,msg] = colstyle(vv);
    if ~isempty(msg),
      error(sprintf('Unknown option "%s".',vv));
    end
    if ~isempty(l), ls = l; end
    if ~isempty(c), col = c; end
    if ~isempty(m), ms = m; plotarrows = 0; end
    if isequal(m,'.'), ms = ''; end % Don't plot '.'
    nin = nin-1;
  end
end
error(nargchk(4,7,nin));
% Check numeric input arguments
if nin<6, % quiver3(z,u,v,w) or quiver3(z,u,v,w,s)
  [msg,x,y,z] = xyzchk(varargin{1});
  u = varargin{2};
  v = varargin{3};
  w = varargin{4};
else % quiver3(x,y,z,u,v,w) or quiver3(x,y,z,u,v,w,s)
  [msg,x,y,z] = xyzchk(varargin{1:3});
  u = varargin{4};
  v = varargin{5};
  w = varargin{6};
end
if ~isempty(msg), error(msg); end
% Scalar expand u,v,w.
if prod(size(u))==1, u = u(ones(size(x))); end
if prod(size(v))==1, v = v(ones(size(u))); end
if prod(size(w))==1, w = w(ones(size(v))); end
% Check sizes
if ~isequal(size(x),size(y),size(z),size(u),size(v),size(w))
  error('The sizes of X,Y,Z,U,V, and W must all be the same.');
end
% Get autoscale value if present
if nin==5 | nin==7, % quiver3(z,u,v,w,s) or quiver3(x,y,z,u,v,w,s)
  autoscale = varargin{nin};
end
if length(autoscale)>1,
  error('S must be a scalar.');
end
if autoscale,
  % Base autoscale value on average spacing in the x and y
  % directions.  Estimate number of points in each direction as
  % either the size of the input arrays or the effective square
  % spacing if x and y are vectors.
  if min(size(x))==1, n=sqrt(prod(size(x))); m=n; else [m,n]=size(x); end
  delx = diff([min(x(:)) max(x(:))])/n;
  dely = diff([min(y(:)) max(y(:))])/m;
  delz = diff([min(z(:)) max(y(:))])/max(m,n);
  del = sqrt(delx.^2 + dely.^2 + delz.^2);
  if del>0
    len = sqrt((u/del).^2 + (v/del).^2 + (w/del).^2);
    maxlen = max(len(:));
  else
    maxlen = 0;
  end

  if maxlen>0
    autoscale = autoscale*0.9 / maxlen;
  else
    autoscale = autoscale*0.9;
  end
  u = u*autoscale; v = v*autoscale; w = w*autoscale;
end
ax = newplot;
next = lower(get(ax,'NextPlot'));
hold_state = ishold;
% Make velocity vectors
x = x(:).'; y = y(:).'; z = z(:).';
u = u(:).'; v = v(:).'; w = w(:).';
uu = [x;x+u;repmat(NaN,size(u))];
vv = [y;y+v;repmat(NaN,size(u))];
ww = [z;z+w;repmat(NaN,size(u))];
h1 = plot3(uu(:),vv(:),ww(:),[col ls]);
if plotarrows,
  beta = beta * sqrt(u.*u + v.*v + w.*w) ./ (sqrt(u.*u + v.*v) + eps);
  uv=sqrt(u.*u + v.*v);
  % Make arrow heads and plot them
  hu = [x+u; x+u-alpha*(u+beta.*(v+eps)); ...
             x+u-alpha*(u-beta.*(v+eps)); ...
        x+u; x+u-alpha*u; x+u-alpha*u; x+u;...
        repmat(NaN,size(u))];
  hv = [y+v; y+v-alpha*(v-beta.*(u+eps)); ...
             y+v-alpha*(v+beta.*(u+eps)); ...
        y+v; y+v-alpha*v; y+v-alpha*v; y+v;...
        repmat(NaN,size(v))];
  hw = [z+w; z+w-alpha*w; z+w-alpha*w; ...
        z+w; z+w-alpha*(w+beta.*(uv+eps)); ...
             z+w-alpha*(w-beta.*(uv+eps)); z+w; ...
        repmat(NaN,size(w))];
  hold on
  h2 = plot3(hu(:),hv(:),hw(:),[col ls]);
else
  h2 = [];
end
if ~isempty(ms), % Plot marker on base
  hu = x; hv = y; hw = z;
  hold on
  h3 = plot3(hu(:),hv(:),hw(:),[col ms]);
  if filled, set(h3,'markerfacecolor',get(h1,'color')); end
else
  h3 = [];
end
if ~hold_state, hold off, view(3); grid on, set(ax,'NextPlot',next); end
if nargout>0, hh = [h1;h2;h3]; end


function [L1,L1_l,L1_x1] = reanchorPlucker(L0,x1)

% REANCHORPLUCKER  Plucker to anchored Plucker line conversion
%   REANCHORPLUCKER(L,X) reanchors the anchored Plucker line L to the point X.
%
%   [L1,L1_l,L1_x] = REANCHORPLUCKER(L,X) returns the Jacobians.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

x0 = L0(1:3);
n0 = L0(4:6);
v0 = L0(7:9);

L1 = [x1; n0 + cross((x0-x1),v0); v0];

if nargout > 1

    Z33 = zeros(3);
    I33 = eye(3);

    L1_l = [...
        Z33      Z33    Z33
        -hat(v0) I33 hat(x0-x1)
        Z33      Z33    I33];

    L1_x1 = [...
        I33
        hat(v0)
        Z33];

end

return

%%
syms t1 t2 t3 x1 x2 x3 a b c d n1 n2 n3 v1 v2 v3 y1 y2 y3 real

L0 = [x1;x2;x3;n1;n2;n3;v1;v2;v3];
y = [y1;y2;y3];

[L1,L1_l,L1_y] = reanchorPlucker(L0,y);

simplify(L1_l - jacobian(L1,L0))
simplify(L1_y - jacobian(L1,y))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


%% ODE_POOL uses the MATLABPOOL command to run the ODE code.
%
%  Discussion:
%
%    Output printed by the function appears directly on the screen.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2011
%
%  Author:
%
%    John Burkardt
%

%
%  Initialize the k and b ranges.
%
  bVals = 0.1 : 0.05 : 5;
  kVals = 1.5 : 0.05 : 5;
%
%  Begin the parameter sweep.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'ODE_POOL\n' );
  fprintf ( 1, '  Sweep through sets of values of parameters B and K,\n' );
  fprintf ( 1, '  computing the solution of the ODE corresponding to each set.\n' );
  fprintf ( 1, '  For each solution X(T), determine the maximum value over time.\n' );
  fprintf ( 1, '  Construct a contour plot of XMAX(B,K).\n' );
  fprintf ( 1, '  Use the PARFOR command to carry out these computations in parallel.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of K values = %d\n', length ( kVals ) );
  fprintf ( 1, '  Number of B values = %d\n', length ( bVals ) );
  fprintf ( 1, '  Number of times the ODE must be solved = %d\n', ...
    length ( kVals ) * length ( bVals ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Begin computation\n' );

  matlabpool open local 4
%
%  Solve the ODE for every pair of K and B values and return the maximum
%  value over the time interval.
%
  tic
  peakVals = ode_fun ( bVals, kVals );
  toc

  matlabpool close
%
%  Now display am image of the data.
%
  ode_display ( bVals, kVals, peakVals );


 function [xs, info] = qpwls_pcg1(x, A, W, yi, C, varargin)
%function [xs, info] = qpwls_pcg1(x, A, W, yi, C, [options])
%|
%| quadratic penalized weighted least squares (QPWLS) via
%| preconditioned conjugate gradients (PCG) algorithm
%| cost(x) = (y-Ax)'W(y-Ax) / 2 + x'C'Cx / 2
%|
%| in
%|	x	[np 1]		initial estimate
%|	A	[nd np]		system matrix
%|	W	[nd nd]		data weighting matrix, usually Gdiag(wi)
%|	yi	[nd 1]		noisy data
%|	C	[nc np]		penalty 'differencing matrix' (0 for unregularized)
%|
%| options
%|	niter			# total iterations (default: 1)
%|					(max # if tol used)
%|	isave	[]		list of iterations to archive (default: 'last')
%|	userfun	@		user defined function handle (see default below)
%|					taking arguments (x, userarg{:})
%|	userarg {}		user arguments to userfun (default {})
%|	precon	[np np]		preconditioner (matrix or object) (or 1)
%|	dircheck 0|1		check descent direction? (default: 1)
%|				set to 0 to save time, if you dare...
%|	stop_diff_tol		stop iterations if norm(xnew-xold)/norm(xnew)
%|				is less than this unitless value.  default: 0
%|	stop_diff_norm		use norm(.,type) for stop rule
%|				choices: 1 | 2 (default) | inf
%|	stop_grad_tol		stop if norm(grad) / y'W y < tol; default: 0
%|	stop_grad_norm		which norm(grad) to use.  default: 2
%|	chat	0|1		verbosity (default 0)
%|
%| out
%|	xs	[np niter]	estimates each iteration
%|	info	[niter 3]	gamma, step size, time each iteration
%|
%| Copyright Jan 1998, Jeff Fessler, University of Michigan

if nargin == 1 && streq(x, 'test'), qpwls_pcg1_test0, return, end
if nargin < 5, ir_usage, end

% defaults
arg.precon = 1;
arg.niter = 1;
arg.isave = [];
arg.userfun = @userfun_default;
arg.userarg = {};
arg.key = 1;
arg.dircheck = true; % default is to check descent direction
arg.stop_diff_tol = 0;
arg.stop_diff_norm = 2;
arg.stop_grad_tol = 0;
arg.stop_grad_norm = 2;
arg.chat = 0;

arg = vararg_pair(arg, varargin, 'subs', ...
{'stop_threshold', 'stop_diff_tol'; 'stop_norm_type', 'stop_diff_norm'});

arg.isave = iter_saver(arg.isave, arg.niter);
if arg.stop_diff_tol
	norm_diff = @(x) norm(x, arg.stop_diff_norm);
end
if arg.stop_grad_tol
	norm_grad = @(g) norm(g, arg.stop_grad_norm) / reale(yi' * (W * yi));
end

if ~isreal(yi) && ~isequal(arg.precon, 1)
	persistent warned
	if isempty(warned), warned = 0; end
	if ~warned
		warning 'not 100% sure about the complex preconditioned case'
		warned = 1;
	end
end

cpu etic
if isempty(x), x = zeros(ncol(A),1); end

x = x(:);
np = length(x);
xs = zeros(np, length(arg.isave), 'single');
if any(arg.isave == 0)
	xs(:, arg.isave == 0) = single(x);
end

%info = zeros(arg.niter, ?); % trick: do not initialize because size may change

% initialize projections
ticker(mfilename, 1, arg.niter)
Ax = A * x;
Cx = C * x;

% iterate
for iter = 1:arg.niter
	ticker(mfilename, iter, arg.niter)

	% (negative) gradient
	ngrad = A' * (W * (yi-Ax)) - C' * Cx;

	if arg.stop_grad_tol && norm_grad(ngrad) < arg.stop_grad_tol
		if arg.chat
			printm('stop at iteration %d with grad %g < %g', ...
				iter, norm_grad(ngrad), arg.stop_grad_tol)
		end
		if isequal(arg.isave, arg.niter) % saving last iterate only?
			xs = single(x); % save 'final' iterate
		else % saving many iterates?
			xs(:, arg.isave > iter) = []; % clear out unused
		end
	return
	end

	% preconditioned gradient
	pregrad = arg.precon * ngrad;

	% search direction
	newinprod = ngrad' * pregrad;
	% fix: should i take the real part?
	newinprod = reale(newinprod, 'warn', 'inprod');
	if iter == 1
		ddir = pregrad;
		gamma = 0;
	else
		if oldinprod == 0
			warn 'inprod=0. going nowhere!'
			gamma = 0;
		else
			gamma = newinprod / oldinprod;	% Fletcher-Reeves
%			gamma = (newinprod - oldgrad' * pregrad) / oldinprod;
		end
		ddir = pregrad + gamma * ddir;
	end
	oldgrad = ngrad;
	oldinprod = newinprod;

	% check if descent direction
	if arg.dircheck && real(ddir' * ngrad) < 0
		warn 'wrong direction; try using stop_grad_tol'
		ratio = norm(ngrad(:), arg.stop_grad_norm) / (yi'*W*yi);
		pr ratio % see how small it is
		if arg.key, keyboard, end
	end

	% step size in search direction
	Adir = A * ddir;
	Cdir = C * ddir;

	denom = Adir'*(W*Adir) + Cdir'*Cdir;
	denom = reale(denom, 'error', 'denom');
	if denom == 0
		warning 'found exact solution??? step=0 now!?'
		step = 0;
	else
		step = (ddir' * ngrad) / denom;
%		step = reale(step, 'warn', 'step');
		step = real(step); % real step sizes seems only logical
	end

	if step < 0
		warning 'downhill?'
		if arg.key, keyboard, end
	end

	% update
	Ax = Ax + step * Adir;
	Cx = Cx + step * Cdir;
	x = x + step * ddir;

	if any(arg.isave == iter)
		xs(:, arg.isave == iter) = single(x);
	end
	info(iter,:) = arg.userfun(x, arg.userarg{:});

	% check norm(xnew-xold) / norm(xnew) vs threshold
	if arg.stop_diff_tol && ...
		norm_diff(step * ddir) / norm_diff(x) < arg.stop_diff_tol
		if arg.chat
			ratio = norm_diff(step * ddir) / norm_diff(x);
			printm('stop at iteration %d with diff %g < %g', ...
				iter, ratio, arg.stop_diff_tol)
		end
		if isequal(arg.isave, arg.niter) % saving last iterate only?
			xs = single(x); % save the 'final' iterate
		else % saving many iterates?
			xs(:, arg.isave > iter) = []; % clear out unused
		end
	return
	end
end

% default user function.
% using this evalin('caller', ...) trick, one can compute anything of interest
function out = userfun_default(x, varargin)
gamma = evalin('caller', 'gamma');
step = evalin('caller', 'step');
out = [gamma step cpu('etoc')];

% qpwls_pcg1_test0()
function qpwls_pcg1_test0
mask = true([8 7]); mask(1) = false;
A = Gblur(mask, 'psf', ones(3)/9);
tmp = ones(size(mask));
A = Gdiag(tmp(mask), 'mask', mask);
xtrue = zeros(size(mask), 'single');
xtrue(end/2, round(end/2)) = 1;
y = A * xtrue(mask);
beta = 2^-7;
beta = 2^-2;
R = Reg1(mask, 'beta', beta, 'order', 1);
qpwls_psf(A, R.C, 1, mask, 1, 'loop', 0);
hess = full(A' * A + R.C' * R.C);
xhat = hess \ (A' * y);
xhat = embed(xhat, mask);
pr fwhm2(xhat)
im clf, im(xhat)

% user functions for tracking time and distance to a reference image
f.userfun = @(x, xref) [cpu('etoc') norm(x(:) - xref(:))];
f.userarg = {xhat(mask)}; % reference image just for testing

xinit = 0 * mask;
xpcg = qpwls_pcg1(xinit(mask(:)), A, 1, y, R.C, 'niter', 100, ...
	'userfun', f.userfun, 'userarg', f.userarg, ...
        'stop_grad_tol', 1e-8, 'stop_grad_norm', 2, ...
        'stop_diff_tol', 0e-6, 'stop_diff_norm', 2, 'chat', 1);
xpcg = embed(xpcg, mask);

im plc 2 2
im(1, xtrue)
im(2, xhat)
im(3, xpcg)
im(4, xpcg - xhat)
equivs(xpcg, xhat)


function g = gammaEval(j, k, LR, N, dim, nVars)
%GAMMAEVAL   Evaluate a gamma-function.
%   g = GAMMAEVAL(L, LR, N, DIM, NVARS) evaluates the gamma-function (J, K) with
%   the contour LR, N grid points, in dimension DIM and with NVARS variables.
%
% See also EXPINTEG/GAMMAFUN.

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

% Get a function handle to the phi function of index L:
g = expinteg.gammaFun(j, k);

% Evaluate it with a contour integral:
g = mean(feval(g, LR), 2);

% Reshape it when nVars>1 or/and dim>1:
g = reshape(g, nVars*N, N^(dim>1), N^(dim>2));

end


function Population = EnvironmentalSelection(Population,N)
% The environmental selection of CMOPSO

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Non-dominated sorting
    [FrontNo,MaxFNo] = NDSort(Population.objs,N);
    Next = false(1,length(FrontNo));
    Next(FrontNo<MaxFNo) = true;

    PopObj = Population.objs;
    fmax   = max(PopObj(FrontNo==1,:),[],1);
    fmin   = min(PopObj(FrontNo==1,:),[],1);
    PopObj = (PopObj-repmat(fmin,size(PopObj,1),1))./repmat(fmax-fmin,size(PopObj,1),1);

    %% Select the solutions in the last front
    Last = find(FrontNo==MaxFNo);
    del  = Truncation(PopObj(Last,:),length(Last)-N+sum(Next));
    Next(Last(~del)) = true;
    % Population for next generation
    Population = Population(Next);
end

function Del = Truncation(PopObj,K)
% Select part of the solutions by truncation

    N = size(PopObj,1);

    %% Truncation
    Distance = pdist2(PopObj,PopObj);
    Distance(logical(eye(length(Distance)))) = inf;
    Del = false(1,N);
    while sum(Del) < K
        Remain   = find(~Del);
        Temp     = sort(Distance(Remain,Remain),2);
        [~,Rank] = sortrows(Temp);
        Del(Remain(Rank(1))) = true;
    end
end


function [relativearea, name] = objectareas(D)

k = 0;
for i = 1:length(D);
    if isfield(D(i).annotation, 'object')
        Nobjects = length(D(i).annotation.object);

        nrows = D(i).annotation.imagesize.nrows;
        ncols = D(i).annotation.imagesize.ncols;

        for n = 1:Nobjects
            [X,Y] = getLMpolygon(D(i).annotation.object(n).polygon);

            area = polyarea(X,Y); % ignores intersections

            k = k+1;
            relativearea(k) = area/(nrows*ncols);
            name{k} = D(i).annotation.object(n).name;
        end
    end
end


% Test whether it is necessary to rotate VB PCA
function nc2010_publish_datasets

n = 200;
m = 50;
d = 10;

ncomps = 30;

randn('state', 1);
rand('state', 1);

% Generate multivariate normal data
disp('Using weak subspace')
eigs = (1 + [d:-1:1 zeros(1,m-d)]) .^ 2
datastring = 'weak';
plot_eigenvalues(eigs, datastring);

disp('Using strong subspace')
eigs = ([5*ones(1,d) 1*ones(1,m-d)]) .^ 2
datastring = 'strong';
plot_eigenvalues(eigs, datastring);

disp('Using no subspace')
eigs = (m:-1:1) .^ 2
datastring = 'no';
plot_eigenvalues(eigs, datastring);

function plot_eigenvalues(eigs, datastring)

filename = sprintf(['/home/jluttine/papers/neurocomputing2010/' ...
                    'fig_eigenvalues_dataset=%s'], datastring);

figure
plot(sqrt(eigs), 'k-');
set(gcf, 'units', 'centimeters', 'paperunits', 'centimeters');
pos = get(gcf, 'position');
set(gcf, 'position', [pos(1:2), 6,4]);
pos = get(gcf, 'paperposition');
set(gcf, 'paperposition', [pos(1:2),6,4])

ylim([0, max(sqrt(eigs))+1]);
xlim([1, length(eigs)]);

%ylabel('standard deviation')

print(gcf, '-depsc2', filename);


% Digital Video Stabilization and Rolling Shutter Correction using Gyroscopes
% Copyright (C) 2011 Alexandre Karpenko
%
% 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
% 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/>.

function [dth, theta] = frame_rotations_non_lin(gyro, gyro_time, frame_time, t0, ts)
% computes the delta in theta between frame times, as well as the shear
% at frame time
dgt = diff(gyro_time);
theta = ((gyro(1:end-1,:) + gyro(2:end,:)) / 2) .* dgt(:,[1 1 1]);
theta = [0 0 0; cumsum(theta, 1)];

sigma2 = 4000;
gauss = exp(-(-120:120).^2 / sigma2);
gauss = gauss ./ sum(gauss);
gyro(:,1) = gyro(:,1) - conv(gyro(:,1), gauss, 'same');
gyro(:,2) = gyro(:,2) - conv(gyro(:,2), gauss, 'same');
gyro(:,3) = gyro(:,3) - conv(gyro(:,3), gauss, 'same');
th = ((gyro(1:end-1,:) + gyro(2:end,:)) / 2) .* dgt(:,[1 1 1]);
th = [0 0 0; cumsum(th, 1)];
dth = diff(lininterp(gyro_time + t0 + ts/2, th, frame_time));


function dUpsilon = lfmvpGradientUpsilonMatrix(gamma, sigma2, t1, ...
    t2, mode, upsilon)

% LFMVPGRADIENTUPSILONMATRIX Gradient upsilon matrix vel. pos.
% FORMAT
% DESC computes the gradient of a portion of the LFM kernel.
% ARG gamma : Gamma value for system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG upsilon : precomputation of the upsilon matrix.
% ARG mode : operation mode, according to the derivative (mode 0,
% derivative wrt t1, mode 1 derivative wrt t2)
% RETURN upsilon : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio A. Alvarez, 2010
%
% SEEALSO : lfmvpComputeUpsilonMatrix.m

% KERN

sigma = sqrt(sigma2);

if nargin<6
    upsilon = lfmComputeUpsilonMatrix(gamma, sigma2, t1, t2);
    if nargin <5
        mode =0;
    end
end

if mode ==0
    dUpsilon = -upsilon - gamma*lfmGradientUpsilonMatrix(gamma, sigma2, t1, t2);
else
    dUpsilon = upsilon + gamma*lfmGradientUpsilonMatrix(gamma, sigma2, t1, t2) ...
        - (2/(sqrt(pi)*sigma))*(t1.*exp(-gamma*t1))*(exp(-(t2.^2)/sigma2)).';
end


function y=comb(x)

% This function is used to create all possible combinations
% of Input Term Nodes Outputs to be fed into the Rule Nodes.
% Each combination is fed to a single rule neuron responsible
% for the processing of this specific combination.

[rows columns] = size(x);

y = zeros(columns^rows,rows);

for i=1:rows

    if i<=rows-1

      j=1;

      for m=0:columns^(rows-i):(columns^rows - columns^(rows-i))

        if j<=columns

             for l=1:columns^(rows-i)
                      y(m+l,i) = x(i,j);
             end

         else

            j=1;
             for l=1:columns^(rows-i)
                     y(m+l,i) = x(i,j);
             end

         end

         j = j + 1;

      end

    elseif i==rows

        for m=0:columns:(columns^rows - columns)

            j=1;
            for l=1:columns
                y(m+l,i) = x(i,j);
                j=j+1;
            end
        end

    end    % end of "if i" loop.

end      % end of " for i" loop.


function dat= func_powerspectrum (data, band, varargin)
% func_powerspectrum :
% This function calculates the power spectrum
%
% Example:
%      dat= func_powerspectrum(dat, [8 15], {'win',[];'step',100})
%
% Input:
%      dat  - data structure of continuous or epoched data
%      band - frequency band
%
% Options:
%      win  - window for FFT, default ones(dat.fs, 1)
%      N    - window width for FFT -> square window, default dat.fs
%      step - step for window (= # of overlapping samples), default N/2
%      db_scaled - boolean, if true values are db scaled (10*log10),
%                  default true
% Retuns:
%     dat - Data structure of power specturm result
%%
% data
dat=data;
ban=band;
opt=opt_cellToStruct(varargin{:});
epo=struct('win',[],'N',[],'step',[],'scale',[]);

if isempty(dat)
    warning('Warning! data is empty');
end
if isempty(ban)
    warning('Band is not exist.');
end

if ~isfield(opt,'win')
   epo.win=dat.fs;
else
    epo.win=opt.win;
end

if ~isfield(opt,'N')
   epo.N=dat.fs;
else
    epo.N=opt.N;
end

if ~isfield(opt,'step')
   epo.step=dat.fs/2;
else
   epo.N=opt.N;
end

if ~isfield(opt,'scale')
   epo.scale='db';
else
    epo.scale=opt.scale;
end

[T, nEvents , nChans]= size(dat.x);

if length(epo.win)==1
    if epo.win>T
        warning('window legth is higher than signal')
    end
    epo.win=ones(epo.win,1);
end
N=length(epo.win);
normwin=norm(epo.win);
Freq=(0:N)/2*dat.fs/N;

%%
XX= zeros(N, nChans*nEvents);
nWindows= 1 + max(0, floor((T-N)/epo.step));
iv= 1:min(N, T);
Win= repmat(epo.win(:), [1 nChans*nEvents]);
bInd= band(1): ban(2);

%%calculate file

switch(lower(epo.scale)),
    case 'db',
        for iw= 1:nWindows,
            XX= XX + abs(fft(dat.x(iv,:).*Win, N)).^2;
            iv= iv + epo.step;
        end
        XX = XX/(nWindows*normwin^2);
        dat.x= reshape( 10*log10( XX(bInd,:)+eps ), [length(bInd), nChans, nEvents]);
        dat.yUnit= 'dB';
    case 'power',
        for iw= 1:nWindows,
            XX= XX + abs(fft(dat.x(iv,:).*Win, N).^2);
            iv= iv + epo.step;
        end
        dat.x= reshape(XX(bInd,:)/(nWindows*normwin^2), [length(bInd), nChans, nEvents]);
        dat.yUnit= 'power';
end

end