function [x,g,j,gg] = kmeans(d,k,x0,l)
%KMEANS Vector quantisation using K-means algorithm [X,ESQ,J]=(D,K,X0,L)
%
%  Inputs:
%
%    D(N,P)  contains N data vectors of dimension P
%    K       is number of centres required
%    X0(K,P) are the initial centres (optional)
%
%      or alternatively
%
%    X0      gives the initialization method
%            'f'   pick K random elements of D as the initial centres [default]
%            'p'   randomly divide D into K sets and choose the centroids
%    L       gives max number of iterations (use 0 if you just want to calculate G and J)
%
%  Outputs:
%
%    X(K,P)  is output row vectors (omitted if L=0)
%    G       is mean square error
%    J(N)    indicates which centre each data vector belongs to
%    GG(L)   gives the mean square error at the start of each iteration (omitted if L=0)
%
% It is often a good idea to scale the input data so that it has equal variance in each
% dimension before calling KMEANS.

%  Originally based on a routine by Chuck Anderson, anderson@cs.colostate.edu, 1996

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: kmeans.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

memsize=voicebox('memsize');
[n,p] = size(d);
nb=min(n,max(1,floor(memsize/(8*p*k))));    % block size for testing data points
nl=ceil(n/nb);                  % number of blocks
if nargin<4
    l=300;                  % very large max iteration count
    if nargin<3
        x0='f';             % use 'f' initialization mode
    end
end
if ischar(x0)
    if k<n
        if any(x0)=='p'                  % Initialize using a random partition
            ix=ceil(rand(1,n)*k);       % allocate to random clusters
            ix(rnsubset(k,n))=1:k;      % but force at least one point per cluster
            x=zeros(k,p);
            for i=1:k
                x(i,:)=mean(d(ix==i,:),1);
            end
        else                                % Forgy initialization: choose k random points [default]
            x=d(rnsubset(k,n),:);         % sample k centres without replacement
        end
    else
        x=d(mod((1:k)-1,n)+1,:);    % just include all points several times
    end
else
    x=x0;
end
m=zeros(n,1);           % minimum distance to a centre
j=zeros(n,1);           % index of closest centre
gg=zeros(l,1);
wp=ones(1,p);
kk=1:p;
kk=kk(ones(n,1),:);
kk=kk(:);

if l>0
    for ll=1:l                 % loop until x==y causes a break

        % find closest centre to each data point [m(:),j(:)] = distance, index

        ix=1;
        jx=n-nl*nb;
        for il=1:nl
            jx=jx+nb;        % increment upper limit
            ii=ix:jx;
            z = disteusq(d(ii,:),x,'x');
            [m(ii),j(ii)] = min(z,[],2);
            ix=jx+1;
        end
        y = x;              % save old centre list

        % calculate new centres as the mean of their assigned data values (or zero for unused centres)

        nd=full(sparse(j,1,1,k,1));         % number of points allocated to each centre
        md=max(nd,1);                       % remove zeros
        jj=j(:,wp);
        x=full(sparse(jj(:),kk,d(:),k,p))./md(:,wp);    % calculate the new means
        fx=find(nd==0);

        % if any centres are unused, assign them to data values that are not exactly on centres
        % choose randomly if there are more such points than needed

        if ~isempty(fx)
            q=find(m~=0);
            if length(q)<=length(fx)
                x(fx(1:length(q)),:)=d(q,:);
            else
                if length(fx)>1
                    [rr,ri]=sort(rand(length(q),1));
                    x(fx,:)=d(q(ri(1:length(fx))),:);
                else
                    x(fx,:) = d(q(ceil(rand(1)*length(q))),:);
                end
            end
        end

        % quit if the centres are unchanged

        gg(ll)=sum(m,1);
        if x==y
            break
        end
    end
    gg=gg(1:ll)/n;
%     ll % *** DEBUG ***
%     gg' % *** DEBUG ***
    g=gg(end);
else            % if l==0 then just calculate G and J (but rename as X and G)
    ix=1;
    jx=n-nl*nb;
    for il=1:nl
        jx=jx+nb;        % increment upper limit
        ii=ix:jx;
        z = disteusq(d(ii,:),x,'x');
        [m(ii),j(ii)] = min(z,[],2);
        ix=jx+1;
    end
    x=sum(m,1)/n;
    g=j;
end


function [sys,x0,str,ts]=ADRC_2(t,x,u,flag,h,TD,ESO,NLSEF,b0)

switch flag
    case 0
        [sys,x0,str,ts]=mdlInitializeSizes(h);
    case 2
        sys=mdlUpdate(x,u,h,TD,ESO,b0);
    case 3
        sys=mdlOutputs(x,NLSEF,b0);
    case 4,
        sys=mdlGetTimeOfNextVarHit(t,h);
    case {1,9}
        sys=[];
    otherwise
        error(['Unhandled flag=',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes(h)
    sizes=simsizes;
    sizes.NumContStates=0;
    sizes.NumDiscStates=5;
    sizes.NumOutputs=2;
    sizes.NumInputs=3;
    sizes.DirFeedthrough=1;
    sizes.NumSampleTimes=1;
    sys=simsizes(sizes);
    x0=[0;0;0;0;0];
    str=[];
    ts=[h 0];
function sys=mdlUpdate(x,u,h,TD,ESO,b0)
    e1=x(1)-u(1);
    fh=fhan(e1,x(2),TD(1),TD(2));
    sys(1)=x(1)+h*x(2);
    sys(2)=x(2)+h*fh;
    e2=x(3)-u(2);
    sys(3)=x(3)+h*(x(4)-ESO(1)*e2);
    sys(4)=x(4)+h*(x(5)-ESO(2)*fal(e2,0.5,ESO(4))+b0*u(3));
    sys(5)=x(5)+h*(-ESO(3)*fal(e2,0.25,ESO(4)));
%     fe=fal(e,0.5,Delta);
%     fe1=fal(e,0.25,Delta);
%     sys(1,1)=x(1)+h2*(x(2)-BB(1)*e);
%     sys(2,1)=x(2)+h2*(x(3)-BB(2)*fe+u(1));
%     sys(3,1)=x(3)+h2*(-BB(3)*fe1);
function sys=mdlOutputs(x,NLSEF,b0)
    e3=x(1)-x(3);
    e4=x(2)-x(4);
    sys(1)=-fhan(e3,NLSEF(1)*e4,NLSEF(2),NLSEF(3))-x(5)/b0;
    sys(2)=x(1);
function sys=mdlGetTimeOfNextVarHit(t,h)
    sys=t+h;

function y=fhan(x1,x2,r,h)
d=r*h;
d0=h*d;
y=x1+h*x2;
a0=sqrt(d^2+8*r*abs(y));
if abs(y)>d0
    a=x2+(a0-d)*sign(y)/2;
else
    a=x2+y/h;
end

if abs(a)>d
    y=-r*sign(a);
else
    y=-r*a/d;
end

function f=fal(e,a,d)
    if abs(e)<d
        f=e*d^(a-1);
    else f=(abs(e))^a*sign(e);
    end


function results = lowrank_sparse_fista( data, params, funs )
% FISTA implementation for solving
%   min 0.5*||AY - T|| + \lambda_*\sum_i||Xi,(i)||_* + \lambda1*||E||_1
% where Z = [ X1,...,X_N,E ]', Y is the FISTA auxiliary variable,
% A is a linear operator, T is given observation.

% tic;
% function handles
eval_f = funs.f;
eval_grad_f = funs.grad_f;
Aprod = funs.Aprod;
Atprod = funs.Atprod;
solve_prox_grad = funs.prox_grad_solver;

T = data.T;
if params.IsTC
    T = data.b;
end
N = length( size(params.X0) );
Y = cell(1,N+1);
for i = 1:N
    Y{i} = params.X0;
end
Y{N+1} = params.E0;
Z = Y;

use_cont = isfield( params, 'use_cont' ) && params.use_cont;
lambda1 = params.lambda;
lambdaS = params.lambdaS;
if use_cont
    if lambdaS > 0
        lambdaS_min = lambdaS;
        r = lambda1 / lambdaS;      lambdaS = 0.99*norm(T);     lambda1 = lambdaS*r;
    else
        r = params.rRatio /sqrt( max(size(data.T)) );
        lambdaS = 0.99*norm(T);     lambda1 = lambdaS*r;
        lambdaS_min = lambdaS * 1e-5;
    end
    eta = 0.97;
end

mu = params.mu0;
beta = 0.5;
t = 0;

fZ = eval_f( Z, Aprod, T );
trnorm = 0;
oneNorm = 0;

for iter = 1:params.max_iter

    % compute gradient at Y
    grad_f = eval_grad_f( Y, Aprod, Atprod, T );

    Zp = Z;     trnorm_p = trnorm;    fZp = fZ;     oneNorm_p = oneNorm;
    [ Z, trnorm ] = solve_prox_grad( Y, mu, grad_f, lambdaS, lambda1 );
    % backtrack linesearch
    fY = eval_f( Y, Aprod, T );
    ZYdiff = tensor_array_diff( Z, Y );
    approx = fY + tensor_array_innerprod( grad_f, ZYdiff ) + tensor_array_norm(ZYdiff)^2 / (2*mu);
    fZ = eval_f( Z, Aprod, T );
    while fZ > approx
        mu = mu * beta;
        [ Z, trnorm ] = solve_prox_grad( Y, mu, grad_f, lambdaS, lambda1 );
        ZYdiff = tensor_array_diff( Z, Y );
        approx = fY + tensor_array_innerprod( grad_f, ZYdiff ) + tensor_array_norm(ZYdiff)^2 / (2*mu);
        fZ = eval_f( Z, Aprod, T );
    end
    oneNorm = tensor_1norm(Z{N+1});

    % FISTA acceleration step
    tp = t;
    t = (1 + sqrt(1+4*tp^2))/2;
    Zdiff = tensor_array_diff( Z, Zp );
    Y = tensor_array_add( Zp, tensor_array_scale(Zdiff,(tp-1)/t) );

    % compute optimality stats
    Fp = fZp + lambdaS*trnorm_p + lambda1*oneNorm_p;
    F = fZ + lambdaS*trnorm + lambda1*oneNorm;
    rel_F = abs(F-Fp)/Fp;
    denom = tensor_array_norm(Zp);
    rel_X = tensor_array_norm(Zdiff) / denom;   if denom == 0; rel_X = 1; end
    rel_err = norm(ten_sum_all(Z(1:N))-data.X) / norm(data.X);

    % print
    if params.verbose && rem( iter, 50 ) == 0
        fprintf('Iter: %d,   fit: %3.2e,   rel_F: %3.2e,    rel_X: %3.2e,   rel_err: %3.2e, S: %3.2e\n', ...
            iter, fZ, rel_F, rel_X, rel_err, lambdaS);
    end

    if max(rel_F,rel_X) < params.opt_tol
        break;
    end

    % update lambda's
    if use_cont
        lambdaS = max( lambdaS*eta, lambdaS_min );
        lambda1 = lambdaS*r;
    end
end

results.vars = Z;
results.V = params.V0;
results.T = data.T;
results.iter = iter;
% results.cpu = toc;
results.mu = mu;
results.lambda1 = lambda1;
results.lambdaS = lambdaS;
results.IsTC = params.IsTC;

end


% some variables needed
u = P.x_trim(4);
v = P.x_trim(5);
w = P.x_trim(6);
Va_trim = sqrt(u^2 + v^2 + w^2);
theta_trim = P.x_trim(8);
alpha_trim = atan(w/u);
delta_e_trim = P.u_trim(1);
delta_t_trim = P.u_trim(4);

% Roll attitude hold
P.delta_a_max = 45 * pi / 180;
P.phi_max = 15 * pi / 180;
P.roll_max = 45 * pi / 180;
a_phi1 = -0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_p * P.b / (2 * Va_trim);
a_phi2 = 0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_delta_a;
P.kp_phi = P.delta_a_max / P.phi_max * sign(a_phi2);
omega_phi = sqrt(abs(a_phi2) * P.delta_a_max / P.phi_max);
zeta_phi = 1.2;   % design parameter
P.kd_phi = (2 * zeta_phi * omega_phi - a_phi1) / a_phi2;
P.ki_phi = 0.2;    % design parameter

% Course hold
W_chi = 8;  % design parameter
omega_chi = 1 / W_chi * omega_phi;
zeta_chi = 0.707;   % design parameter
Vg = P.Va0;
P.kp_chi = 2 * zeta_chi * omega_chi * Vg / P.gravity;
P.ki_chi = omega_chi^2 * Vg / P.gravity;

% Pitch attitude hold
P.delta_e_max = 45 * pi / 180;
P.e_theta_max = 10 * pi / 180;
a_theta1 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_q * P.c / (2 * P.Jy * 2 * Va_trim);
a_theta2 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_alpha / (2 * P.Jy);
a_theta3 = P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_delta_e / (2 * P.Jy);
P.kp_theta = P.delta_e_max / P.e_theta_max * sign(a_theta3);
omega_theta = sqrt(a_theta2 + P.delta_e_max / P.e_theta_max * abs(a_theta3));
zeta_theta = 0.707;     % design parameter
P.kd_theta = (2 * zeta_theta * omega_theta - a_theta1) / a_theta3;
P.theta_max = 20 * pi / 180;
P.pitch_max = 40 * pi / 180;

% Airspeed hold using Throttle
a_V1 = P.rho * Va_trim * P.S_wing / P.mass ...
       * (P.C_D_0 + P.C_D_alpha * alpha_trim + P.C_L_delta_e * delta_e_trim) ...
       + P.rho * P.S_prop / P.mass * P.C_prop * Va_trim;
a_V2 = P.rho * P.S_prop / P.mass * P.C_prop * P.k_motor^2 * delta_t_trim;
a_V3 = P.gravity * cos(theta_trim - alpha_trim);
omega_v = 5;   % design parameter
zeta_v = 0.707;     % design parameter
P.delta_t_max = 1;
P.delta_t_min = 0;
P.ki_v = omega_v^2 / a_V2;
P.kp_v = (2 * zeta_v * omega_v - a_V1) / a_V2;

% Airspeed hold using Pitch
W_v2 = 7;   % design parameter
zeta_v2 = 0.707;    % design parameter
omega_v2 = 1 / W_v2 * omega_theta;
K_theta_dc = P.kp_theta * a_theta3 / (a_theta2 + P.kp_theta * a_theta3);
P.ki_v2 = -omega_v2^2/(K_theta_dc * P.gravity);
P.kp_v2 = (a_V1 - 2 * zeta_v2 * omega_v2) / (K_theta_dc * P.gravity);

% Altitude hold using Pitch
W_h = 10;   % design parameter
omega_h = 1 / W_h * omega_theta;
Va = P.Va0;
zeta_h = 1.2;     % design parameter
P.h_max = 1000;
P.h_min = 0;
P.ki_h = omega_h^2 / (K_theta_dc * Va);
P.kp_h = 2 * zeta_h * omega_h / (K_theta_dc * Va);


function y = centroid(M, x)
% Attempts the computation of a centroid of a set of points on a manifold.
%
% function y = centroid(M, x)
%
% M is a structure representing a manifold.
% x is a cell of points on that manifold.

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:

    % For now, just apply a few steps of gradient descent for Karcher means

    n = numel(x);

    problem.M = M;

    problem.cost = @cost;
    function val = cost(y)
        val = 0;
        for i = 1 : n
            val = val + M.dist(y, x{i})^2;
        end
        val = val/2;
    end

    problem.grad = @grad;
    function g = grad(y)
        g = M.zerovec(y);
        for i = 1 : n
            g = M.lincomb(y, 1, g, -1, M.log(y, x{i}));
        end
    end

    % This line can be uncommented to check that the gradient is indeed
    % correct. This should always be the case if the dist and the log
    % functions in the manifold are correct.
    % checkgradient(problem); pause;

    query = warning('query', 'manopt:getHessian:approx');
    warning('off', 'manopt:getHessian:approx');
    options.verbosity = 0;
    options.maxiter = 15;
    y = trustregions(problem, x{randi(n)}, options);
    warning(query.state, 'manopt:getHessian:approx');

end


function [ edge_pointer, edge_data, xy ] = grf_example ( node_num, edge_num )

%*****************************************************************************80
%
%% GRF_EXAMPLE sets up a GRF example.
%
%  Discussion:
%
%    The example is known as the Coxeter graph.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Stephen Skiena,
%    Implementing Discrete Mathematics,
%    Combinatorics and Graph Theory with Mathematica,
%    Addison-Wesley, 1990.
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer EDGE_NUM, the number of edges.
%
%    Output, integer EDGE_POINTER(NODE_NUM+1), pointers to
%    the beginning of edge data for each node.
%
%    Output, integer EDGE_DATA(EDGE_NUM), the edge data.
%
%    Output, real XY(2,NODE_NUM), the node coordinates.
%
  edge_pointer = [ ...
    1,  4,  7, 10, 13, 16, 19, 22, 25, 28, ...
   31, 34, 37, 40, 43, 46, 49, 52, 55, 58, ...
   61, 64, 67, 70, 73, 76, 79, 82, 85 ]';

  edge_data = [ ...
     8,   2,   3, ...
    14,   1,   5, ...
     9,   4,   1, ...
    10,   7,   3, ...
    13,   2,   6, ...
    12,   5,   7, ...
    11,   6,   4, ...
    25,  20,   1, ...
    24,  21,   3, ...
    23,  15,   4, ...
    22,  16,   7, ...
    28,  17,   6, ...
    27,  18,   5, ...
    26,  19,   2, ...
    10,  18,  19, ...
    11,  19,  20, ...
    12,  21,  20, ...
    13,  15,  21, ...
    14,  16,  15, ...
     8,  17,  16, ...
     9,  18,  17, ...
    11,  27,  24, ...
    10,  28,  25, ...
     9,  26,  22, ...
     8,  23,  27, ...
    14,  24,  28, ...
    13,  25,  22, ...
    12,  26,  23 ]';

  xy = [ ...
    0.412,   0.984; ...
    0.494,   0.984; ...
    0.366,   0.926; ...
    0.388,   0.862; ...
    0.546,   0.926; ...
    0.518,   0.860; ...
    0.458,   0.818; ...
    0.152,   0.684; ...
    0.264,   0.682; ...
    0.354,   0.680; ...
    0.458,   0.670; ...
    0.554,   0.672; ...
    0.658,   0.668; ...
    0.774,   0.692; ...
    0.164,   0.450; ...
    0.228,   0.448; ...
    0.274,   0.390; ...
    0.242,   0.330; ...
    0.194,   0.278; ...
    0.146,   0.328; ...
    0.102,   0.390; ...
    0.668,   0.472; ...
    0.638,   0.416; ...
    0.656,   0.334; ...
    0.714,   0.270; ...
    0.798,   0.326; ...
    0.830,   0.408; ...
    0.754,   0.466 ]';

  return
end


function I = domIntegral4(data)
%+========================================================================+
%|                                                                        |
%|              OPENDOM - LIBRARY FOR NUMERICAL INTEGRATION               |
%|           openDom is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal & Francois Alouges (c) 2017-2018.          |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : domIntegral4.m                                |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Numerical integation with 4 input arguments   |
%|  `---'  |                                                              |
%+========================================================================+

%%% FINITE ELEMENT OPERATOR --> \int_{mesh(x)} psi(x)' f(x) psi(x) dx
if isa(data{1},'dom') && isa(data{2},'fem')
    % Domain with quadrature
    Xdom   = data{1};
    [X,Wx] = Xdom.qud;
    Nx     = size(X,1);
    Wx     = spdiags(Wx,0,Nx,Nx);

    % Integrated finite element matrix
    u  = data{2};
    Mu = u.uqm(Xdom);
    if iscell(Mu)
        Mu{1} = Mu{1}' * Wx;
        Mu{2} = Mu{2}' * Wx;
        Mu{3} = Mu{3}' * Wx;
    else
        Mu = Mu' * Wx;
    end

    % Function evaluation on quadrature
    F = data{3};
    if iscell(F)
        Fx{1} = spdiags(F{1}(X),0,Nx,Nx);
        Fx{2} = spdiags(F{2}(X),0,Nx,Nx);
        Fx{3} = spdiags(F{3}(X),0,Nx,Nx);
    else
        Fx = spdiags(F(X),0,Nx,Nx);
    end

    % Finite element matrix
    v  = data{4};
    Mv = v.uqm(Xdom);

    % Integration
    I = femMultiplyCell(Mu,Fx,Mv);

%%% BOUNDARY ELEMENT INTEGRATION --> \int_{mesh(y)} f(x,y) psi(y) dy
elseif isnumeric(data{1}) && isa(data{2},'dom')
    % Evaluation points
    X  = data{1};
    Nx = size(X,1);

    % Domain with quadrature
    Ydom   = data{2};
    [Y,Wy] = Ydom.qud;
    Ny     = size(Y,1);
    Wy     = spdiags(Wy,0,Ny,Ny);

    % Function evaluation on quadrature
    F = data{3};
    if iscell(F)
        Fxy = {zeros(Nx,Ny),zeros(Nx,Ny),zeros(Nx,Ny)};
        for i = 1:3
            for j = 1:Ny
                Fxy{i}(:,j) = F{i}(X,Y(j,:));
            end
        end
    else
        Fxy = zeros(Nx,Ny);
        for j = 1:Ny
            Fxy(:,j) = F(X,Y(j,:));
        end
    end

    % Integrated finite element matrix
    v  = data{4};
    Mv = v.uqm(Ydom);
    if iscell(Mv)
        Mv{1} = Wy * Mv{1};
        Mv{2} = Wy * Mv{2};
        Mv{3} = Wy * Mv{3};
    else
        Mv = Wy * Mv;
    end

    % Integration
    I = femMultiplyCell(Fxy,Mv);

%%% BOUNDARY ELEMENT INTEGRATION --> \int_{mesh(x)} psi(x)' f(x,y) dx
elseif isa(data{1},'dom') && isnumeric(data{2})
    % Domain with quadrature
    Xdom   = data{1};
    [X,Wx] = Xdom.qud;
    Nx     = size(X,1);
    Wx     = spdiags(Wx,0,Nx,Nx);

    % Evaluation points
    Y  = data{2};
    Ny = size(Y,1);

    % Integrated finite element matrix
    u  = data{3};
    Mu = u.uqm(Xdom);
    if iscell(Mu)
        Mu{1} = Mu{1}' * Wx;
        Mu{2} = Mu{2}' * Wx;
        Mu{3} = Mu{3}' * Wx;
    else
        Mu = Mu' * Wx;
    end

    % Function evaluation on quadrature
    F = data{4};
    if iscell(F)
        Fxy = {zeros(Nx,Ny),zeros(Nx,Ny),zeros(Nx,Ny)};
        for i = 1:3
            for j = 1:Ny
                Fxy{i}(:,j) = F{i}(X,Y(j,:));
            end
        end
    else
        Fxy = zeros(Nx,Ny);
        for j = 1:Ny
            Fxy(:,j) = F(X,Y(j,:));
        end
    end

    % Integration
    I = femMultiplyCell(Mu,Fxy);

else
    error('domIntegral4.m : unavailable case')
end
end


function [ceq,ceqJac] = autoGen_cst_steplength(q1m,q2m,q4m,q5m,l1,l2,l4,l5,stepLength)
%AUTOGEN_CST_STEPLENGTH
%    [CEQ,CEQJAC] = AUTOGEN_CST_STEPLENGTH(Q1M,Q2M,Q4M,Q5M,L1,L2,L4,L5,STEPLENGTH)

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

t2 = cos(q1m);
t3 = l1.*t2;
t4 = cos(q2m);
t5 = l2.*t4;
t6 = cos(q4m);
t7 = cos(q5m);
t8 = sin(q1m);
t9 = sin(q2m);
t10 = sin(q4m);
t11 = l4.*t10;
t12 = sin(q5m);
t13 = l5.*t12;
ceq = [-stepLength+t11+t13-l1.*t8-l2.*t9;t3+t5-l4.*t6-l5.*t7];
if nargout > 1
    ceqJac = reshape([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,-t3,-l1.*t8,-t5,-l2.*t9,0.0,0.0,l4.*t6,t11,l5.*t7,t13,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2, 22]);
end


function [ p, t ] = cvt_square_uniform ( n, sample_num, delaunay_display )

%*****************************************************************************80
%
%% CVT_SQUARE_UNIFORM demonstrates how a CVT can be computed and displayed in MATLAB.
%
%  Discussion:
%
%    This simple example carries out an iterative CVT calculation in the
%    unit square, with a uniform density.  The initial placement of the
%    generators is random.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Qiang Du, Vance Faber, Max Gunzburger,
%    Centroidal Voronoi Tessellations: Applications and Algorithms,
%    SIAM Review,
%    Volume 41, 1999, pages 637-676.
%
%  Parameters:
%
%    Input, integer N, the number of generators.
%
%    Input, integer SAMPLE_NUM, the number of sample points.
%
%    Input, logical DELAUNAY_DISPLAY, is TRUE (nonzero) if the Delaunay
%    triangulation is to be displayed.
%
%    Output, real P(N,2), the location of the generators.
%
%    Output, integer T(NT,3), information defining the Delaunay
%    triangulation of the generators.  NT is the number of triangles,
%    which varies depending on the arrangement of the generators.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_SQUARE_UNIFORM:\n' );
  fprintf ( 1, '  A simple demonstration of a CVT computation\n' );
  fprintf ( 1, '  (Centroidal Voronoi Tessellation)\n' );
  fprintf ( 1, '  in a square, with a uniform density.\n' );

  if ( nargin < 1 )
    n = 100;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_SQUARE_UNIFORM - Note:\n' );
    fprintf ( 1, '  No value of N was supplied.\n' );
    fprintf ( 1, '  N is the number of generators.\n' );
    fprintf ( 1, '  A default value N = %d will be used.\n', n );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  User specified number of generators = %d\n',  n );
  end

  if ( nargin < 2 )
    sample_num = 1000 * n;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_SQUARE_UNIFORM - Note:\n' );
    fprintf ( 1, '  No value of SAMPLE_NUM was supplied.\n' );
    fprintf ( 1, '  SAMPLE_NUM is the number of sample points.\n' );
    fprintf ( 1, '  A default value SAMPLE_NUM = %d will be used.\n', ...
      sample_num );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  User specified number of sample points = %d\n', ...
      sample_num );
  end

  if ( nargin < 3 )
    delaunay_display = 0;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_SQUARE_UNIFORM - Note:\n' );
    fprintf ( 1, '  No value of DELAUNAY_DISPLAY was supplied.\n' );
    fprintf ( 1, '  DELAUNAY_DISPLAY is TRUE (nonzero) if the\n' );
    fprintf ( 1, '  Delaunay triangulation is also to be displayed.\n' );
    fprintf ( 1, '  A default value DELAUNAY_DISPLAY = %d will be used.\n', ...
      delaunay_display );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  User specified DELAUNAY_DISPLAY = %d\n', ...
      delaunay_display );
  end
%
%  This switch is set to 1 (TRUE) if the ACCUMARRAY command is available.
%  That speeds up the calculation a lot.  If you don't have the ACCUMARRAY
%  command, just set this to 0.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_SQUARE_UNIFORM:\n' );
  fprintf ( 1, '  MATLAB''s ACCUMARRAY command can be used for faster\n' );
  fprintf ( 1, '  computation.  This command is not available in\n' );
  fprintf ( 1, '  some versions of MATLAB.  If ACCUMARRAY is available,\n' );
  fprintf ( 1, '  simply make sure that the ACCUMARAY_AVAILABLE variable\n' );
  fprintf ( 1, '  is set to 1!\n' );

  accumarray_available = 1;

  if ( accumarray_available )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  The ACCUMARRAY command will be used.\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  The ACCUMARRAY command will NOT be used.\n' );
  end
%
%  Clear the figure screen, if already open.
%
  clf
%
%  Randomize the initial locations of the generators in the unit square.
%  If another region is used, then this initialization should be changed.
%
  p = rand ( n, 2 );
  plot ( p(:,1), p(:,2), 'b.' );
  axis ( [ 0.0, 1.0, 0.0, 1.0 ] )
  line ( [ 0.0, 1.0, 1.0, 0.0, 0.0 ], [ 0.0, 0.0, 1.0, 1.0, 0.0 ], ...
    'Color', 'r', 'LineWidth', 2 );
  title_string = sprintf ( 'Initial Generators' );
  title ( title_string );
  axis equal
  drawnow

  string = input ( 'RETURN, or Q to quit: ', 's' );

  if ( string == 'q' | string == 'Q' )
    return
  end

  it = 0;

  while ( 1 )
%
%  Compute the Delaunay triangle information T for the current nodes.
%
    t = delaunay ( p(:,1), p(:,2) );
%
%  Display the Delaunay triangulation, if requested.
%
    if ( delaunay_display )
      subplot ( 1, 2, 2 )
      trimesh ( t, p(:,1), p(:,2), zeros(n,1), 'EdgeColor', 'b' )
      hold on
      plot ( p(:,1), p(:,2), 'b.' );
      axis ( [ -0.0, 1.0, -0.0, 1.0 ] )
      line ( [ 0.0, 1.0, 1.0, 0.0, 0.0 ], [ 0.0, 0.0, 1.0, 1.0, 0.0 ], ...
        'Color', 'r', 'LineWidth', 2 );
      title_string = sprintf ( 'Delaunay, step %d', it );
      title ( title_string );
      axis equal
      view ( 2 )
      hold off
    end
%
%  Display the CVT generators, and the associated Voronoi diagram.
%
    if ( delaunay_display )
      subplot ( 1, 2, 1 )
    end

    voronoi ( p(:,1), p(:,2), t );
    axis ( [ -0.0, 1.0, -0.0, 1.0 ] )
    line ( [ 0.0, 1.0, 1.0, 0.0, 0.0 ], [ 0.0, 0.0, 1.0, 1.0, 0.0 ], ...
      'Color', 'r', 'LineWidth', 2 );
    title_string = sprintf ( 'Voronoi, step %d', it );
    title ( title_string );
    axis equal
    axis tight
    drawnow
%
%  Generate sample points.
%
%  These sample points implicitly define the geometry of the region.
%  If the region is not a unit square, then the range of the sample
%  data must be changed.
%
%  The data is sampled uniformly.  If a nonuniform density is desired,
%  then the sampling must be done in a biased way.
%
    xs = rand ( sample_num, 1 );
    ys = rand ( sample_num, 1 );
%
%  For each sample point, find K, the index of the nearest generator.
%  We do this efficiently by using the Delaunay information with
%  Matlab's DSEARCH command, rather than a brute force nearest neighbor
%  computation.
%
    k(1:sample_num,1) = dsearch ( p(:,1), p(:,2), t, xs, ys );
%
%  The centroid of the Voronoi region associated with each generator
%  is approximated by the average of the sample points it was closest to.
%
    if ( accumarray_available )

      count(1:n) = accumarray ( k, ones(sample_num,1) );
      centroid(1,1:n) = accumarray ( k, xs );
      centroid(2,1:n) = accumarray ( k, ys );

    else

      count(1:n) = 0;
      centroid(1,1:n) = 0.0;
      centroid(2,1:n) = 0.0;

      for i = 1 : sample_num
        j = k(i);
        count(j) = count(j) + 1;
        centroid(1,j) = centroid(1,j) + xs(i);
        centroid(2,j) = centroid(2,j) + ys(i);
      end

    end
%
%  Replace the generators by the centroids.
%
    p(1:n,1) = ( centroid(1,1:n) ./ count(1:n) )';
    p(1:n,2) = ( centroid(2,1:n) ./ count(1:n) )';

    string = input ( 'RETURN, or Q to quit: ', 's' );

    if ( string == 'q' | string == 'Q' )
      break
    end

    it = it + 1;

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_SQUARE_UNIFORM:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end
function p = square_uniform ( n )

%*****************************************************************************80
%
%% SQUARE_UNIFORM returns sample points from the unit square.
%
%  Discussion:
%
%    This routine returns N points sampled uniformly at random
%    from within the unit square.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 May 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points to generate.
%
%    Output, real P(N,2), the sample points.
%
  p(1:n,1:2) = rand(n,2);

  return
end


%% Mean-Shift Video Tracking
% by Sylvain Bernhardt
% July 2008
%% Description
% Estimate the density of data samples
% (here colour histogram) in a patch T
% with a kernel profile k. Lmap is the
% colormap length and H,W the patch size.

function q = Density_estim(T,Lmap,k,H,W,graph)
q = zeros(Lmap,1);
colour = linspace(1,Lmap,Lmap);
for x=1:W
    for y=1:H
        q(T(y,x)+1) = q(T(y,x)+1)+k(y,x);
    end
end

% Normalizing
C = 1/sum(sum(k));
q = C.*q;

% Plotting the estimated densities
if graph==1
    figure
    plot(colour,q);
end


function [ fea, out ] = ex_planestress2( varargin )
%EX_PLANESTRESS2 NAFEMS benchmark challenge 1 plane stress example.
%
%   [ FEA, OUT ] = EX_PLANESTRESS2( VARARGIN ) NAFEMS benchmark example to calculate von Mieses stress
%   thin plate under plane stress assumption with loads all around.
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       E           scalar {210e9}         Modulus of elasticity
%       nu          scalar {0.3}           Poissons ratio
%       igrid       scalar 1/{0}           Cell type (0=quadrilaterals, 1=triangles)
%       hmax        scalar {1/20}          Max grid cell size
%       sfun        string {sflag1}        Shape function for displacements
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
  'E',        210e9; ...
  'nu',       0.3; ...
  'igrid',    0; ...
  'hmax',     1/20; ...
  'sfun',     'sflag1'; ...
  'iplot',    1; ...
  'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

% Geometry definition.
gobj1 = gobj_rectangle( 0, 1, 0, 1, 'R1' );
fea.geom.objects = { gobj1 };
fea.sdim = { 'x' 'y' };

% Grid generation.
if( opt.igrid==1 )
  fea.grid = gridgen(fea,'hmax',opt.hmax,'fid',fid);
else
  nx = 1/opt.hmax;
  fea.grid = rectgrid( nx, nx );
end

% Boundary conditions.
dtol = 0.1;
if( opt.igrid==1 )
  lbdr = findbdr( fea, ['x<',num2str(dtol)] );     % Left boundary number.
  rbdr = findbdr( fea, ['x>',num2str(1-dtol)] );   % Right boundary number.
  tbdr = findbdr( fea, ['y>',num2str(1-dtol)] );   % Top boundary number.
  bbdr = findbdr( fea, ['y<',num2str(dtol)] );     % Bottom boundary number.
else
  lbdr = 4;
  rbdr = 2;
  tbdr = 3;
  bbdr = 1;
end

% Add plane stress physics mode.
fea = addphys(fea,@planestress);
fea.phys.pss.eqn.coef{1,end} = { opt.nu };
fea.phys.pss.eqn.coef{2,end} = { opt.E  };
fea.phys.pss.sfun            = { opt.sfun opt.sfun };

% Set boundary condition types.
bctype = mat2cell( zeros(2,4), [1 1], [1 1 1 1] );
fea.phys.pss.bdr.coef{1,5}   = bctype;

% Set loads on boundary.
bccoef = mat2cell( zeros(2,4), [1 1], [1 1 1 1] );
bccoef{1,tbdr} = '-x';
bccoef{2,tbdr} = 'x';
bccoef{1,bbdr} = '-(1-x)';
bccoef{2,bbdr} = '1-x';
bccoef{1,lbdr} = '1-y';
bccoef{2,lbdr} = '-(1-y)';
bccoef{1,rbdr} = 'y';
bccoef{2,rbdr} = '-y';
fea.phys.pss.bdr.coef{1,end} = bccoef;

% Parse and solve problem.
fea       = parsephys(fea);             % Check and parse physics modes.
fea       = parseprob(fea);             % Check and parse problem struct.
fea.sol.u = solvestat(fea,'fid',fid);   % Call to stationary solver.

% Postprocessing.
s_vm = fea.phys.pss.eqn.vars{1,2};
if ( opt.iplot>0 )
  figure
  postplot( fea, 'surfexpr', s_vm, 'isoexpr', s_vm )
  title('von Mieses stress')
end

% Error checking.
dtol = sqrt(eps);
sm  = evalexpr( s_vm, [0.5;0.5], fea );
s01 = evalexpr( s_vm, [dtol;1],    fea );
s10 = evalexpr( s_vm, [1;dtol],    fea );
s00 = evalexpr( s_vm, [dtol;dtol], fea );
s11 = evalexpr( s_vm, [1;1],       fea );

sol = [ sm s01 s10 s00 s11 ];
ref = [  0   0   0   2   2 ];
out.err    = norm(sol-ref)/norm(ref);
out.pass   = out.err < 0.1;

if ( nargout==0 )
  clear fea out
end


function [error_x,error_y] = stokespost_q1q1_bc(aez,fezx,fezy,elerrorx,elerrory,xy,ev,ebound)
%stokespost_q1q1_bc   postprocesses Poisson error estimator
%   [error_x,error_y] = stokespost_q1q1_bc(aez,fezx,fezy,elerrorx,elerrory,xy,ev,ebound);
%   input
%          aez                  elementwise Poisson problem matrices
%          fezx,fezy            elementwise rhs vectors
%          elerrorx, elerrory   elementwise error estimate (without BC imposition)
%          xy                   vertex coordinate vector
%          ev                   element mapping matrix
%          ebound               element edge boundary matrix
%   output
%          error_x, error_y     component elementwise error estimate
%
%   calls function localbc_xy
%   IFISS function: DJS; 8 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      x=xy(:,1); y=xy(:,2);
      nel=length(ev(:,1));
      lev=[ev,ev(:,1)];
	  error_x=elerrorx;  error_y=elerrory;
%
% recompute contributions from elements with Dirichlet boundaries
      nbde=length(ebound(:,1));
      ebdy = zeros(nel,1);
      edge = zeros(nel,1);
% isolate boundary elements
      for el = 1:nbde
      ee = ebound(el,1);
      ebdy(ee) = ebdy(ee)+1; edge(ee)=ebound(el,2);
      end
%
% two edge elements
      k2=find(ebdy==2);
      nel2b=length(k2);
% loop over two edge elements
      for el = 1:nel2b
      el2e=k2(el);
      kk=find(ebound(:,1) == el2e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
	  ae=squeeze(aez(el2e,1:5,1:5));
      fex=fezx(el2e,:)'; fey=fezy(el2e,:)';
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el2e,:)); yl=y(lev(el2e,:));
	  [bae,fex,fey] = localbc_xy(ae,fex,fey,edges,xl,yl);
% solve local problems
      errx=bae\fex;  erry=bae\fey;
	  error_x(el2e,1) = errx'*ae*errx; error_y(el2e,1) = erry'*ae*erry;
	  end
% end of element loop
%
% one edge elements
      k1=find(ebdy==1);
      nel1b=length(k1);
% loop over one edge elements
      for el = 1:nel1b
      el1e=k1(el);
      kk=find(ebound(:,1) == el1e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
      fex=fezx(el1e,:)'; fey=fezy(el1e,:)';
	  ae=squeeze(aez(el1e,1:5,1:5));
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el1e,:)); yl=y(lev(el1e,:));
	  [bae,fex,fey] = localbc_xy(ae,fex,fey,edges,xl,yl);
% solve local problems
      errx=bae\fex;  erry=bae\fey;
	  error_x(el1e,1) = errx'*ae*errx; error_y(el1e,1) = erry'*ae*erry;

      end
% end of element loop
%
      err_x = sqrt(sum(error_x)); error_x = sqrt(error_x);
	  err_y = sqrt(sum(error_y)); error_y = sqrt(error_y);
      fprintf('estimated velocity error (in energy):  (%10.6e,%10.6e) \n',err_x,err_y)
 return


function wishart_test04 ( )

%*****************************************************************************80
%
%% WISHART_TEST04 demonstrates the Wishart sampling function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 July 2013
%
%  Author:
%
%    John Burkardt
%

%
%  Access the PDFLIB and RNGLIB libraries.
%
  addpath ( '../pdflib' );
  addpath ( '../rnglib' );
%
%  Initialize the RNGLIB library.
%
  initialize ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'WISHART_TEST04:\n' );
  fprintf ( 1, '  We can compute sample Wishart matrices by:\n' );
  fprintf ( 1, '    W = wishart_sample ( n, df, sigma );\n' );
%
%  Set the parameters and call.
%
  n = 5;
  df = 8;
  sigma = eye ( 5, 5 );
  w = wishart_sample ( n, df, sigma );
  r8mat_print ( n, n, w, '  wishart_sample ( 5, 8, Identity ):' );
%
%  Calling again yields a new matrix.
%
  w = wishart_sample ( n, df, sigma );
  r8mat_print ( n, n, w, '  wishart_sample ( 5, 8, Identity ):' );
%
%  Try a diagonal matrix.
%
  sigma = diag ( [1.0, 2.0, 3.0, 4.0, 5.0 ] );
  w = wishart_sample ( n, df, sigma );
  r8mat_print ( n, n, w, '  wishart_sample ( 5, 8, diag(1,2,3,4,5) ):' );
%
%  Try a smaller matrix.  Sigma must be positive definite symmetric.
%
  n = 3;
  df = 3;
  r = [ 5.0, 1.0, 3.0; ...
        0.0, 4.0, 2.0; ...
        0.0, 0.0, 6.0 ];
  sigma = r' * r;
  r8mat_print ( n, n, sigma, '  Set covariance SIGMA:' );
  w = wishart_sample ( n, df, sigma );
  r8mat_print ( n, n, w, '  wishart_sample ( 3, 3, sigma ):' );
%
%  What is the eigendecomposition of this matrix?
%
  [ v, lambda ] = eigs ( w );
  r8mat_print ( n, n, v, '  Eigenvectors of previous matrix:' );
  r8mat_print ( n, n, lambda, '  Eigenvalues of previous matrix:' );

  rmpath ( '../pdflib' );
  rmpath ( '../rnglib' );

  return
end


function z=rpsola(x,y,t)
  % The solution of a Riemann problem (continuous)
  z=zeros(size(x));
  lne=0.25*t;
  lm=-0.5*t;
  lM=0.5*t;

  ind=find((x>=lM)&(y>=lM));
  z(ind)=0.5;

  ind=find((x<lM)&(x>=lne)&(y>=lne)&(y>=x));
  z(ind)=x(ind)/t;

  ind=find((x>=lne)&(y<lM)&(y>=lne)&(y<x));
  z(ind)=y(ind)/t;

  ind=find(((x>=lne)&(y<lne))|((x<lne)&(y>=lne)));
  z(ind)=0.25;

  ind=find((x<lne)&(x>=lm)&(x>=y));
  z(ind)=x(ind)/t;

  ind=find((y<lne)&(y>=lm)&(x<y));
  z(ind)=y(ind)/t;

  ind=find((x<=lm)&(y<=lm));
  z(ind)=-0.5;


function [PB] = GB2PB(GB)
% Convert computery things from gigabytes to petabytes.
% Chad A. Greene 2012
PB = GB*2^-20 ;


function [sma, ecc, inc, raan, arg, tru] = vect_getKeplerFromState(rVect,vVect,gmu, varargin)
% getKeplerFromState() returns Keplerian orbital elements when provided
% with the state (cartesian position vector, cartesian velocity vector) of
% a spacecraft or celestial body.
%
%INPUTS
% rVect - a 3x1 vector that contains the x,y,z components of the orbiting
% body's current position relative to the central body.  Units: [km]
% vVect - a 3x1 vector that contains the x,y,z components of the orbiting
% body's current velocity vector relative to the central body.  Units: [km/sec]
% muCB - the gravitational parameter of the central body.  Units: km^3/s^2
%
%OUTPUTS
% sma - semi-major axis of the orbit.  Units: [km]
% ecc - eccentricity of the orbit.  Units: dimensionless
% inc - inclination angle of the orbit.  Units: radian
% raan - Longitude of ascending node of the orbit.  Units: radian
% arg - Argument of periapse of the orbit.  Units: radian.
% tru - Current true anomaly of the spacecraft/body in the orbit.
% Units: radian
if(~isempty(varargin))
    consistencyCheck = varargin{1};
else
    consistencyCheck = true;
end

numRV = size(rVect,2);

rVect = reshape(rVect,3,numRV);
vVect = reshape(vVect,3,numRV);

if(isscalar(gmu))
    gmu = gmu * ones(1,numRV);
end

try
    [sma, ecc, inc, raan, arg, tru] = vect_getKeplerFromState_Alg_mex(rVect,vVect,gmu(:)');
catch
    [sma, ecc, inc, raan, arg, tru] = vect_getKeplerFromState_Alg(rVect,vVect,gmu(:)');
end

bool = ecc<1.0;

tru(bool)=AngleZero2Pi(tru(bool));
tru(~bool) = angleNegPiToPi(tru(~bool));

tol = 1E-6;
if(consistencyCheck)
    [rVect2,~]=vect_getStatefromKepler(sma, ecc, inc, raan, arg, tru, gmu, false);
    rDang = real(acos(dot(rVect,rVect2)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect2).^2,1)) )));
    if(all(rDang < tol))
        return;
    end

    [rVect3,~]=vect_getStatefromKepler(sma, ecc, inc, raan-rDang, arg+rDang, tru, gmu, false);
    rDang2 = real(acos(dot(rVect,rVect3)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect3).^2,1)) )));
    if(any(rDang2 < tol))
        raan(rDang2 < tol) = raan(rDang2 < tol) - rDang(rDang2 < tol);
        arg(rDang2 < tol)  = arg(rDang2 < tol)  + rDang(rDang2 < tol);
        raan = AngleZero2Pi(raan);
        arg = AngleZero2Pi(arg);

        if(all(rDang2 < tol))
            return;
        end
    end

    [rVect4,~]=vect_getStatefromKepler(sma, ecc, inc, raan+rDang, arg-rDang, tru, gmu, false);
    rDang3 = real(acos(dot(rVect,rVect4)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect4).^2,1)) )));
    if(any(rDang3 < tol))
        raan(rDang3 < tol) = raan(rDang3 < tol) + rDang(rDang3 < tol);
        arg(rDang3 < tol)  = arg(rDang3 < tol)  - rDang(rDang3 < tol);
        raan = AngleZero2Pi(raan);
        arg = AngleZero2Pi(arg);

        if(all(rDang3 < tol))
            return;
        end
    end

    [rVect5,~]=vect_getStatefromKepler(sma, ecc, inc, raan+rDang, arg+rDang, tru, gmu, false);
    rDang4 = real(acos(dot(rVect,rVect5)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect5).^2,1)) )));
    if(any(rDang4 < tol))
        raan(rDang4 < tol) = raan(rDang4 < tol) + rDang(rDang4 < tol);
        arg(rDang4 < tol)  = arg(rDang4 < tol) + rDang(rDang4 < tol);
        raan = AngleZero2Pi(raan);
        arg = AngleZero2Pi(arg);

        if(all(rDang4 < tol))
            return;
        end
    end

    [rVect6,~]=vect_getStatefromKepler(sma, ecc, inc, raan-rDang, arg-rDang, tru, gmu, false);
    rDang5 = real(acos(dot(rVect,rVect6)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect6).^2,1)) )));
    if(any(rDang5 < tol))
        raan(rDang5 < tol) = raan(rDang5 < tol) - rDang(rDang5 < tol);
        arg(rDang5 < tol)  = arg(rDang5 < tol) - rDang(rDang5 < tol);
        raan = AngleZero2Pi(raan);
        arg = AngleZero2Pi(arg);

        if(all(rDang5 < tol))
            return;
        end
    end

    [rVect7,~]=vect_getStatefromKepler(sma, ecc, inc, raan, arg, tru-rDang, gmu, false);
    rDang6 = real(acos(dot(rVect,rVect7)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect7).^2,1)) )));
    if(any(rDang6 < tol))
        tru(rDang6 < tol) = tru(rDang6 < tol) - rDang(rDang6 < tol);

        if(all(rDang6 < tol))
            return;
        end
    end

    [rVect8,~]=vect_getStatefromKepler(sma, ecc, inc, raan, arg, tru+rDang, gmu, false);
    rDang7 = real(acos(dot(rVect,rVect8)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect8).^2,1)) )));
    if(any(rDang7 < tol))
        tru(rDang7 < tol) = tru(rDang7 < tol) + rDang(rDang7 < tol);

        if(all(rDang7 < tol))
            return;
        end
    end
end


function [p,CI] = compareAUC(X1,X2,Y)
% -------------------------------------------------------------------------
% function [p,CI] = compareAUC(X1,X2,Y)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function calculates the significance (p-value) for the difference
% between two CORRELATED AUCs (i.e. computed from the same samples)
% computed for two different prediction situations. The implementation of
% this function is based on ref. [1].
% -------------------------------------------------------------------------
% REFERENCE:
% [1] DeLong, E. R., DeLong, D. M. & Clarke-Pearson, D. L. Comparing the
%     areas under two or more correlated receiver operating characteristic
%     curves: a nonparametric approach. Biometrics 44, 837–845 (1988).
% -------------------------------------------------------------------------
% INPUTS:
% 1. X1: [N X 1] vector of probabilities of outcome Y for situation 1.
% 2. X2: [N X 1] vector of probabilities of outcome Y for situation 2.
% 3. Y:  [N X 1] vector defining if each nth intance has the outcome or not
%        (1's or 0's), where N is the total number of patients.
% -------------------------------------------------------------------------
% OUTPUTS:
% 1. p: p-value for the difference in AUC.
% 2. CI: 95 % confidence interval for the difference in AUCs (diff), to be
%    subsequently applied as diff - CI and diff + CI.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: January 2017
% -------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015-2017  Martin Vallieres
%
%    This package 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 package 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 package.  If not, see <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------

% SANITY CHECKS
if numel(X1) ~= numel(Y) || numel(X2) ~= numel(Y)
    error('The number of instances in vectors X1, X2, and Y must be the same')
end
Y = logical(Y);

% PARAMETERS OR INTEREST
sigma = 1.96; % For 95 % confidence interval
L = [1,-1]; % Contrast vector. We are always comparing 2 variables at a time, not more. This is hardcoded for now and should stay as is.

% INITIALIZATION
X = [X1,X2]; % Combining the variables into a single matrix. We are always comparing 2 variables at a time, not more.
nVar = size(X,2); % Number of variables we compare (referred to "r" in ref. [1], so here, "v" --> "r" in ref. [1])
nInst = numel(Y); nPos = sum(Y); nNeg = nInst - nPos; % Total number of instances, number of positive instances ("nPos" --> "m" in ref. [1]) and number of negative instances ("nNeg" --> "n" in ref. [1]), respectively
Vpos = zeros(nPos,nVar); Vneg = zeros(nNeg,nVar); % Matrices "Vpos" --> "V10" and "Vneg" --> "V01" in ref. [1], respectively.
auc = zeros(1,nVar); % AUC estimates ("auc" --> "theta" in ref. [1])

% COMPUTATION OF AUC ESTIMATES ("auc" --> "theta"), VPOS ("Vpos" --> "V10") and VNEG ("Vneg" --> "V01")
Xpos = X(Y,:); Xneg = X(~Y,:); % Separating positive and negative instances ("Xpos" --> "X" and Xneg --> "Y" in ref. [1]. Here, our "Y" refers to the outcomes, or targets, to defined if an instance as the condition or not)
for v = 1:nVar
    for i = 1:nPos
        val = Xpos(i,v);
        Vpos(i,v) = sum(Xneg(:,v) < val) + 0.5*sum(Xneg(:,v) == val);
    end
    for i = 1:nNeg
        val = Xneg(i,v);
        Vneg(i,v) = sum(Xpos(:,v) > val) + 0.5*sum(Xpos(:,v) == val);
    end
end
auc = sum(Vpos)/(nNeg*nPos);
Vpos = Vpos/nNeg; Vneg = Vneg/nPos;

% COMPUTATION OF COVARIANCE MATRICES ("Spos" --> "S10" and "Sneg" --> "S01" in ref. [1])
Spos = (1/nPos) * ((Vpos'*Vpos) - nPos*(auc*auc'));
Sneg = (1/nNeg) * ((Vneg'*Vneg) - nNeg*(auc*auc'));
S = (1/nPos)*Spos + (1/nNeg)*Sneg;

% COMPUTATION OF CHI-SQUARE STATISTICS
LSL = L*S*L';
val_chi2 = (auc*L')*(inv(LSL))*(L*auc')*sqrt(2);
df_chi2 = rank(LSL);
p = 1-chi2cdf(val_chi2,df_chi2);
CI = sigma * sqrt(LSL);

end


classdef SOP_F21 < PROBLEM
% <single> <real> <expensive/none>
% Shekel's family

%------------------------------- Reference --------------------------------
% X. Yao, Y. Liu, and G. Lin, Evolutionary programming made faster, IEEE
% Transactions on Evolutionary Computation, 1999, 3(2): 82-102.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 1;
            obj.D = 4;
            obj.lower    = zeros(1,obj.D);
            obj.upper    = zeros(1,obj.D) + 10;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            a = [4 4 4 4;1 1 1 1;8 8 8 8;6 6 6 6;3 7 3 7];
            c = [0.1;0.2;0.2;0.4;0.4];
            PopObj = zeros(size(PopDec,1),1);
            for i = 1 : size(PopDec,1)
                PopObj(i) = -sum(1./(sum((repmat(PopDec(i,:),5,1)-a).^2,2)+c));
            end
        end
        %% Generate the minimum objective value
        function R = GetOptimum(obj,N)
            R = -10.16;
        end
    end
end


function out = EN_SampEn(y,M,r,preProcessHow)
% EN_SampEn     Sample Entropy of a time series
%
% SampEn(m,r), using code from PhysioNet.
%
% Uses a compiled C version of the code if available, otherwise uses a (slower)
% Matlab implementation (which can actually be faster for shorter time series
% due to overheads of reading/writing to disk)
%
% The publicly-available PhysioNet Matlab code, sampenc (renamed here to
% RN_sampenc) is available from:
% http://www.physionet.org/physiotools/sampen/matlab/1.1/sampenc.m
%
% cf. "Physiological time-series analysis using approximate entropy and sample
% entropy", J. S. Richman and J. R. Moorman, Am. J. Physiol. Heart Circ.
% Physiol., 278(6) H2039 (2000)
%
% This function can also calculate the SampEn of successive increments of time
% series, i.e., we using an incremental differencing pre-processing, as
% used in the so-called Control Entropy quantity:
%
% "Control Entropy: A complexity measure for nonstationary signals"
% E. M. Bollt and J. Skufca, Math. Biosci. Eng., 6(1) 1 (2009)
%
%---INPUTS:
% y, the input time series
% M, the embedding dimension
% r, the threshold
% preProcessHow [opt], (i) 'diff1', incremental differencing (as per 'Control Entropy').

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

% Check y is a column vector:
if isempty(y)
    error('Must input a valid data vector');
end
if size(y,1)==1
    y = y';
end

% Embedding dimension:
if nargin < 2
    M = 2;
end

% Tolerance:
if nargin < 3
    r = 0.1*std(y);
end

if nargin < 4
    preProcessHow = ''; % don't apply any preprocessing
end

%-------------------------------------------------------------------------------
% Can specify to first apply an incremental differencing of the time series
% thus yielding the 'Control Entropy':
% "Control Entropy: A complexity measure for nonstationary signals"
% E. M. Bollt and J. Skufca, Math. Biosci. Eng., 6(1) 1 (2009)
if ~isempty(preProcessHow)
    y = BF_PreProcess(y,preProcessHow);
end

% ------------------------------------------------------------------------------
% Use the physionet code to calculate the Sample Entropy using these parameters:
% ------------------------------------------------------------------------------
% Check if a compiled C version exists:
% [a,b] = system('which sampen');

sampEn = sampen_mex(y',M+1,r);
sampEn = sampEn(1:M+1); % always that extra one for the M=0

%-------------------------------------------------------------------------------
% This is dangerous because it could lead to inconsistent implementations across runs
%-------------------------------------------------------------------------------
% warning('No mex file found: using a slower native Matlab implementation instead');
% No mex version available; use (much slower) Matlab implementation
% if isempty(b) || length(y) < 3000 % faster to run within Matlab
% sampEn = PN_sampenc(y,M+1,r,false);
%     fprintf('Using compiled C code~~~\n')
%     % http://www.physionet.org/physiotools/sampen/c/
%     % (use Makefile in Toolboxes/Physionet/ to run make, then make install)
%
%     % Run compiled C code:
%     filePath = BF_WriteTempFile(y);
%     command = sprintf('sampen -m %u -r %f < %s',M,r,filePath);
%     [~,res] = system(command);
%     fprintf(1,'%s\n',res)
%     s = textscan(res,'%[^\n]'); s = s{1};
%     sampEn = zeros(M,1);
%     for i = 1:M
%         [~,params] = regexp(s{i},'\((\S+)\)','tokens','match');
%         params = regexp(params{1}(2:end-1),',','split');
%         [~,result] = regexp(s{i},'= (\S+)','tokens','match');
%         result = str2num(result{1}(3:end));
%         sampEn(i) = result;
%     end
% end
% end

% ------------------------------------------------------------------------------
% Compute outputs from the code
% ------------------------------------------------------------------------------
for i = 1:M+1
    % Sample entropy:
    out.(sprintf('sampen%u',i-1)) = sampEn(i);

    % Quadratic sample entropy (QSE), Lake (2006):
    % (allows better comparison across r values)
    out.(sprintf('quadSampEn%u',i-1)) = sampEn(i) + log(2*r);

    % COSEn (Lake and Moorman, 2011), doesn't really make sense in general;
    % especially for z-scored series!:
    % out.(sprintf('COSEn%u',i)) = sampEn(i) + log(2*r) - log(mean(y));
end

if M > 1
    out.meanchsampen = mean(diff(sampEn));
end

end


function im = ibwt(decomp)
% function im = ibwt(decomp)
%
% BWT reconstruction
% Version 1.2
%
% Arguments:
%  decomp: A square BWT decomposition, as produced by bwt.m
%
% Result:
%  im: An image
%
% Citation:
%  Willmore B, Prenger RJ, Wu MC and Gallant JL (2008). The Berkeley
%  Wavelet Transform: A biologically-inspired orthogonal wavelet transform.
%  Neural Computation 20:6, 1537-1564
%
% The article is available at:
%  <http://dx.doi.org/10.1162/neco.2007.05-07-513>
%
% Copyright (c) 2008 Ben Willmore
%
% Permission is hereby granted, free of charge, to any person
% obtaining a copy of this software and associated documentation
% files (the "Software"), to deal in the Software without
% restriction, including without limitation the rights to use,
% copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the
% Software is furnished to do so, subject to the following
% conditions:
%
% The above copyright notice and this permission notice shall be
% included in all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
% EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
% OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
% NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
% HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
% WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
% FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
% OTHER DEALINGS IN THE SOFTWARE.

sz = size(decomp);

if (length(sz) ~= 2) || (sz(1) ~= sz(2))
  disp('Input must be square');
  im = nan;
  return;
end

sz = sz(1);

numlevels = log(sz)/log(3);

if ( (numlevels-floor(numlevels)) > abs(numlevels)*eps )
  disp('Input side length must be a power of 3');
  im = nan;
  return;
end

im = zeros(sz);

for level = 1:numlevels
  ssz = size(decomp,1);
  decomp_thislevel = decomp;

  if (ssz>3)
    decomp_thislevel(end-ssz/3+1:end,1:ssz/3) = 0;
  end

  im_thislevel = ibwt_onelevel(decomp_thislevel)/sz*ssz*((3^level)^2);
  idx = ceil((1:sz)*ssz/sz);
  im = im + im_thislevel(idx,idx); % upsample by sz/ssz times

  if (ssz>1)
    decomp = decomp(end-ssz/3+1:end,1:ssz/3);
  end
end


function log_b = binomial_coeff(n_max)
    % Compute log binomial coefficients
    log_b = {};
    for n = 0:n_max
        Ks = 0:n;
        log_b{n+1} = gammaln(n+1) - gammaln(Ks+1) - gammaln(n-Ks+1);
    end
end


% capillary pressure curve for drainage
% Written by Ali A. Eftekhari
% TBD, probably not required in the new IMPES formulation
function res=dpc_drain(sw, pce, swc, labda, pc_max)
sw0=swc+(1-labda*log(pc_max/pce)+sqrt((-1+labda*log(pc_max/pce))^2+...
      4*swc/(1-swc)))/2*(1-swc);
res = (sw>sw0).*(-1.0/((1-swc)*labda)*pce*((sw-swc)/(1-swc)).^(-1.0/labda-1))+...
    (0.0<=sw).*(sw<=sw0).*(-1.0/((1-swc)*labda)*pce*((sw0-swc)/(1-swc)).^(-1.0/labda-1))+...
    (sw<0)*0.0;
end


function [F, subF, Data] = ObjFun(Track, UAV)
%OBJFUN 目标函数、适应度函数（一个agent的）

% 多目标优化权重设置（必需为行向量）
weight = [ 0.05, 0.05, 0.1, 0.7, 0.7 ]; % 默认权重

% 表达式系数调整（将各项指标无量纲化）
p1 = 1;   % 燃料项（已除以最大航程）
p21 = 1; % 高度项
p22 = 1; % 低度项
p31 = 1.2; % 雷达威胁
p32 = 1.1; % 其余威胁
p4 = 1.2;   % 时间同步项
p5 = 1;   % 碰撞项

% 进行航迹检测
report = TrackDetect(Track, UAV);  % Track 为 struck结构

% 燃油（论文3）
ZZ = sum(UAV.limt.L);
MaxL_mt = ZZ(2);
f_o = p1 * report.L_mt / MaxL_mt;

% 高度（论文3）
dim = UAV.PointDim;
if dim < 3
    f_h = 0;
else
    f_h = 0;
    for i = 1 : UAV.num
        Hmax = UAV.limt.h(i, 2);
        Hmin = UAV.limt.h(i, 1);
        for k = 1 : UAV.PointNum(i)
            z = Track.P{i}(3, k);
            if z>Hmax
                fk = p21 * (z - Hmax);
            elseif z >= Hmin
                fk = 0;
            else
                fk = p22 * (Hmin - z);
            end
            f_h = f_h + fk;
        end
    end
end

% 威胁（论文2）
% 注意：只适应球或圆区域，不适合圆柱区域
O_r = UAV.Menace.radar(: ,1:end-1);             %雷达
O_o = UAV.Menace.other(: ,1:end-1);            %导弹，火炮，气象等
f_t = 0;  % 威胁代价
for i = 1 : UAV.num
    for k = 1 : UAV.PointNum(i)
        P = Track.P{i}(:, k)' ;  % 转置成 1*dim
        for m = 1 : size(O_r, 1)
            fk = p31 / (norm(P - O_r(m, :)))^4;
            f_t = f_t + fk;
        end
        for m = 1 : size(O_o, 1)
            fk = p32 / norm(P - O_o(m, :));
            f_t = f_t + fk;
        end
    end
end

% 同步（论文1）
f_m = 0;
for i = 1 : UAV.num
    Li = report.L(i);
    tmax = Li / UAV.limt.v(i,1);
    tmin = Li / UAV.limt.v(i,2);
    ti = report.time(i);
    tc = UAV.tc;
    if tc <= tmax && tc >= tmin
        fk = 0;
    else
        fk = p4 * abs(ti - tc);
    end
    f_m = f_m + fk;
end

% 碰撞（论文1）
f_c = p5 * report.col_times;

% 目标函数分量（必需是列向量，否则无法聚类）
subF = [ f_o; f_h; f_t; f_m; f_c ]; % 5*1

% 加权目标函数
F = weight * subF ;

% 输出信息
Data.ProbPoint = report.ProbPoint;      % 所有有问题的点
Data.AngleProb = report.AngleProb;    % 不满足角度约束的点
Data.TrajProb = report.TrajProb;           % 不满足最小航迹间隔的点
Data.Threat = report.Threat;                  % 受威胁的点

Data.L = report.L;                                      % 每个无人机的航程
Data.t = report.time;                                 % 每个无人机的时间
Data.c = report.col_times;                         % 所有无人机总碰撞次数

end


function [w0, t] = adw_fan(ob, wi, mask, Phi, varargin)
% function [w0, phi, t] = adw_fan(ob, wi)
% Compute the angular-dependent weighting for 3rd gen fan-beam
% w0(\Phi) = w(x0, y0, \Phi)
%         for fully corrected penalty:
%          = w(s',\beta')*J(s')|\phi'=\Phi + ...
%            w(s',\beta')*J(s')|\phi'=\Phi-pi
% see fessler chapter 3.5.3
%
% in:
%    ob         a struct or object
%               including system parameters, such as pixel-size etc.
%    wi         cov(yi)
%    varargin   default: only compute phi' = Phi
%               'all': compute phi' = Phi + phi' = Phi- pi
% out:
%    w0     angular-dependent weighting
%    t      computation time
%
% Copyright 2005-4-07, Yingying Zhang, The University of Michigan

all = logical(0);
if ~isempty(varargin)
    arg = varargin{1};
    if isempty(arg)
			%	do nothing for empty arguments

	elseif ~ischar(arg)
        error 'unknown non-string argument?'
    elseif streq(arg, 'all')
        all = logical(1);
    else
        error 'unknown string argument?'
    end
end

% default argument
ob.Df = 0; % 3rd generation CT scanner

% image domain pixel locations
% only compute within mask

yflip = (-1); % since the top is positive in y direction %
% x = ([1:ob.nx] - (ob.nx+1)/2)* ob.dx;
% y = ([1:ob.ny] - (ob.ny+1)/2)* ob.dx * yflip;

x = ([0:ob.nx-1] - ob.nx/2)* ob.dx;
y = ([0:ob.ny-1] - ob.ny/2)* ob.dx * yflip;

[x y] = ndgrid(x, y);

rr = sqrt(x.^2 + y.^2);
theta = atan2(y,x); % use it for our purpose since atan will have pi flip

r0 = rr(mask(:));
phi0 = theta(mask(:));

clear rr theta

% dummy variables in freq. domain
phi = Phi;

% if ~isempty(varargin) & isnumeric(varargin{1})
%     phi = varargin{1};
% end

% sinogram domain locations
ob.beta = ([0:ob.na-1]'/ob.na * ob.orbit ...
    + ob.orbit_start) * pi / 180;
ob.s = ([-(ob.nb-1)/2:(ob.nb-1)/2]' ...
			- ob.offset_s) * ob.ds;
ob.gamma = ob.s / ob.dis_src_det;
[ss, beta] = meshgrid(ob.s,ob.beta); % meshgrid and ndgrid reverse

% loop over \phi_i
Dcf = ob.dis_src_det + ob.Df;
ratio_fcf = ob.Df/Dcf;
% w0 = zeros(ob.nx, ob.ny, ob.na);
w0 = zeros([size(r0,1) length(phi)]);

tic
for ia = 1:length(phi)
    % -------------------------
    % for \phi' = \phi
    % -------------------------

    % compute new variables in (3.5.2)
    r_p = r0 .* cos(phi(ia) - phi0); % use ob.beta, not outer_sum(ob.gamma, ob.beta)
%                                         since evaluate at \phi_p = \phi(view angle)
    s_p = Dcf.*(asin(r_p./ob.dis_src_iso) - asin(ratio_fcf.*r_p./ob.dis_src_iso));
    beta_p = phi(ia) - asin(r_p./ob.dis_src_iso);

    % compute Jacobian determinant (2.8.9)
    Jacob = ob.dis_src_det ./ sqrt(ob.dis_src_iso.^2 - r_p.^2);

%     % find corresponding wi: nearest neighbor method
%     s_cen = (ob.nb - 1)/2 + ob.offset_s; % for nb = even % <-----(yy) plus:(to me account for the right index) or (hugo) minus?
%     s_loc = round(s_p./ob.ds + s_cen);
%     s_loc = min(s_loc, ob.nb);
%     s_loc = max(s_loc, 1);
%     beta_loc = round((mod(beta_p,2*pi)-ob.orbit_start)./(2*pi/ob.na)) + 1;
%     beta_loc = min(beta_loc, ob.na);
%     beta_loc = max(beta_loc, 1);
%
%     % find the corresponding index in column-stack wi
%     loc = 1 + (s_loc(:)-1) + (beta_loc(:)-1)*ob.nb;

    % compute w0 at \phi' = \phi
%     w0(:,ia) =wi(loc(:)) .* Jacob;
    si = min(s_p, ob.s(end)); si = max(si, ob.s(1));
    betai = min(beta_p, ob.beta(end)); betai = max(betai, ob.beta(1));
    w0(:,ia) = interp2(ss, beta, wi.', si(:), betai(:)) .* Jacob;

    clear s_loc beta_loc loc
    % -------------------------
    % for \phi' = \phi - pi
    % -------------------------
    if all
        phi_pi = phi(ia) - pi;
        r_p = r0 .* cos(phi_pi - phi0);
        s_p = Dcf.*(asin(r_p./ob.dis_src_iso) - asin(ratio_fcf.*r_p./ob.dis_src_iso));
        beta_p = phi_pi - asin(r_p./ob.dis_src_iso);

% %         Jacob_p again: not needed since same
%         Jacob_p = abs(ob.dis_src_iso*cos(s_p./ob.dis_src_det) - ...
%             ob.source_offset*sin(s_p./ob.dis_src_det))./ob.dis_src_det;
%
%         % find corresponding wi: nearest neighbor method
%         s_loc = round(s_p./ob.ds + s_cen);
%         s_loc = min(s_loc, ob.nb);
%         s_loc = max(s_loc, 1);
%
%         beta_loc = round((mod(beta_p,2*pi)-ob.orbit_start)./(2*pi/ob.na)) + 1;
%         beta_loc = min(beta_loc, ob.na);
%         beta_loc = max(beta_loc, 1);
%
%         % find the corresponding index in column-stack wi
%         loc = 1 + (s_loc(:)-1) + (beta_loc(:)-1)*ob.nb;
%
%         % compute w0 at \phi' = \phi - pi and sum with \phi' = \phi
%         w0(:,ia) = w0(:,ia) + wi(loc(:)).*Jacob;

        si = min(s_p, ob.s(end)); si = max(si, ob.s(1));
        betai = min(beta_p, ob.beta(end)); betai = max(betai, ob.beta(1));
        w0(:,ia) = w0(:,ia) + interp2(ss, beta, wi.', si(:), betai(:)) .* Jacob;
        clear s_loc beta_loc loc
    end

%     % angle_dependent blur
%     b0(:,:,ia) = (ob.ray_spacing./Dcf) .* sqrt(ob.dis_src_iso^2 + r0^2 + 2 .* ob.dis_src_iso .* Dcf .* cos(phi(ia)-phi0));
end

t = toc;
sprintf('angular weighting computation time %.4f', t)


function spm_slice_timing(P, sliceorder, refslice, timing, prefix)
% Correct differences in slice acquisition times
% FORMAT spm_slice_timing(P, sliceorder, refslice, timing, prefix)
% P           - char array of image filenames
%               can also be a cell array of the above (multiple subjects).
% sliceorder  - slice acquisition order, a vector of integers, each
%               integer referring the slice number in the image file
%               (1=first), and the order of integers representing their
%               temporal acquisition order
%               OR vector containig the acquisition time for each slice
%               in milliseconds
% refslice    - slice for time 0
%               OR time in milliseconds for the reference slice
% timing      - additional information for sequence timing
%               timing(1) = time between slices
%                         = TA / (nslices - 1)
%               timing(2) = time between last slices and next volume
%                         = TR - TA
%               OR timing = [0 TR] when previous inputs are specified in
%               milliseconds
% prefix      - filename prefix for corrected image files, defaults to 'a'
%__________________________________________________________________________
%
%   Note: The sliceorder arg that specifies slice acquisition order is
%   a vector of N numbers, where N is the number of slices per volume.
%   Each number refers to the position of a slice within the image file.
%   The order of numbers within the vector is the temporal order in which
%   those slices were acquired.
%
%   To check the order of slices within an image file, use the SPM Display
%   option and move the crosshairs to a voxel co-ordinate of z=1.  This
%   corresponds to a point in the first slice of the volume.
%
%   The function corrects differences in slice acquisition times.
%   This routine is intended to correct for the staggered order of
%   slice acquisition that is used during echoplanar scanning. The
%   correction is necessary to make the data on each slice correspond
%   to the same point in time. Without correction, the data on one
%   slice will represent a point in time as far removed as 1/2 the TR
%   from an adjacent slice (in the case of an interleaved sequence).
%
%   This routine "shifts" a signal in time to provide an output
%   vector that represents the same (continuous) signal sampled
%   starting either later or earlier. This is accomplished by a simple
%   shift of the phase of the sines that make up the signal.
%
%   Recall that a Fourier transform allows for a representation of any
%   signal as the linear combination of sinusoids of different
%   frequencies and phases. Effectively, we will add a constant
%   to the phase of every frequency, shifting the data in time.
%
%   Shifter - This is the filter by which the signal will be convolved
%   to introduce the phase shift. It is constructed explicitly in
%   the Fourier domain. In the time domain, it may be described as
%   an impulse (delta function) that has been shifted in time the
%   amount described by TimeShift.
%
%   The correction works by lagging (shifting forward) the time-series
%   data on each slice using sinc-interpolation. This results in each
%   time series having the values that would have been obtained had
%   the slice been acquired at the same time as the reference slice.
%
%   To make this clear, consider a neural event (and ensuing hemodynamic
%   response) that occurs simultaneously on two adjacent slices. Values
%   from slice "A" are acquired starting at time zero, simultaneous to
%   the neural event, while values from slice "B" are acquired one
%   second later. Without corection, the "B" values will describe a
%   hemodynamic response that will appear to have began one second
%   EARLIER on the "B" slice than on slice "A". To correct for this,
%   the "B" values need to be shifted towards the Right, i.e., towards
%   the last value.
%
% Written by Darren Gitelman at Northwestern U., 1998
%
% Based (in large part) on ACQCORRECT.PRO from G. Aguirre and E. Zarahn
% at U. Penn.
%
% Modified by R. Henson, C. Buechel and J. Ashburner, FIL, to
% handle different reference slices and memory mapping.
%
% Modified by M. Erb, at U. Tuebingen, 1999, to ask for non-continuous
% slice timing and number of sessions.
%
% Modified by R. Henson for more general slice order and SPM2.
%
% Modified by A. Hoffmann, M. Woletz and C. Windischberger from Medical
% University of Vienna, Austria, to handle multi-band EPI sequences.
%__________________________________________________________________________
% Copyright (C) 1998-2014 Wellcome Trust Centre for Neuroimaging

% Darren Gitelman et al.
% $Id: spm_slice_timing.m 6130 2014-08-01 17:41:18Z guillaume $

SVNid = '$Rev: 6130 $';

%-Say hello
%--------------------------------------------------------------------------
SPMid = spm('FnBanner',mfilename,SVNid);

%-Parameters & Arguments
%==========================================================================
if nargin < 4, error('Not enough input arguments.'); end
if nargin < 5, prefix = 'a'; end

if ~iscell(P), P = {P}; end
nsubjects = numel(P);

% Acquisition order: 1=first slice in image
% Reference slice: 1=first slice in image, in Analyze format, slice 1 = bottom
% TR: Interscan interval (TR) {secs}
% TA: Acquisition Time (TA) {secs} [Def: TR-TR/nslices], TA <= TR
% timing(2) = TR - TA, time between last slices and next volume
% timing(1) = TA / (nslices -1), time between slices

Vin     = spm_vol(P{1}(1,:));
nslices = Vin(1).dim(3);

TR      = (nslices-1)*timing(1)+timing(2);
fprintf('%-40s: %30s\n','Number of slices is...',num2str(nslices))      %-#
fprintf('%-40s: %30s\n','Time to Repeat (TR) is...',num2str(TR))        %-#

if ~isequal(1:nslices,sort(sliceorder))
    if ~all(sliceorder >= 0 & sliceorder <= TR*1000)
        error('Input is neither slice indices nor slice times.');
    end
    unit = 'slice times (ms)';
else
    if ~ismember(refslice,sliceorder)
        error('Reference slice should contain a slice index.');
    end
    unit = 'slice indices';
end
fprintf('%-40s: %30s\n','Parameters are specified as...',unit)          %-#

if nslices ~= numel(sliceorder)
    error('Mismatch between number of slices and length of ''sliceorder'' vector.');
end

%-Slice timing correction
%==========================================================================
for subj = 1:nsubjects
    Vin   = spm_vol(P{subj});
    nimgo = numel(Vin);
    nimg  = 2^(floor(log2(nimgo))+1);
    if Vin(1).dim(3) ~= nslices
        error('Number of slices differ: %d vs %d.', nslices, Vin(1).dim(3));
    end

    % Create new header files
    Vout  = Vin;
    for k=1:nimgo
        Vout(k).fname  = spm_file(Vin(k).fname, 'prefix', prefix);
        if isfield(Vout(k),'descrip')
            desc = [Vout(k).descrip ' '];
        else
            desc = '';
        end
        Vout(k).descrip = [desc 'acq-fix ref-slice ' num2str(refslice)];
    end
    Vout = spm_create_vol(Vout);

    % Set up [time x voxels] matrix for holding image info
    slices = zeros([Vout(1).dim(1:2) nimgo]);
    stack  = zeros([nimg Vout(1).dim(1)]);

    task = sprintf('Correcting acquisition delay: session %d', subj);
    spm_progress_bar('Init',nslices,task,'planes complete');

    % Compute shifting amount from reference slice and slice order
    if isequal(unit,'slice times (ms)')
        % Compute time difference between the acquisition time of the
        % reference slice and the current slice by using slice times
        % supplied in sliceorder vector
        shiftamount = (sliceorder - refslice) / (1000 * TR);
    else
        rslice      = find(sliceorder==refslice);
        [Y, I]      = sort(sliceorder);
        shiftamount = (I - rslice) * timing(1) / TR;
    end

    % For loop to perform correction slice by slice
    for k = 1:nslices

        % Read in slice data
        B  = spm_matrix([0 0 k]);
        for m=1:nimgo
            slices(:,:,m) = spm_slice_vol(Vin(m),B,Vin(1).dim(1:2),1);
        end

        % Set up shifting variables
        len     = size(stack,1);
        phi     = zeros(1,len);

        % Check if signal is odd or even -- impacts how Phi is reflected
        %  across the Nyquist frequency. Opposite to use in pvwave.
        OffSet  = 0;
        if rem(len,2) ~= 0, OffSet = 1; end

        % Phi represents a range of phases up to the Nyquist frequency
        % Shifted phi 1 to right.
        for f = 1:len/2
            phi(f+1) = -1*shiftamount(k)*2*pi/(len/f);
        end

        % Mirror phi about the center
        % 1 is added on both sides to reflect Matlab's 1 based indices
        % Offset is opposite to program in pvwave again because indices are 1 based
        phi(len/2+1+1-OffSet:len) = -fliplr(phi(1+1:len/2+OffSet));

        % Transform phi to the frequency domain and take the complex transpose
        shifter = [cos(phi) + sin(phi)*sqrt(-1)].';
        shifter = shifter(:,ones(size(stack,2),1)); % Tony's trick

        % Loop over columns
        for i=1:Vout(1).dim(2)

            % Extract columns from slices
            stack(1:nimgo,:) = reshape(slices(:,i,:),[Vout(1).dim(1) nimgo])';

            % Fill in continous function to avoid edge effects
            for g=1:size(stack,2)
                stack(nimgo+1:end,g) = linspace(stack(nimgo,g),...
                    stack(1,g),nimg-nimgo)';
            end

            % Shift the columns
            stack = real(ifft(fft(stack,[],1).*shifter,[],1));

            % Re-insert shifted columns
            slices(:,i,:) = reshape(stack(1:nimgo,:)',[Vout(1).dim(1) 1 nimgo]);
        end

        % Write out the slice for all volumes
        for p = 1:nimgo
            Vout(p) = spm_write_plane(Vout(p),slices(:,:,p),k);
        end
        spm_progress_bar('Set',k);
    end
    spm_progress_bar('Clear');
end

fprintf('%-40s: %30s\n','Completed',spm('time'))                        %-#


function x = r83p_sl ( n, a_lu, b, job, work2, work3, work4 )

%*****************************************************************************80
%
%% R83P_SL solves a R83P system.
%
%  Discussion:
%
%    The R83P storage format stores a periodic tridiagonal matrix as
%    a 3 by N array, in which each row corresponds to a diagonal, and
%    column locations are preserved.  The matrix value
%    A(1,N) is stored as the array entry A(3,N), and the matrix value
%    A(N,1) is stored as the array entry A(1,1).
%
%    The linear system must have been factored by R83P_FA.
%
%  Example:
%
%    Here is how a R83P matrix of order 5 would be stored:
%
%      A51 A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54 A15
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be at least 3.
%
%    Input, real A_LU(3,N), the LU factors from R83P_FA.
%
%    Input, real B(N), the right hand side of the linear system.
%
%    Input, integer JOB, specifies the system to solve.
%    0, solve A * x = b.
%    nonzero, solve A' * x = b.
%
%    Input, real WORK2(N-1), WORK3(N-1), WORK4, factor data from R83P_FA.
%
%    Output, real X(N), the solution to the linear system.
%
  x(1:n) = b(1:n);

  if ( job == 0 )
%
%  Solve A1 * X1 = B1.
%
    x(1:n-1) = r83_np_sl ( n-1, a_lu, x, job );
%
%  X2 = B2 - A3 * X1
%
    x(n) = x(n) - a_lu(1,1) * x(1) - a_lu(3,n-1) * x(n-1);
%
%  Solve A4 * X2 = X2
%
    x(n) = x(n) / work4;
%
%  X1 := X1 - inverse ( A1 ) * A2 * X2.
%
    x(1:n-1) = x(1:n-1) - work2(1:n-1) * x(n);

  else
%
%  Solve A1' * X1 = B1.
%
    x(1:n-1) = r83_np_sl ( n-1, a_lu, x, job );
%
%  X2 := X2 - A2' * B1
%
    x(n) = x(n) - a_lu(3,n) * x(1) - a_lu(1,n) * x(n-1);
%
%  Solve A4 * X2 = X2.
%
    x(n) = x(n) / work4;
%
%  X1 := X1 - transpose ( inverse ( A1 ) * A3 ) * X2.
%
    x(1:n-1) = x(1:n-1) - work3(1:n-1) * x(n);

  end

  return
end


function [ value, x ] = randlc_jump ( x, k )

%*****************************************************************************80
%
%% RANDLC_JUMP returns the K-th element of a uniform pseudorandom sequence.
%
%  Discussion:
%
%    The sequence uses the linear congruential generator:
%
%      X(K+1) = A * X(K)  mod 2^46
%
%    The K-th element, which can be represented as
%
%      X(K) = A^K * X(0)  mod 2^46
%
%    is computed directly using the binary algorithm for exponentiation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    David Bailey, Eric Barszcz, John Barton, D Browning, Robert Carter,
%    Leonardo Dagum, Rod Fatoohi,
%    Samuel Fineberg, Paul Frederickson, Thomas Lasinski, Robert Schreiber,
%    Horst Simon, V Venkatakrishnan, Sisira Weeratunga,
%    The NAS Parallel Benchmarks,
%    RNR Technical Report RNR-94-007,
%    March 1994.
%
%    Donald Knuth,
%    The Art of Computer Programming,
%    Volume 2, Seminumerical Algorithms,
%    Third Edition,
%    Addison Wesley, 1997,
%    ISBN: 0201896842,
%    LC: QA76.6.K64.
%
%  Parameters:
%
%    Input, uint64 X, the initial seed (with index 0).
%
%    Input, integer K, the index of the desired value.
%
%    Output, real VALUE, the K-th value in the sequence.
%
%    Output, uint64 X, the K-th seed.
%
  persistent a;
  persistent a1;
  persistent a2;
  persistent ks;
  persistent r23;
  persistent r46;
  persistent t23;
  persistent t46;
%
%  If this is the first call, compute
%
%    R23 = 2 ^ -23,
%    R46 = 2 ^ -46,
%    T23 = 2 ^ 23,
%    T46 = 2 ^ 46.
%
%  These are computed in loops, rather than by merely using the power operator,
%  in order to insure that the results are exact on all systems.
%
  if ( isempty ( ks ) )

    r23 = 1.0;
    r46 = 1.0;
    t23 = 1.0;
    t46 = 1.0;

    for i = 1 : 23
      r23 = 0.5 * r23;
      t23 = 2.0 * t23;
    end

    for i = 1 : 46
      r46 = 0.5 * r46;
      t46 = 2.0 * t46;
    end

    a = 1220703125.0;
%
%  Break A into two parts such that A = 2^23 * A1 + A2.
%
    t1 = r23 * a;
    a1 = floor ( t1 );
    a2 = a - t23 * a1;

    ks = 1;

  end

  if ( k < 0 )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'RANDLC_JUMP - Fatal error!\n' );
    fprintf ( 1, '  K < 0.\n' );
    error ( 'RANDLC_JUMP - Fatal error!' )

  elseif ( k == 0 )

%
%  Find M so that K < 2^M.
%
  else

    m = 1;
    twom = 2;
    while ( twom <= k )
      twom = twom * 2;
      m = m + 1;
    end

    b = a;
    b1 = a1;
    b2 = a2;

    for i = 1 : m

      j = floor ( k / 2 );
%
%  Replace X by A * X, if appropriate.
%
      if ( 2 * j ~= k )

        t1 = r23 * x;
        x1 = floor ( t1 );
        x2 = x - t23 * x1;

        t1 = b1 * x2 + b2 * x1;
        t2 = floor ( r23 * t1 );
        z = t1 - t23 * t2;

        t3 = t23 * z + b2 * x2;
        t4 = floor ( r46 * t3 );
        x = t3 - t46 * t4;

      end
%
%  Replace A by A * A mod 2^46.
%
      t1 = r23 * b;
      x1 = floor ( t1 );
      x2 = b - t23 * x1;

      t1 = b1 * x2 + b2 * x1;
      t2 = floor ( r23 * t1 );
      z = t1 - t23 * t2;

      t3 = t23 * z + b2 * x2;
      t4 = floor ( r46 * t3 );
      b = t3 - t46 * t4;
%
%  Update A1, A2.
%
      t1 = r23 * b;
      b1 = floor ( t1 );
      b2 = b - t23 * b1;

      k = j;

    end

  end

  value = r46 * x;

  return
end


function [A, C, Q, R, initx, initV, LL] = ...
    learn_kalman(data, A, C, Q, R, initx, initV, max_iter, diagQ, diagR, ARmode, constr_fun, varargin)
% LEARN_KALMAN Find the ML parameters of a stochastic Linear Dynamical System using EM.
%
% [A, C, Q, R, INITX, INITV, LL] = LEARN_KALMAN(DATA, A0, C0, Q0, R0, INITX0, INITV0) fits
% the parameters which are defined as follows
%   x(t+1) = A*x(t) + w(t),  w ~ N(0, Q),  x(0) ~ N(init_x, init_V)
%   y(t)   = C*x(t) + v(t),  v ~ N(0, R)
% A0 is the initial value, A is the final value, etc.
% DATA(:,t,l) is the observation vector at time t for sequence l. If the sequences are of
% different lengths, you can pass in a cell array, so DATA{l} is an O*T matrix.
% LL is the "learning curve": a vector of the log lik. values at each iteration.
% LL might go positive, since prob. densities can exceed 1, although this probably
% indicates that something has gone wrong e.g., a variance has collapsed to 0.
%
% There are several optional arguments, that should be passed in the following order.
% LEARN_KALMAN(DATA, A0, C0, Q0, R0, INITX0, INITV0, MAX_ITER, DIAGQ, DIAGR, ARmode)
% MAX_ITER specifies the maximum number of EM iterations (default 10).
% DIAGQ=1 specifies that the Q matrix should be diagonal. (Default 0).
% DIAGR=1 specifies that the R matrix should also be diagonal. (Default 0).
% ARMODE=1 specifies that C=I, R=0. i.e., a Gauss-Markov process. (Default 0).
% This problem has a global MLE. Hence the initial parameter values are not important.
%
% LEARN_KALMAN(DATA, A0, C0, Q0, R0, INITX0, INITV0, MAX_ITER, DIAGQ, DIAGR, F, P1, P2, ...)
% calls [A,C,Q,R,initx,initV] = f(A,C,Q,R,initx,initV,P1,P2,...) after every M step. f can be
% used to enforce any constraints on the params.
%
% For details, see
% - Ghahramani and Hinton, "Parameter Estimation for LDS", U. Toronto tech. report, 1996
% - Digalakis, Rohlicek and Ostendorf, "ML Estimation of a stochastic linear system with the EM
%      algorithm and its application to speech recognition",
%       IEEE Trans. Speech and Audio Proc., 1(4):431--442, 1993.

%    learn_kalman(data, A, C, Q, R, initx, initV, max_iter, diagQ, diagR, ARmode, constr_fun, varargin)
if nargin < 8, max_iter = 10; end
if nargin < 9, diagQ = 0; end
if nargin < 10, diagR = 0; end
if nargin < 11, ARmode = 0; end
if nargin < 12, constr_fun = []; end
verbose = 1;
thresh = 1e-4;

if ~iscell(data)
  N = size(data, 3);
  data = num2cell(data, [1 2]); % each elt of the 3rd dim gets its own cell
else
  N = length(data);
end

N = length(data);
ss = size(A, 1);
os = size(C,1);

alpha = zeros(os, os);
Tsum = 0;
for ex = 1:N
  %y = data(:,:,ex);
  y = data{ex};
  T = length(y);
  Tsum = Tsum + T;
  alpha_temp = zeros(os, os);
  for t=1:T
    alpha_temp = alpha_temp + y(:,t)*y(:,t)';
  end
  alpha = alpha + alpha_temp;
end

previous_loglik = -inf;
loglik = 0;
converged = 0;
num_iter = 1;
LL = [];

% Convert to inline function as needed.
if ~isempty(constr_fun)
  constr_fun = fcnchk(constr_fun,length(varargin));
end

while ~converged & (num_iter <= max_iter)

  %%% E step

  delta = zeros(os, ss);
  gamma = zeros(ss, ss);
  gamma1 = zeros(ss, ss);
  gamma2 = zeros(ss, ss);
  beta = zeros(ss, ss);
  P1sum = zeros(ss, ss);
  x1sum = zeros(ss, 1);
  loglik = 0;

  for ex = 1:N
    y = data{ex};
    T = length(y);
    [beta_t, gamma_t, delta_t, gamma1_t, gamma2_t, x1, V1, loglik_t] = ...
	Estep(y, A, C, Q, R, initx, initV, ARmode);
    beta = beta + beta_t;
    gamma = gamma + gamma_t;
    delta = delta + delta_t;
    gamma1 = gamma1 + gamma1_t;
    gamma2 = gamma2 + gamma2_t;
    P1sum = P1sum + V1 + x1*x1';
    x1sum = x1sum + x1;
    %fprintf(1, 'example %d, ll/T %5.3f\n', ex, loglik_t/T);
    loglik = loglik + loglik_t;
  end
  LL = [LL loglik];
  if verbose, fprintf(1, 'iteration %d, loglik = %f\n', num_iter, loglik); end
  %fprintf(1, 'iteration %d, loglik/NT = %f\n', num_iter, loglik/Tsum);
  num_iter =  num_iter + 1;

  %%% M step

  % Tsum =  N*T
  % Tsum1 = N*(T-1);
  Tsum1 = Tsum - N;
  A = beta * inv(gamma1);
  %A = (gamma1' \ beta')';
  Q = (gamma2 - A*beta') / Tsum1;
  if diagQ
    Q = diag(diag(Q));
  end
  if ~ARmode
    C = delta * inv(gamma);
    %C = (gamma' \ delta')';
    R = (alpha - C*delta') / Tsum;
    if diagR
      R = diag(diag(R));
    end
  end
  initx = x1sum / N;
  initV = P1sum/N - initx*initx';

  if ~isempty(constr_fun)
    [A,C,Q,R,initx,initV] = feval(constr_fun, A, C, Q, R, initx, initV, varargin{:});
  end

  converged = em_converged(loglik, previous_loglik, thresh);
  previous_loglik = loglik;
end

%%%%%%%%%

function [beta, gamma, delta, gamma1, gamma2, x1, V1, loglik] = ...
    Estep(y, A, C, Q, R, initx, initV, ARmode)
%
% Compute the (expected) sufficient statistics for a single Kalman filter sequence.
%

[os T] = size(y);
ss = length(A);

if ARmode
  xsmooth = y;
  Vsmooth = zeros(ss, ss, T); % no uncertainty about the hidden states
  VVsmooth = zeros(ss, ss, T);
  loglik = 0;
else
  [xsmooth, Vsmooth, VVsmooth, loglik] = kalman_smoother(y, A, C, Q, R, initx, initV);
end

delta = zeros(os, ss);
gamma = zeros(ss, ss);
beta = zeros(ss, ss);
for t=1:T
  delta = delta + y(:,t)*xsmooth(:,t)';
  gamma = gamma + xsmooth(:,t)*xsmooth(:,t)' + Vsmooth(:,:,t);
  if t>1 beta = beta + xsmooth(:,t)*xsmooth(:,t-1)' + VVsmooth(:,:,t); end
end
gamma1 = gamma - xsmooth(:,T)*xsmooth(:,T)' - Vsmooth(:,:,T);
gamma2 = gamma - xsmooth(:,1)*xsmooth(:,1)' - Vsmooth(:,:,1);

x1 = xsmooth(:,1);
V1 = Vsmooth(:,:,1);


function [gains]=lookup_gain_in_table(G,a_post,a_priori,a_post_range,a_priori_range,step);
% function [gains]=lookup_gain_in_table(G,a_post,a_priori,a_post_range,a_priori_range,step);
% This function selects the right gain value from the table G, given
% vectors with a priori and a posteriori SNRs
%
% INPUT variables:
% G: Matrix with gain values for speech DFT or magnitude estimation,
% evaluated at all combinations of a priori and a posteriori SNR in the
% input variables Rksi and Rgam.
%
%
% a_priori: Array of "a priori" SNR (SNRprior) values for which values
% have to be selected from the gain table   NOTE: The values must be in dBs.
% a_post: Array of "a posteriori" SNR (SNRpost) values for which values
% have to be selected from the gain table  NOTE: The values must be in dBs.
%
% a_post_range: The range of "a posteriori" SNR values
%
% a_priori_range: The range of "a priori" SNR values
%
% step: step is the stepsize in db's in the table
%
% OUTPUT variables:
% gains: Matrix with gain values that are selected from the gain table G
%
%
% Copyright 2007: Delft University of Technology, Information and
% Communication Theory Group. The software is free for non-commercial use.
% This program comes WITHOUT ANY WARRANTY.
%
% Last modified: 22-11-2007.

a_prioridb=round(10*log10(a_priori)/step)*step;
a_postdb=round(10*log10(a_post)/step)*step;
[Ia_post]=min(max(min(a_post_range),a_postdb), max(a_post_range));
Ia_post=Ia_post-min(a_post_range)+1;
Ia_post=Ia_post/step;
[Ia_priori]=min(max(min(a_priori_range),a_prioridb), max(a_priori_range));
Ia_priori=Ia_priori-min(a_priori_range)+1;
Ia_priori=Ia_priori/step;

gains=G(Ia_priori+(Ia_post-1)*length(G(:,1)));


% Copyright (C) 2009 International Business Machines
% All Rights Reserved.
% This code is published under the Eclipse Public License.
%
% $Id: hs071_c.c 699 2006-04-05 21:05:18Z andreasw $
%
% Author:  Andreas Waechter               IBM    2009-04-02
%
% This file is part of the Ipopt tutorial.  It is a version with
% mistakes for the matlab implemention of the coding exercise problem
% (in AMPL formulation):
%
% param n := 4;
%
% var x {1..n} <= 0, >= -1.5, := -0.5;
%
% minimize obj:
%   sum{i in 1..n} (x[i]-1)^2;
%
% subject to constr {i in 2..n-1}:
%   (x[i]^2+1.5*x[i]-i/n)*cos(x[i+1]) - x[i-1] = 0;
%
% The constant term "i/n" in the constraint is supposed to be input data

function [x, info] = TutorialMatlab

  % Size of the problem
  n = 5;

  % Problem data
  a = ((1:n)/n)';

  % Starting point
  x0 = -0.5*ones(n,1);

  % Lower and upper bounds for the variables
  options.lb = -1.5*ones(n,1);
  options.ub = zeros(n,1);

  % Constraint bounds
  options.cl = zeros(n-2,1);
  options.cu = zeros(n-2,1);

  % Set the Ipopt options
  % options.ipopt.mu_strategy = 'adaptive';
  options.ipopt.derivative_test = 'second-order';
  options.ipopt.max_iter = 10;

  % Set the callback functions
  funcs.objective         = @eval_f;
  funcs.constraints       = @eval_g;
  funcs.gradient          = @eval_grad_f;
  funcs.jacobian          = @eval_jac_g;
  funcs.jacobianstructure = @eval_jac_g_struct;
  funcs.hessian           = @eval_hess;
  funcs.hessianstructure  = @eval_hess_struct;

  [x info] = ipopt(x0, funcs, options);

  % End of main function
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Evaluate value of objective function
  function f = eval_f(x)

    tmp = (x - 1).^2;
    f = sum(tmp);

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Evaluate gradient of objective function
  function df = eval_grad_f(x)

    df = (x - 1);

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Evaluate value of constraint bodies
  function g = eval_g(x)

    g = (x(2:n-1).^2 + 1.5*x(1:n-2) - a(2:n-1)).*cos(x(3:n)) - x(1:n-2);

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Return constraint Jacobian strcture
  function A = eval_jac_g_struct

    % tri-diagonal structure
    Diags = ones(n, 3);
    A = spdiags(Diags, 0:2, n-2, n);

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Evaluate constraint Jacobian
  function A = eval_jac_g(x)

    Diags = -1*ones(n-2,3);
    Diags(:,2) = (2*x(2:n-1)+1.5).*cos(x(3:n));
    Diags(:,3) = -(x(2:n-1).^2 + 1.5*x(2:n-1) - a(2:n-1)).*sin(x(3:n));
    A = spdiags(Diags, 0:2, n-2, n);

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Return Hessian of Lagrangian function structure
  function H = eval_hess_struct

    Diags = ones(n, 2);
    H = spdiags(Diags, -1:0, n, n);

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Evaluate Hessian of Lagrangian function
  function H = eval_hess(x, sigma, lambda)

    Diags = zeros(n,2);

    % part from the objective function
    Diags(:,2) = sigma * 2;

    % (x_i , x_i) part
    Diags(2:n-1,2) = Diags(2:n-1,2) + lambda.*cos(x(3:n));

    % (x_{i+1}, x_{i+1}) part
    Diags(3:n,2) = Diags(3:n,2) ...
      - lambda.*(x(2:n-1).^2 + 1.5*x(1:n-2) - a(2:n-1)).*cos(x(3:n));

    % (x_i, x_{i+1}) part
    Diags(2:n-1,1) = -lambda.*sin(x(3:n)).*(2*x(2:n-1)+1.5);

    H = spdiags(Diags, -1:0, n, n);

  end

end


%FINDHOMOGRAPHY  Finds a perspective transformation between two planes
%
%     H = cv.findHomography(srcPoints, dstPoints)
%     [H, mask] = cv.findHomography(...)
%     [...] = cv.findHomography(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __srcPoints__ Coordinates of the points in the original plane, a numeric
%   array of size Nx2/1xNx2/Nx1x2 or cell array of 2-elements vectors
%   `{[x,y], ...}` (single floating-point precision).
% * __dstPoints__ Coordinates of the points in the target plane, of same size
%   and type as `srcPoints`.
%
% ## Output
% * __H__ 3x3 Homography matrix.
% * __mask__ Nx1 mask array of same length as input points, indicates inliers
%   (which points were actually used in the best computation of `H`).
%
% ## Options
% * __Method__ Method used to compute a homography matrix. The following
%   methods are possible:
%   * __0__ a regular method using all the points, i.e. the least squares
%     method (default)
%   * __Ransac__ RANSAC-based robust method.
%   * __LMedS__ Least-Median of squares robust method.
%   * __Rho__ PROSAC-based robust method, introduced in [Bazargani15].
%     (weighted RANSAC modification, faster in the case of many outliers).
% * __RansacReprojThreshold__ Maximum allowed reprojection error to treat a
%   point pair as an inlier (used in the RANSAC and RHO methods only). That
%   is, if
%   `|| dstPoints_i - convertPointsToHomogeneous(H*srcPoints_i) ||_2 > RansacReprojThreshold`
%   then the point `i` is considered as an outlier. If `srcPoints` and
%   `dstPoints` are measured in pixels, it usually makes sense to set this
%   parameter somewhere in the range of 1 to 10. default 3.0.
% * __MaxIters__ The maximum number of RANSAC iterations. default 2000
% * __Confidence__ Confidence level, between 0 and 1. default 0.995
%
% The function finds and returns the perspective transformation `H` between
% the source and the destination planes:
%
%     s_i * [x_i'; y_i'; 1] ~ H * [x_i; y_i; 1]
%
% so that the back-projection error:
%
%     sum_{i} (x_i' - (h11*x_i + h12*y_i + h13)/(h31*x_i + h32*y_i + h33))^2 +
%             (y_i' - (h21*x_i + h22*y_i + h23)/(h31*x_i + h32*y_i + h33))^2
%
% is minimized. If the parameter method is set to the default value 0, the
% function uses all the point pairs to compute an initial homography estimate
% with a simple least-squares scheme.
%
% However, if not all of the point pairs `(srcPoints_i, dstPoints_i)` fit the
% rigid perspective transformation (that is, there are some outliers), this
% initial estimate will be poor. In this case, you can use one of the three
% robust methods. The methods RANSAC, LMedS and RHO try many different
% random subsets of the corresponding point pairs (of four pairs each,
% collinear pairs are discarded), estimate the homography matrix using this
% subset and a simple least-squares algorithm, and then compute the
% quality/goodness of the computed homography (which is the number of inliers
% for RANSAC or the least median re-projection error for LMedS). The best
% subset is then used to produce the initial estimate of the homography matrix
% and the mask of inliers/outliers.
%
% Regardless of the method, robust or not, the computed homography matrix
% is refined further (using inliers only in case of a robust method) with
% the Levenberg-Marquardt method to reduce the re-projection error even
% more.
%
% The methods RANSAC and RHO handle practically any ratio of outliers but
% need a threshold to distinguish inliers from outliers. The method LMedS
% does not need any threshold but it works correctly only when there are
% more than 50% of inliers. Finally, if there are no outliers and the noise
% is rather small, use the default method (`Method=0`).
%
% The function is used to find initial intrinsic and extrinsic matrices.
% Homography matrix is determined up to a scale. Thus, it is normalized so
% that `h33 = 1`. Note that whenever an `H` matrix cannot be estimated, an
% empty one will be returned.
%
% ## References
% [Fischler81]:
% > Fischler, M. A., and R. C. Bolles. "Random sample consensus: A paradigm
% > for model fitting with applications to image analysis and automated
% > cartography", Communications of the Association for Computing Machinery
% > 24 (1981): 381-395.
%
% [Rousseeuw84]:
% > Rousseeuw, P. J. "Least median of squares regression", Journal of the
% > American Statistical Association, 79 (1984): 871-880.
%
% [Inui03]:
% > Inui, K., S. Kaneko, and S. Igarashi. "Robust Line Fitting using LMedS
% > Clustering", Systems and Computers in Japan 34 (2003): 92-100.
%
% [Bazargani15]:
% > Hamid Bazargani, Olexa Bilaniuk, and Robert Laganiere. "A fast and robust
% > homography scheme for real-time planar target detection". Journal of
% > Real-Time Image Processing (2015): 1-20.
%
% See also: cv.getAffineTransform, cv.estimateAffine2D,
% cv.estimateAffinePartial2D, cv.getPerspectiveTransform,
% cv.estimateRigidTransform, cv.warpPerspective, cv.perspectiveTransform
%