function [U] = hyperNfindr(M, q)
% HYPERNFINDR Performs the N-FINDR (endmember extraction) algorithm
%   Performs the N-FINDR algorithm to find q endmembers. If only M is
%  given as input, this function calls hyperHfcVd to estimate the number
%  of endmembers (q) and then hyperPct to reduce dimensionality to (q-1).
%
% Usage
%   [U] = hyperNfindr(M)
%   [U] = hyperNfindr(M, q)
% Inputs
%   M - 2d matrix of HSI data (p x N)
%   q - Number of endmembers to find
%       -- if not given, q is obtained from hyperHfcVd(M, 10^-3)
% Outputs
%   U - Recovered endmembers (p x q)
%
% References
%   M. Winter, "N-findr: an algorithm for fast autonomous
% spectral endmember determination in hyperspectral data," SPIE’s
% International Symposium on Optical Science, Engineering, and
% Instrumentation, pages 266–275. International Society for Optics
% and Photonics, 1999.

% Error trapping
if ndims(M) ~= 2
    warning('WarnTests:dim', ...
            'Input image must be p x N.\n',...
            'Converting with hyperConvert2d.\n')
    M = hyperConvert2d(M);
end

M_orig = M;
[p, N] = size(M);

if nargin == 1
    fprintf('Implementing hyperHfcVd to determine the number of endmembers.\n')
    q = hyperHfcVd(M_orig, [10^-3]);
    fprintf('Reducing dimensionality to (q-1) using hyperPct.\n')
    M = hyperPct(M, q-1);
elseif q < p+1
    warning('WarnTests:dim', ...
    strcat('N-FINDR requires (q-1) spectral bands.\n',...
           'Performing PCA to reduce dimensionality.\n'))
    M = hyperPct(M, q-1);
elseif q > p+1
    warning('WarnTests:dim', ...
    strcat('N-FINDR requires (q-1) spectral bands.\n',...
           'Performing PCA to reduce dimensionality.\n'))
    error('ErrTests:dim', ...
        strcat('N-FINDR cannot find more than (p+1) endmembers (q),\n', ...
               'where p is the number of available spectral bands.\n'))
end

% Initialize
U_idx = randperm(N,q); % Random endmember selection
E     = M(:,U_idx);    % Endmember matrix
V     = abs(det([ones(1,q); E])) / factorial(q-1); % Simplex volume
vols  = zeros(q,1);

% Search for maximum volume simplex
for j = 1:N;
    % Replace each column of E with sample vector M(:,j)
    % and compute the volume for each
    for k = 1:q;
        E_tmp      = E;
        E_tmp(:,k) = M(:,j);
        vols(k)    = abs(det([ones(1,q); E_tmp])) / factorial(q-1);
    end
    % If max volume is greater than previous V, update E and V
    [V_tmp,k_idx] = max(vols);
    if V_tmp > V
        V            = V_tmp;
        E(:,k_idx)   = M(:,j);
        U_idx(k_idx) = j;
    end
end

% Return endmembers
U = M_orig(:, U_idx);


function [tt]=tt_qutrtoepl(x)

% returns the multilevel upper-triangular Toeplitz matrix tt generated by the multi-dimensional input vector x
% in the QTT format
%
% If the size of the input vector is N,
% then the size of the output matrix is N x N.
%
% April 20, 2011
% Vladimir Kazeev
% vladimir.kazeev@gmail.com
% INM RAS
% Moscow, Russia
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% For details please see the preprint
% http://www.mis.mpg.de/publications/preprints/2011/prepr2011-36.html
% Vladimir A. Kazeev, Boris N. Khoromskij and Eugene E. Tyrtyshnikov
% Multilevel Toeplitz matrices generated by QTT tensor-structured vectors and convolution with logarithmic complexity
% January 12, 2012
% Vladimir Kazeev,
% Seminar for Applied Mathematics, ETH Zurich
% vladimir.kazeev@sam.math.ethz.ch
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

d=size(x,1);
tt=tt_qshiftstack_r(d);
tt=tt_qreshape(tt,3,[4*ones(d,1),2*ones(d,1)]);
tt=tt_mv(tt,x);
tt=tt_qreshape(tt,1,2*ones(d,2));

return
end


function [doy,yr]=jd2doy(jd);
% JD2DOY  Converts Julian date to year and day of year.
% . Non-vectorized version. See also CAL2JD, DOY2JD,
%   GPS2JD, JD2CAL, JD2DOW, JD2GPS, JD2YR, YR2JD.
% Version: 24 Apr 99
% Usage:   [doy,yr]=jd2doy(jd)
% Input:   jd  - Julian date
% Output:  doy - day of year
%          yr  - year

% Copyright (c) 2011, Michael R. Craymer
% All rights reserved.
% Email: mike@craymer.com

if nargin ~= 1
  warning('Incorrect number of arguments');
  return;
end
if jd < 0
  warning('Julian date must be greater than or equal to zero');
  return;
end

[yr,mn,dy] = jd2cal(jd);
doy = jd - cal2jd(yr,1,0);


function [p sres sres_ns] = ResidScan(res, FWHM)
% function [p sres sres_ns] = ResidScan(res, FWHM)
%
% Calculates P(M>=t) where M is the max value of the smoothed residuals.
% In this implementation the residuals are smoothed using a Gaussian
% kernel.
%
% INPUT:
%
% res - residual time course
% FWHM - Full Width Half Maximum (in time units)
%
% OUTPUT:
%
% p - pvalues
% sres - smoothed residuals
% sres_ns - smoothed residuals (non standardized)
%
% By Martin Lindquist & Ji-Meng Loh, July 2007
%
% Edited by ML on 10/02/09

res_ns = res;
res = res./std(res);
len = length(res);

% Create Gaussian Kernel
sig = ceil(FWHM/(2*sqrt(2*log(2))));
klen = 3*sig;
kern = normpdf((-klen:klen),0,sig);
kern = kern./sqrt(sum(kern.^2));

% Convolve
x = conv(res,kern);
sres = x((klen + 1):(end-klen));

x = conv(res_ns,kern/sum(kern));
sres_ns = x((klen + 1):(end-klen));

% Find Max value
[a,location] = max(abs(sres));

% Find p-values using Gaussian Random Field theory
z = Euler_p(1, a, len, FWHM);
z = 2*z;        %Two-sided test
p = min(1, z);

end

% END MAIN FUNCTION

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Subfunctions
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function pval = Euler_p(myDim, value, N, fwhm)
% function z = Euler_p(myDim, value, N, fwhm)
%
% Finds the p value using the expected Euler characteristic.
%
% This function returns P(M \ge value) using the approximation
% \sum_{d=0}^D R_d(V) \rho_d(value) following Worsley et al's "A Unified
% Statistical Approach for Determining Significant Signals in Images of
% Cerebral Activation".
%
% INPUTS:
%
% myDim - the number of dimensions in the data
% value - the value of the maximum.
% N     - the number of (time) points in that 1 dimension
% fwhm  - the full width half maximum
%
% OUTPUTS:
%
% pval - the p-value

% NOTE: CURRENTLY THIS FUNCTION IS ONLY IMPLEMENTED FOR THE 1D CASE

  % Constants
  myfactor = 4*log(2);
  pi2 = 2*pi;
  exptsq = exp(-(value^2)/2);

  % Euler Characteristc Densties
  rho = zeros(5,1);
  rho(1) = 1-normcdf(value);
  rho(2) = myfactor^(0.5)*exptsq/pi2;
  rho(3) = myfactor * exptsq * value / (pi2 ^ (1.5));
  rho(4) = myfactor ^ (1.5) * exptsq * (value^2-1) / (pi2 ^2);
  rho(5) = myfactor ^2 * exptsq * (value^3-3*value) / (pi2 ^ (5/2));

  % Resel Count
  R0 = 1;
  R1 = N/fwhm;

  % P-value
  pval = R0 * rho(1) + R1 * rho(2);

end


function [ g , mu ] = gsp_design_translates(G, g0,N )
%GSP_DESIGN_TRANSLATES Create a filterbank by uniformly translating a window
%   Usage: g = gsp_design_translates( G, g0, Ntrans );
%
%   Inputs parameters:
%       G       : Graph structure
%       g0      : Mother window (anonymous function)
%       N       : Number of translate
%
%   Outputs parameters:
%       g       : filterbank
%       mu      : Centers of the filters
%
%   This function construct a filter bank of *N* uniformly translated
%   filter from the mother filter *g0*.
%

% Author : Nathanael Perraudin
% Date: 6 January 2016

if isstruct(G)
    if ~isfield(G,'lmax')
        if param.verbose
            fprintf('GSP_DESIGN_TRANSLATE has to compute lmax \n')
        end
        G = gsp_estimate_lmax(G);
    end
   lmax = G.lmax;
else
   lmax = G;
end

mu = linspace(0,lmax,N);

g = cell(length(mu),1);

for ii = 1:length(mu)
    g{ii} = @(x) g0(x-mu(ii));
end

end


function b = r83_mv ( m, n, a, x )

%*****************************************************************************80
%
%% R83_MV multiplies an R83 matrix times an R8VEC.
%
%  Discussion:
%
%    The R83 storage format is used for a tridiagonal matrix.
%    The superdiagonal is stored in entries (1,2:N), the diagonal in
%    entries (2,1:N), and the subdiagonal in (3,1:N-1).  Thus, the
%    original matrix is "collapsed" vertically into the array.
%
%  Example:
%
%    Here is how an R83 matrix of order 5 would be stored:
%
%       *  A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54  *
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(3,N), the R83 matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(M), the product A * x.
%
  b = zeros ( m, 1 );

  mn = min ( m, n );

  if ( n == 1 )
    b(1,1) = a(2,1) * x(1,1);
    if ( 1 < m )
      b(2,1) = a(3,1) * x(1,1);
    end
    return
  end

  b(1,1)      = a(2,1)       * x(1,1) ...
              + a(1,2)       * x(2,1);

  b(2:mn-1,1) = a(3,1:mn-2)' .* x(1:mn-2,1) ...
              + a(2,2:mn-1)' .* x(2:mn-1,1) ...
              + a(1,3:mn)'   .* x(3:mn,1);

  b(mn,1)     = a(3,mn-1)    * x(mn-1,1) ...
              + a(2,mn)      * x(mn,1);

  if ( n < m )
    b(mn+1,1) = b(mn+1,1) + a(3,mn) * x(mn,1);
  end

  if ( m < n )
    b(mn,1) = b(mn,1) + a(1,mn+1) * x(mn+1,1);
  end

  return
end


%% UNO Rosenbrock
clc
clear
%Objective
obj = @(x) (1-x(1))^2 + 100 *(x(2)-x(1)^2)^2;
%Setup Options
opts = optiset('solver','m1qn3','display','iter');
%Build & Solve
Opt = opti('obj',obj,'ndec',2,'options',opts)
x0 = [0 0]';
[x,fval,exitflag,info]= solve(Opt,x0)

%%
% grad = Opt.nlprob.funcs.gradient;
grad = @(x) autoJac(obj,x);
opts.display = 2;
opts.nupdates = 5;
opts.maxtime = 0.5;
opts.iterfun = [];

[x,f,e,i,ii] = m1qn3(obj,grad,x0,opts);

%% 4x Rosenbrock [1,1,1,1]
clc
%Objective
fun = @(x) sum(100*(x(1:end-1).^2-x(2:end)).^2+(x(1:end-1)-1).^2);
grad = @(x) mklJac(fun,x);
x0 = [randn(1,100) 1];
opts.display = 2;
[x,fval,ef,iter] = m1qn3(fun,grad,x0,opts);


function [ final_dist ] = re_ranking( feat, M, W, query_num, k1, k2, lambda)
% k-reciprocal re-ranking

%% initial ranking list
original_dist = MahDist(M, feat' * W, feat' * W);
[~, initial_rank] = sort(original_dist, 2, 'ascend');
gallery_num = size(original_dist,2);
%% compute k-reciprocal feature
V = zeros(size(original_dist), 'single');
original_dist = original_dist./ repmat(max(original_dist, [], 2), 1, size(original_dist, 2));
for i = 1:size(original_dist, 1)
    %% k-reciprocal neighbors
    forward_k_neigh_index = initial_rank(i, 1: k1 + 1);
    backward_k_neigh_index  = initial_rank(forward_k_neigh_index, 1: k1+1);
    [fi, ~, ~]= find(backward_k_neigh_index == i);
    k_reciprocal_index  = forward_k_neigh_index(fi);
    k_reciprocal_expansion_index = k_reciprocal_index;
    for j = 1: length(k_reciprocal_index)
        candidate = k_reciprocal_index(j);
        candidate_forward_k_neigh_index = initial_rank(candidate, 1: round((k1+1)/2));
        candidate_backward_k_neigh_index = initial_rank(candidate_forward_k_neigh_index, 1: round((k1+1)/2));
        [fi_candidate, ~, ~]= find(candidate_backward_k_neigh_index == candidate);
        candidate_k_reciprocal_index = candidate_forward_k_neigh_index(fi_candidate);
        if length(intersect(k_reciprocal_index, candidate_k_reciprocal_index)) > 2/3*length(candidate_k_reciprocal_index)
            k_reciprocal_expansion_index = [k_reciprocal_expansion_index candidate_k_reciprocal_index];
        end
    end
    k_reciprocal_expansion_index = unique(k_reciprocal_expansion_index);
    %% feature encoding
    weight = exp(-original_dist(i, k_reciprocal_expansion_index));
    V(i, k_reciprocal_expansion_index) = weight/sum(weight);
end
%% local query expansion
if k2 ~=1
    V_qe = zeros(size(V), 'single');
    for i = 1:size(V, 1)
        V_qe(i, :) = single(mean(V(initial_rank(i, 1:k2), :), 1));
    end
    V = V_qe;
    V_qe = [];
end

%% Inverted Index
% Inpsired by Song Bai, Xiang Bai,
% Sparse Contextual Activation for Efficient Visual Re-ranking, TIP, 2016.
% We apply the inverted index to quickly compute the Jaccard distance.

invIndex = cell(gallery_num, 1);
for i = 1:gallery_num
    invIndex{i} = find(V(:, i) ~=0);
end

jaccard_dist = zeros(size(original_dist), 'single');

for i = 1:query_num
    temp_min = zeros(1, gallery_num, 'single');
    indNonZero = find( V( i, : ) ~= 0 );
    indImages = invIndex( indNonZero );
    for j = 1 : length( indNonZero )
        temp_min( 1, indImages{j} ) = temp_min( 1, indImages{j} )...
            + single( min( V(i, indNonZero(j)), V(indImages{j}, indNonZero(j)) ) )';
    end
    jaccard_dist(i, :) = bsxfun(@minus, 1, temp_min./(2 - temp_min));
end

final_dist = jaccard_dist*(1-lambda) + original_dist*lambda;
final_dist = final_dist(1:query_num,query_num+1:end)';

end


function [db,f]=v_lpccc2db(cc,np,nc,c0)
%V_LPCCC2DB Convert complex cepstrum to dB power spectrum DB=(CC,NP,NC)
%
%  Inputs: cc(nf,n)     Complex ceptral coefficients excluding c(0), one frame per row
%          np           Size of output spectrum is np+1 [n]
%                       Alternatively, np can be a vector of output frequencies in the range 0 to 0.5
%          nc           Highest cepstral coefficient to use [np or, if np is a vector, n]
%                       Set nc=-1 to use n coefficients
%          c0(nf,1)     Cepstral coefficient cc(0) [0]
%
% Outputs: db(nf,np+2)  Power spectrum from DC to Nyquist in dB
%          f(1,np+2)    Normalized frequencies (0 to 0.5)
%
% The "complex cepstral coefficients", cc(n), are the inverse discrete-time Fourier transform
% of the log of the complex-valued spectrum. The cc(n) are real-valued and, for n<0, cc(n)=0.
% The "real cepstral coeffcients", rc(n), are the inverse discrete-time Fourier transform
% of the log of the magnitude spectrum; rc(0)=cc(0) and rc(n)=0.5*cc(n) for n~=0.
% For highest speed, choose np to be a power of 2.

%      Copyright (C) Mike Brookes 1998-2014
%      Version: $Id: v_lpccc2db.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nf,mc]=size(cc);
if nargin<2 || ~numel(np)
    if nargout
        np=mc;
    else
        np=128;
    end
end
k=10/log(10);
if nargin>=3 && numel(nc)==1 && nc==-1 nc=mc; end
if nargin<4 || ~numel(c0) c0=zeros(nf,1); end
if numel(np)>1 || np(1)<1
    if nargin<3 || ~numel(nc) nc=mc; end
    f=np(:)';
    if nc==mc
        db=k*(2*[c0 cc]*cos(2*pi*(0:mc)'*f));
    else
        db=k*(2*[c0 lpccc2cc(cc,nc)]*cos(2*pi*(0:nc)'*f));
    end
else
    if nargin<3 || ~numel(nc) nc=np; end
    if nc==mc
        db=k*(2*real(v_rfft([c0 cc].',2*np).'));
    else
        db=k*(2*real(v_rfft([c0 v_lpccc2cc(cc,nc)].',2*np).'));
    end
    f=linspace(0,0.5,np+1);
end
if ~nargout
    plot(f,db.');
    xlabel('Normalized frequency f/f_s');
    ylabel('Gain (dB)');
end


function Rth=th_rot(phi,theta)
%TH_ROT Calculates the rotation matrix for the RF pulse flip angle, taking into
% consideration of the RF pulses phase.
%
%   phi: RF pulse flip-angle (radian)
%   theta: RF pulse phase (radian).

Rz = z_rot(-theta);
Rx = x_rot(phi);
Rth = inv(Rz)*Rx*Rz;

end


function [psi] = dynpcm22psi(dynpcm2)
% Convert pressure from dynes per square centimeter to pounds per square
% inch
% Chad Greene 2012
psi = dynpcm2*0.0000145038;


function [O, DzDw  ] = convo ( I , gamma , DzDy1 ,  DzDy2)
%% Convolution layer
%% This layer has a parameter: D_{l} = sum_{m=1}^{s} gamma_{l,m}B_{m};
%% Copyright (c) 2017 Yan Yang
%% All rights reserved.

%% network setting
config;
fN = nnconfig.FilterNumber;
fS = nnconfig.FilterSize;
pad = nnconfig.Padding;
gp = nnconfig.EnableGPU;
s=fS*fS-1;
[m,n] = size(I);

D = zeros(fS, fS, fN);
B = filter_base( );
for i = 1:fN
    D(:,:,i) = reshape(B*gamma(:,i),fS,fS);
end
DT = rot90(D,2);

if nargin == 2
    for i=1:fN
        O(:,:,i) = imfilter( double(I) ,double(D(:,:,i)),'same','circular','conv');
    end
end

if nargin == 4
    DzDy = DzDy1 + DzDy2 ;
    if gp
        Dp = gpuArray(D);
        E = gpuArray(zeros(m,m));
        D1p = gpuArray(DT);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  O
        for i = 1:fN
            d1 = padImage_g(DzDy(:,:,i),[pad,pad],'circular');
            d1 = gpuArray(d1);
            dd = conv2(d1,D1p(:,:,i),'valid');
            E = E + dd;
        end
        O = gather(E);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DzDw
        DzDy = gpuArray(DzDy);
        Bj1p = FBASE(B,1);
        Bjp = rot90(Bj1p,2);
        DzDw = gpuArray(zeros(s,fN));
        d1 = padImage_g(I,[pad,pad],'circular');
        d1 = gpuArray(d1);

        for j = 1:fN
            for k=1:s  % m
                dd = conv2(d1,Bjp(:,:,k),'valid');
                tp = DzDy(:,:,j).* dd;
                DzDw (k,j)=sum(tp(:));
            end
        end

        DzDw=gather(DzDw);

    else

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  O
        O = zeros(m,n);
        DT = rot90(D,2);
        for i=1:fN
            O = O + imfilter(double(DzDy(:,:,i)),double(DT(:,:,i)),'same','circular','conv');
        end
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DzDw
        for j = 1:fN
            for k=1:s
                Bj = reshape(B(:,k),fS,fS);
                tp = DzDy(:,:,j).* imfilter(double(I),double(Bj),'same','circular','conv');
                DzDw (k,j)=sum(tp(:));
            end
        end
    end
end
end


function disp (F)
%DISP displays the factorization F
%
% Example
%   F = factorize (A)
%   disp (F)
%
% See also factorize.

% Copyright 2009, Timothy A. Davis, University of Florida

fprintf ('  A:\n') ;
disp (F.A) ;

if (~isempty (F.L))
    fprintf ('  L:\n') ;
    disp (F.L) ;
end

if (~isempty (F.U))
    fprintf ('  U:\n') ;
    disp (F.U) ;
end

if (~isempty (F.Q))
    fprintf ('  Q:\n') ;
    disp (F.Q) ;
end

if (~isempty (F.R))
    fprintf ('  R:\n') ;
    disp (F.R) ;
end

if (~isempty (F.p))
    fprintf ('  p:\n') ;
    disp (F.p) ;
end

if (~isempty (F.q))
    fprintf ('  q:\n') ;
    disp (F.q) ;
end

fprintf ('  is_inverse: %d kind: %d\n', F.is_inverse, F.kind) ;

% print the kind of factorization that F contains

switch F.kind

    case 1

        fprintf ('  Q-less economy sparse QR factorization: ') ;
        fprintf ('(A*q)''*A*q = R''*R\n');

    case 2

        fprintf ('  dense economy QR factorization: A = Q*R\n') ;

    case 3

        fprintf ('  Q-less economy sparse QR factorization: ') ;
        fprintf ('(p*A)*(p*A)'' = R''*R\n') ;

    case 4

        fprintf ('  dense economy QR factorization: A'' = Q*R\n') ;

    case 5

        fprintf ('  sparse Cholesky factorization: ')  ;
        fprintf ('q*A*q'' = L*L''\n');

    case 6

        fprintf ('  dense Cholesky factorization: A = L*L''\n') ;

    case 7

        fprintf ('  sparse LU factorization: p*A*q = L*U\n') ;

    case 8

        fprintf ('  dense LU factorization: p*A = L*U\n') ;

end


function Trajectory = Create_Trajectory(Params)
%
% FUNCTION:
%   This function is used to draw the road that the tractor trailer truck
%   is driving on. A different representation is used for the forward and
%   reverse versions of the road.
%
%   This function defines a trajectory as a series of points and gains
%   associated with that point. Trajectory is a struct with two fields.
%
% INPUT:
%   Params = a struct of parameters for this problem. The field Traj is
%   used in this function. See Set_Parameters.m and Trajectory_Examples.m
%   for more details.
%
% OUTPUT:
%   Trajectory.States = a (2xN) cell array with the state at each of the N points
%   Trajectory.Inputs = a (2xN) cell array the nominal actuator effort
%   Trajectory.Gains = A (2xN) cell array.
%   {1,:} ==> Forward
%   {2,:} ==> Reverse

Npts = Params.Traj.Npts;

if Params.Traj.Func == false

    %Set constraint points:
    SetPts = Params.Traj.SetPts;
    Order = Params.Traj.Order;

    P = polyfit(SetPts(:,1),SetPts(:,2),Order);
    x = linspace(min(SetPts(:,1)),max(SetPts(:,1)),Npts+1);
    y = polyval(P,x);
else
   x = Params.Traj.x;
   y = Params.Traj.y;
end

%Now we need the desired trailer angle theta, which is defined as the angle
%between the trailer center line and the positive y axis.

%Vector direction between successive points
x_step = diff(x);
y_step = diff(y);
angle = atan2(y_step,x_step);  %angle from the positive x axis
th = angle-pi/2;

%Assume that the cab angle should always try to be centered
phi = zeros(size(th));

%Truncate the vectors
x=x(1:end-1);
y=y(1:end-1);

% % % %PLOT THINGS
% % %
% % % subplot(2,1,2)
% % % plot(x(1:end-1),th)
% % % title('Angle vs X Position')
% % %
% % % subplot(2,1,1)
% % % plot(x(1:end-1),y(1:end-1))
% % % axis equal
% % % title('Road')

%% Now do gain matrix calculations

Trajectory.States = cell(2,Npts);
Trajectory.Inputs = cell(2,Npts);
Trajectory.Gains = cell(2,Npts);
for i=1:Npts
   %Drive Forward
        State = [x(i); y(i); th(i); phi(i)];
        Input = [1;0];  %[speed;steering]
        Trajectory.States{1,i} = State;
        Trajectory.Inputs{1,i} = Input;
        Trajectory.Gains{1,i} =  Set_Gain_Matrix(State, Input, Params);

   %Drive Backward
        State_Rev = [x(i); y(i); th(i)+pi; phi(i)];
        Input_Rev = [-1,0];   %[speed;steering]
        Trajectory.States{2,i} = State_Rev;
        Trajectory.Inputs{2,i} = Input_Rev;
        Trajectory.Gains{2,i} =  Set_Gain_Matrix(State_Rev, Input_Rev, Params);
end

end


function new = colormap_optimization(cmap)
%colormap_optimisation: minimally modify colormap to satisfy constraints.
% In this case, the constraints are for linear grayscale luminance and a
% neutral mid-gray central color.
%
% Example:
%  sprung = colormap_optimization(spring(5));
%  colormap_visualization(spring(5))
%  colormap_visualization(sprung)
%
% See also: colormap_investigation, colormap_visualization, bipolar

% Copyright 2009 Ged Ridgway at gmail com

%% Conversion from RGB to grayscale brightness/luminance

% From http://www.poynton.com/ColorFAQ.html (Q.9)
% coef = [0.2126 0.7152 0.0722]'

% From rgb2gray:
% T = inv([1.0 0.956 0.621; 1.0 -0.272 -0.647; 1.0 -1.106 1.703]);
% coef = T(1,:)';

% From  http://gimp-savvy.com/BOOK/index.html?node54.html
coef = [0.3 0.59 0.11]';
% Note, the Gimp coef is almost identical to that from rgb2gray, and these
% seem nicer in a print('-dps') grayscale copy than the Poynton version.

%% Constraints
m = size(cmap, 1);
mid = zeros(1, m);
mid(round((m+1)/2)) = 1;
Aeq = [
    kron(coef', eye(m)) % linearity in luminance
    kron(eye(2,3), mid) % centrality
    ];
l = 0.1; % lower luminance (avoid black, to give contrast with text/lines)
u = 0.9; % upper luminance (avoid white, to give contrast with background)
beq = [
    linspace(l, u, m)'  % linearity in luminance
    ones(2, 1) / 2      % centrality
    ];

%% Optimisation (using MATLAB optimisation toolbox)
orig = warning('off', 'optim:lsqlin:LinConstraints');
x = lsqlin(eye(3*m), cmap(:), [], [], Aeq, beq, zeros(3*m, 1), ones(3*m, 1));
x = max(min(x, 1), 0); % enforce bounds, since lsqlin can give e.g. -1e-18
new = reshape(x, m, 3);
warning(orig);


function test_approx_test08 ( )

%*****************************************************************************80
%
%% TEST_APPROX_TEST08 plots a cubic spline interpolant for problem 7.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 August 2011
%
%  Author:
%
%    John Burkardt
%
  approx_filename = 'test08_approx.txt';
  data_filename = 'test08_data.txt';
  jmax = 7;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST_APPROX_TEST08\n' );
  fprintf ( 1, '  Plot a cubic spline interpolant for problem 7.\n' );

  prob = 7;
%
%  Get the data.
%
  data_num = p00_data_num ( prob );

  [ xdata, ydata ] = p00_dat ( prob, data_num );

  r8vec2_write ( data_filename, data_num, xdata, ydata );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Data values stored in "%s"\n', data_filename );
%
%  Set up the interpolation function.
%
  ibcbeg = 0;
  ibcend = 0;
  ybcbeg = 0.0;
  ybcend = 0.0;

  ypp = spline_cubic_set ( data_num, xdata, ydata, ibcbeg, ybcbeg, ibcend, ...
    ybcend );
%
%  Evaluate the interpolation function.
%
  plot = 0;
  nplot = ( jmax - 1 ) * ( data_num - 1 ) + 1;
  xplot = zeros ( nplot, 1 );
  yplot = zeros ( nplot, 1 );

  for i = 1 : data_num - 1

    if ( i == data_num - 1 )
      jhi = jmax;
    else
      jhi = jmax - 1;
    end

   for j = 1 : jhi

      xval = ( ( jmax - j     ) * xdata(i)     ...
             + (        j - 1 ) * xdata(i+1) ) ...
             / ( jmax     - 1 );

      [ yval, ypval, yppval ] = spline_cubic_val ( data_num, xdata, ydata, ...
        ypp, xval );

      plot = plot + 1;
      xplot(plot) = xval;
      yplot(plot) = yval;

    end

  end

  r8vec2_write ( approx_filename, nplot, xplot, yplot );

  fprintf ( 1, '  Approximant values stored in "%s"\n', approx_filename );

  return
end


function [ SV ] = moveFV(V,F,S)
% MOVEFV  Move a scalar field defined on faces to vertices by averaging
%
% [ SV ] = moveFV(V,F,S)
%
% Input:
%   V,F  mesh
%   S  scalar field defined on faces, Fx1
%
% Output:
%   SV  scalar field defined on vertices

SV = zeros(size(V,1),size(S,2));
COUNT = zeros(size(V,1),1);

for i=1:size(F,1)
    SV(F(i,:)',:) = SV(F(i,:)',:) + repmat(S(i,:),size(F,2),1);
    COUNT(F(i,:)') = COUNT(F(i,:)') + 1;
end

SV = SV ./ repmat(COUNT, 1, size(S,2));

end


%SPATIALGRADIENT  Calculates the first order image derivative in both x and y using a Sobel operator
%
%     [dx, dy] = cv.spatialGradient(src)
%     [...] = cv.spatialGradient(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __src__ input image, 8-bit single-channel.
%
% ## Output
% * __dx__ output `int16` image with first-order derivative in x.
% * __dy__ output `int16` image with first-order derivative in y.
%
% ## Options
% * __KSize__ size of Sobel kernel. It must be 3 in the current implementation.
%   default 3
% * __BorderType__ Pixel extrapolation method, see cv.copyMakeBorder. Only
%   'Default', 'Reflect101', and 'Replicate' are supported. default 'Default'
%
% Equivalent to calling:
%
%     dx = cv.Sobel(src, 'DDepth','int16', 'XOrder',1, 'YOrder',0, 'KSize',3);
%     dy = cv.Sobel(src, 'DDepth','int16', 'XOrder',0, 'YOrder',1, 'KSize',3);
%
% See also: cv.Sobel, imgradientxy, imgradient
%


function wathen_test11 ( )

%*****************************************************************************80
%
%% WATHEN_TEST11 assemble, factor and solve using WATHEN_ST + CG_ST.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 June 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'WATHEN_TEST11\n' );
  fprintf ( 1, '  Assemble, factor and solve a Wathen system\n' );
  fprintf ( 1, '  defined by WATHEN_ST and CG_ST.\n' );
  fprintf ( 1, '\n' );

  nx = 1;
  ny = 1;
  fprintf ( 1, '  Elements in X direction NX = %d\n', nx );
  fprintf ( 1, '  Elements in Y direction NY = %d\n', ny );
  fprintf ( 1, '  Number of elements = %d\n', nx * ny );
%
%  Compute the number of unknowns.
%
  n = wathen_order ( nx, ny );
  fprintf ( 1, '  Number of nodes N = %d\n', n );
%
%  Compute the matrix size.
%
  nz_num = wathen_st_size ( nx, ny );
  fprintf ( 1, '  Number of nonzeros = %d\n', nz_num );
%
%  Set up a random solution X1.
%
  seed = 123456789;
  [ x1, seed ] = r8vec_uniform_01 ( n, seed );
%
%  Compute the matrix.
%
  seed = 123456789;
  [ row, col, a, seed ] = wathen_st ( nx, ny, nz_num, seed );
%
%  Compute the corresponding right hand side B.
%
  b = mv_st ( n, n, nz_num, row, col, a, x1 );
%
%  Solve the linear system.
%
  x2 = ones ( n, 1 );
  x2 = cg_st ( n, nz_num, row, col, a, b, x2 );
%
%  Compute the maximum solution error.
%
  e = max ( abs ( x1 - x2 ) );
  fprintf ( 1, '  Maximum solution error is %g\n', e );

  return
end


function [indexes] = FindCornerSolutions(front)

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

% This function is written by Annibale Panichella

[m,n] = size(front);

%% let's normalize the objectives
if m<=n
  indexes = 1:m;
  return
end

%% let's define the axes of the n-dimensional spaces
W = eye(n);
[r,~]= size(W);
indexes = zeros(1,n);
for i=1:r
   [~, index] = min(Point2LineDistance(front, zeros(1,n), W(i,:)));
   indexes(i) = index;
end

end


function [M, L, Y, C] = lmnn(X, labels)
%LMNN Learns a metric using large-margin nearest neighbor metric learning
%
%   [M, L, Y, C] = lmnn(X, labels)
%
% The function uses large-margin nearest neighbor (LMNN) metric learning to
% learn a metric on the data set specified by the NxD matrix X and the
% corresponding Nx1 vector labels. The metric is returned in M.
%
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    % Initialize some variables
    [N, D] = size(X);
    assert(length(labels) == N);
    [lablist, ~, labels] = unique(labels);
    K = length(lablist);
    label_matrix = false(N, K);
    label_matrix(sub2ind(size(label_matrix), (1:length(labels))', labels)) = true;
    same_label = logical(double(label_matrix) * double(label_matrix'));
    M = eye(D);
    C = Inf; prev_C = Inf;

    % Set learning parameters
    min_iter = 50;          % minimum number of iterations
    max_iter = 1000;        % maximum number of iterations
    eta = .1;               % learning rate
    mu = .5;                % weighting of pull and push terms
    tol = 1e-3;             % tolerance for convergence
    best_C = Inf;           % best error obtained so far
    best_M = M;             % best metric found so far
    no_targets = 3;         % number of target neighbors

    % Select target neighbors
    sum_X = sum(X .^ 2, 2);
    DD = bsxfun(@plus, sum_X, bsxfun(@plus, sum_X', -2 * (X * X')));
    DD(~same_label) = Inf; DD(1:N + 1:end) = Inf;
    [~, targets_ind] = sort(DD, 2, 'ascend');
    targets_ind = targets_ind(:,1:no_targets);
    targets = false(N, N);
    targets(sub2ind([N N], vec(repmat((1:N)', [1 no_targets])), vec(targets_ind))) = true;

    % Compute pulling term between target neigbhors to initialize gradient
    slack = zeros(N, N, no_targets);
    G = zeros(D, D);
    for i=1:no_targets
        G = G + (1 - mu) .* (X - X(targets_ind(:,i),:))' * (X - X(targets_ind(:,i),:));
    end

    % Perform main learning iterations
    iter = 0;
    while (prev_C - C > tol || iter < min_iter) && iter < max_iter

        % Compute pairwise distances under current metric
        XM = X * M;
        sum_X = sum(XM .* X, 2);
        DD = bsxfun(@plus, sum_X, bsxfun(@plus, sum_X', -2 * (XM * X')));

        % Compute value of slack variables
        old_slack = slack;
        for i=1:no_targets
            slack(:,:,i) = ~same_label .* max(0, bsxfun(@minus, 1 + DD(sub2ind([N N], (1:N)', targets_ind(:,i))), DD));
        end

        % Compute value of cost function
        prev_C = C;
        C = (1 - mu) .* sum(DD(targets)) + ...  % push terms between target neighbors
                 mu  .* sum(slack(:));          % pull terms between impostors

        % Maintain best solution found so far (subgradient method)
        if C < best_C
            best_C = C;
            best_M = M;
        end

        % Perform gradient update
        for i=1:no_targets

            % Add terms for new violations
            [r, c] = find(slack(:,:,i) > 0 & old_slack(:,:,i) == 0);
            G = G + mu .* ((X(r,:) - X(targets_ind(r, i),:))' * ...
                           (X(r,:) - X(targets_ind(r, i),:)) - ...
                           (X(r,:) - X(c,:))' * (X(r,:) - X(c,:)));

            % Remove terms for resolved violations
            [r, c] = find(slack(:,:,i) == 0 & old_slack(:,:,i) > 0);
            G = G - mu .* ((X(r,:) - X(targets_ind(r, i),:))' * ...
                           (X(r,:) - X(targets_ind(r, i),:)) - ...
                           (X(r,:) - X(c,:))' * (X(r,:) - X(c,:)));
        end
        M = M - (eta ./ N) .* G;

        % Project metric back onto the PSD cone
        [V, L] = eig(M);
        V = real(V); L = real(L);
        ind = find(diag(L) > 0);
        if isempty(ind)
            warning('Projection onto PSD cone failed. All eigenvalues were negative.'); break
        end
        M = V(:,ind) * L(ind, ind) * V(:,ind)';
        if any(isinf(M(:)))
            warning('Projection onto PSD cone failed. Metric contains Inf values.'); break
        end
        if any(isnan(M(:)))
            warning('Projection onto PSD cone failed. Metric contains NaN values.'); break
        end

        % Update learning rate
        if prev_C > C
            eta = eta * 1.01;
        else
            eta = eta * .5;
        end

        % Print out progress
        iter = iter + 1;
        no_slack = sum(slack(:) > 0);
        if rem(iter, 10) == 0
            [~, sort_ind] = sort(DD, 2, 'ascend');
            disp(['Iteration ' num2str(iter) ': error is ' num2str(C ./ N) ...
                  ', nearest neighbor error is ' num2str(sum(labels(sort_ind(:,2)) ~= labels) ./ N) ...
                  ', number of constraints: ' num2str(no_slack)]);
        end
    end

    % Return best metric and error
    M = best_M;
    C = best_C;

    % Compute mapped data
    [L, S, ~] = svd(M);
    L = bsxfun(@times, sqrt(diag(S)), L);
    Y = X * L;
end

function x = vec(x)
    x = x(:);
end


function [ CostMatric,Cost ] =  MinimalDirectedMSF( CostMatric )
% MST on a directed graph
% Chu-Liu/Edmonds Algorithm:
%1 Discard the arcs entering the root if any; For each node other than the root, select the entering arc with the highest cost; Let the selected n-1 arcs be the set S.
%  If no cycle formed, G( N,S ) is a MST. Otherwise, continue.
%2 For each cycle formed, contract the nodes in the cycle into a pseudo-node (k), and modify the cost of each arc which enters a node (j) in the cycle from some node (i)
%  outside the cycle according to the following equation.  c(i,k)=c(i,j)-(c(x(j),j)-min_{j}(c(x(j),j)) where c(x(j),j) is the cost of the arc in the cycle which enters j.
%3 For each pseudo-node, select the entering arc which has the smallest modified cost; Replace the arc which enters the same real node in S by the new selected arc.
%4 Go to step 2 with the contracted graph.

% Pay attention, we extend the Chu-Liu algorithm into considering
% forest.And we consider of discarding the premise of giving the root here.
% CostMatric must be symmetric

  [ Number, Component ] = conncomp(  biograph( CostMatric ),'Weak', true );
  for k = 1:Number
      LocalNodes = find( Component == k );
      Dim = size( LocalNodes,2 );
      if Dim == 1
         continue
      elseif Dim == 2
          if CostMatric( LocalNodes(1),LocalNodes(2) ) > CostMatric( LocalNodes(2),LocalNodes(1) )
              CostMatric( LocalNodes(2),LocalNodes(1) ) = 0;
          else
              CostMatric( LocalNodes(1),LocalNodes(2) ) = 0;
          end
      elseif Dim > 2
      ComponentCost = CostMatric( LocalNodes,LocalNodes );
      MinTree = zeros( Dim ); MinTreeCost = Inf;
      for m = 1:Dim
          Root = m;
          LocalCostMatric = ComponentCost;
          LocalCostMatric( :,Root ) = zeros( Dim,1 );
          [ LocalTree,LocalCostTree ] =  DirectedMinimalSpanningTree( LocalCostMatric,Root );
          if MinTreeCost > LocalCostTree
             MinTree = LocalTree;
             MinTreeCost = LocalCostTree ;
          end
      end
      CostMatric( LocalNodes,LocalNodes ) = MinTree;
      end
   end
   Cost = sum( sum( CostMatric ));
end


function varargout=plotmodt(t,tper,y,fitflag,MarkerSize)

% plotmodt(t,tper,y)
% plotmodt(t,tper,y,fitflag)
% plotmodt(t,tper,y,fitflag,MarkerSize)
% [tt,yy]=plotmodt(t,tper,y)
% [tt,yy,yyres]=plotmodt(t,tper,y,1)
%
% The Modulo Time Plot is useful for quick visual inspection
% of system performance including dynamic range, distortion and
% error plots, the detection of random bit errors, and timing errors
% between the test signal and the sample clock. [1]
%
% The method also works well on plotting eye diagrams.
%
% INPUTS        t: time vector, required as t is not always linearly spaced
%               tper: the trace period
%               y: input signal
%               fitflag: if 1, perform fit to find the residual
%                        works well for distorted 1-tones, see also (*)
%               MarkeSize: To adjust the resolution of the plot default:4
%                         (if y is large, MarkerSize should be small)
% OUPUTS        tt & yy: inputs t and y rearranged into a trace period
%               yyres: residual of y (if fit is done) also rearranged
%
% type plotmodt to launch two examples
%
% Author: Marko Neitola, University of Oulu, Finland
%
%[1] F. H. Irons andD. M. Hummels,
%"The Modulo Time Plot - a Useful Data Acquisition Diagnostic Tool"
%IEEE Tr. on Instrumentation and Measurement, Vol.45, NO. 3, June 1996
%(*) the order of fit is fixed to 17, unless you download
%    the "polydeg" function (optimal polyn. degree) by Damien Garcia:
%    http://www.biomecardio.com/matlab/polydeg.html

if nargin ==0
    % Example 1 grabbed from for EYEMAP File ID: #15900
    n_simb = 1024; %Number of symbols
    ak=round(rand(1,n_simb)); %create random binary sequence
    n_pt=128; %Number of samples per symbol
    signal=zeros(1,length(ak)*n_pt); %initialize signal variable
    for k=1:length(ak) %Generate polar signal from digital sequence
        signal((k-1)*n_pt+1:k*n_pt) = 2*(ak(k)-0.5);
    end;
    %Filter signal using a gaussian filter
    signal=real(ifft(fft(signal).*exp(-(1:length(signal)).^2/n_simb^2)));
    %Generate random gaussian noise
    noise=randn(size(signal))*0.1;
    %Visualize eye diagram of signal and noise
    figure
    plotmodt(0:131071,n_pt*4,signal+noise,0,4)
    %eyemap(signal+noise,n_pt*2)  % you can compare this to File ID: #15900

    %example 2: distorted sinusoid
    N=pow2(11);          	%vector length
    fs = pi*1e3;            %sampling freq
    ts = 1/fs;              %sampling interval
    freq = (N/128-1)*fs/N;  %freq (Hz)
    phase = -pi/8;          %phase (rad)
    offset = pi*4;          %offset (i.e mean)
    amplitude = pi/3;       %amplitude
    t = (0:(N-1))*ts;       %time vector
    std_jitter = 1e-2;  %standard deviation of jitter noise
    std_addnoi = 1e-1;  %standard deviation of  noise added to signal
    std_phase  = 0.2;   %standard deviation of phase noise
    noise = randn(1,N);
    std1_noise = noise/std(noise);  % random vector with stdev = 1
    jit_noise = std_jitter*std1_noise;
    phase_noise = std_phase*std1_noise;
    add_noise = std_addnoi*std1_noise;
    w=2*pi*freq;
    t = t + ts*jit_noise;                          % add clock jitter
    A2 = 0.1;     % 2. harmonic ampl
    A3 = 0.5;     % 3. harmonic ampl
    yin = cos(w*t+phase+phase_noise);  % sinusoid with phase noise
    yin = offset+amplitude*yin+A2*yin.*yin+A3*yin.*yin.*yin+add_noise; %add offset, noise & harmonics
    figure
    plotmodt(t,1/freq,yin,1,4)
    return
end

ts = norm(diff(t))/sqrt(length(t)-1);
lstyle = 'k.';

%The main idea is in these 3 lines:
tt=mod(t,tper);
[tt,index]=sort(tt);
yy=y(index);

if nargin == 3
    fitflag=0;
end
if nargin < 5
    MarkerSize = 4;
end

if fitflag == 1
    if exist('polydeg','file')==2
        n_poly = polydeg(tt,yy);
    else
        n_poly=17;
    end
    warning off %#ok<WNOFF>
    p=polyfit(tt,yy,n_poly);
    warning on %#ok<WNON>
    yest=polyval(p,tt);
    legendtext = ['polyfit (o:',int2str(n_poly),')'];

    yyresi = yy-yest;
    subplot(211)

    plot(tt,yy,lstyle,tt,yest,'-r','MarkerSize',MarkerSize)
    title(['trace period plot, period is ', num2str(tper,2),'s. t_s = ', num2str(ts,2),'s'])
    axis tight,legend('data',legendtext,1),xlabel('time, 1 period')

    subplot(212)
    plot(tt,yyresi,lstyle,'MarkerSize',MarkerSize)
    title('trace period plot, residual'), axis tight,xlabel('time, 1 period')

else
    subplot(111)
    plot(tt,yy,lstyle,'MarkerSize',MarkerSize)
    title(['trace period plot, period is ', num2str(tper,2),'s. t_s = ', num2str(ts,2),'s'])
    axis tight,xlabel('time, 1 period')
end

if nargout>0
    varargout{1}=tt;
    varargout{2}=yy;
    if nargout == 3
        varargout{3}=yyresi;
    end
end


function asp = aspectRatio(grains,varargin)
% aspectratio = length / width
%
% the aspect ratio is the ratio between the two
% <grain2d.principalComponents.html,principal componentes> of a grain
%
% Input
%  g - @grain2d
%
% Output
%  asp   - aspect--ratio
%
% See also
% grain2d/principalcomponents

[~,a,b] = principalComponents(grains);
asp = abs(a ./ b);

% asp2 = diameter(grains).^2 ./ area(grains);


function linplus_test265 ( )

%*****************************************************************************80
%
%% TEST265 tests R8GB_TO_R8GE, R8GE_TO_R8GB.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  m = 5;
  n = 8;
  ml = 2;
  mu = 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST265\n' );
  fprintf ( 1, '  R8GB_TO_R8GE copies a R8GB matrix to a R8GE matrix.\n' );
  fprintf ( 1, '  R8GE_TO_R8GB copies a R8GE matrix to a R8GB matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix rows M =      %d\n', m );
  fprintf ( 1, '  Matrix columns N =   %d\n', n );
  fprintf ( 1, '  Lower bandwidth ML = %d\n', ml );
  fprintf ( 1, '  Upper bandwidth MU = %d\n', mu );

  a = r8gb_indicator ( m, n, ml, mu );

  r8gb_print ( m, n, ml, mu, a, '  The R8GB matrix:' );

  b = r8gb_to_r8ge ( m, n, ml, mu, a );

  r8ge_print ( m, n, b, '  The R8GE matrix:' );

  c = r8ge_to_r8gb ( m, n, ml, mu, b );

  r8gb_print ( m, n, ml, mu, c, '  The recovered R8GB matrix:' );

  return
end


% Script demonstrating usage of the cbpdnjnt function.
%
% Author: Brendt Wohlberg <brendt@lanl.gov>  Modified: 2016-07-01
%
% This file is part of the SPORCO library. Details of the copyright
% and user license can be found in the 'Copyright' and 'License' files
% distributed with the library.

% Load dictionary
load([sporco_path '/Data/ConvDict.mat']);
dmap = containers.Map(ConvDict.Label, ConvDict.Dict);
D = dmap('12x12x36');

% Load test image
s = single(stdimage('lena'))/255;
if isempty(s),
  error('Data required for demo scripts has not been installed.');
end

% Highpass filter test image
npd = 16;
fltlmbd = 5;
[sl, sh] = lowpass(s, fltlmbd, npd);

% Compute representation
lambda = 1e-2;
mu = 1e-3;
opt = [];
opt.Verbose = 1;
opt.MaxMainIter = 500;
opt.rho = 100*lambda + 1;
opt.RelStopTol = 1e-3;
opt.AuxVarObj = 0;
opt.HighMemSolve = 1;
[X, optinf] = cbpdnjnt(D, sh, lambda, mu, opt);

% Compute reconstruction
DX = squeeze(ifft2(sum(bsxfun(@times, fft2(D, size(X,1), ...
             size(X, 2)), fft2(X)),3), 'symmetric'));

figure;
subplot(1,3,1);
plot(optinf.itstat(:,2));
xlabel('Iterations');
ylabel('Functional value');
subplot(1,3,2);
semilogy(optinf.itstat(:,6));
xlabel('Iterations');
ylabel('Primal residual');
subplot(1,3,3);
semilogy(optinf.itstat(:,7));
xlabel('Iterations');
ylabel('Dual residual');

figure;
imagesc(squeeze(sum(abs(X),3)));
title('Sum of absolute value of coefficient maps');

figure;
subplot(1,3,1);
imdisp(s);
title('Original image');
subplot(1,3,2);
imdisp(DX + sl);
title(sprintf('Reconstructed image (SNR: %.2fdB)', snr(s, DX + sl)));
subplot(1,3,3);
sdf = DX + sl - s;
sdf = sdf - min(sdf(:));
sdf = sdf / max(sdf(:));
imagesc(sdf);
axis image; axis off;
title('Difference');


% NOLM_Attractor - Plot a chaotic attractor for the NOLM.
% Copyright Springer 2013 A.L. Steele and S. Lynch.
clear
N=50000;P=100;start=500;
lambda=1.55E-6;n2=3.2E-20;Aeff=30E-12;L=80;
E1(1)=0;phi=0*pi;
Eout(1:N)=0;
phif=0*pi; E6p=0;

kappa1=0.25;kappa2=0.8;kappa3=0.8;
rootk1 = sqrt(kappa1); irootk1 = 1i*sqrt(1-kappa1);
rootk2 = sqrt(kappa2); irootk2 = 1i*sqrt(1-kappa2);
rootk3 = sqrt(kappa3); irootk3 = 1i*sqrt(1-kappa3);

G=sqrt((1-kappa2)*(1-kappa3));
% iterate
for n=1:N

    E1 = rootk3*sqrt(P)+irootk3*E6p;
    P1 = abs(E1)^2;

    E3 = rootk1*E1;
    E4 = irootk1*E1;

    phic=2*pi*n2*L*(2-kappa1)*P1/(lambda*Aeff);
    phicc=2*pi*n2*L*(1+kappa1)*P1/(lambda*Aeff);

    E3p = E3*exp(-1i*(phi+phic));
    E4p = E4*exp(-1i*(phi+phicc));

    E5 = rootk1*E3p+irootk1*E4p;

    Eout(n) = rootk2*E5;
    x(n)=real(Eout(n));
    y(n)=imag(Eout(n));
    E6 = irootk2*E5;

    E6p = E6*exp(1i*phif);

end
axis([-10 10 -10 10])
axis equal
plot(x(start:N),y(start:N),'.','MarkerSize',1);
fsize=15;
set(gca,'xtick',-10:5:10,'FontSize',fsize)
set(gca,'ytick',-10:5:10,'FontSize',fsize)
xlabel('Real E','FontSize',fsize)
ylabel('Imag E','FontSize',fsize)


function E = energyVector(C,x,V,P,varargin)
% Calculates Energy velocity vector (km/s)
%
% Description
% Energy velocity for lossless elastic medium (i.e. no attenuation)
% Good proxy for group velocity, which typically has some energy loss
% The formula is given by
% F.I. Fedorov(1968)Theory of Elastic Waves in Crystals, 375 pp. New York: Penum Press.
%
% Ve_i = C_ijkl P_j P_l X_k / rho*V
%
% N.B. E_magnitude should be equal or more than plane wave velocity vp, vs1 or vs2
%
% David Mainprice 6/02/2018
%
% Syntax
%   E = energyVector(C,x,v,p)
%   E = energyVector(C,x,v,p,rho)
%   E = energyVector(C,[],vFun,pFun)
%   E = energyVector(C,[],vFun,pFun,rho)
%
% Input
%  C - @stiffnessTensor (units GPa)
%  x - @vector3d propagation direction
%  v - plane wave velocity (unit km/s) e.g. vp,vs1 or vs2
%  p - @vector3d plane wave polarization vector e.g. pp,ps1 or ps2
%  vFun - @S2Fun plane wave velocity (unit km/s) e.g. vp,vs1 or vs2
%  pFun - @S2AxisField plane wave polarization vector e.g. pp,ps1 or ps2
%  rho - density in g/cm3
%
% Output
%  E - Energy velocity vector (units km/s)
%

% return a function if required
if isempty(x)
  if isa(V,'S2FunTri')
    E = S2VectorFieldTri(V.tri,energyVector(C,V.vertices,V.values,P.values,varargin{:}));
  else
    E = S2VectorFieldHarmonic.quadrature(@(x) energyVector(C,x,V,P,varargin{:}),'bandwidth',128,C.CS);
  end
  return
end

% make X and P to be unit vectors
x = x.normalize;

if ~isa(P,'vector3d'), P = P.eval(x); end
if ~isnumeric(V), V = V.eval(x); end
P = P.normalize;

% get density
if nargin == 5
  rho = varargin{1};
elseif isfield(C.opt,'density')
  rho = C.opt.density;
else
  rho = 1;
  warning('No density given! I''m going to use the density rho=1.');
end

% E_vector
E = vector3d(EinsteinSum(C,[1 -2 -3 -4],P(:),-2,P(:),-4,x(:),-3))./rho ./V(:);

end


% vgg_line3d_from_lP_nonlin  Non-linear estimation of (possibly constrained) 3D line segment from image line segments.
%
% SYNOPSIS
% L = vgg_line3d_from_lP_nonlin(s,P [,imsize] [,L0] [,X] [,nonlin_opt])
%
% s ... cell(K) of double(3,3), inv. covariance matrices of the K image line segments:-
%   - If the segments are estimated from edges, it is s(:,k) = x*x',
%     where x (3-by-N) are homog. coordinates of the edgels with last components 1.
%   - If only end points are available, s(:,k) = d*x*y' where x, y (column 2-vectors)
%     are the segment's end points and d its length.
%
% P ... K-cell with 3-by-4 camera matrices
%
% imsize ... size (2,K), image size(s) for preconditiong.
%   Omit if s and P are already preconditioned.
%
% L0 ... double(4,2), initial scene line (optional). Homogeneous points L0(:,i) span
%   the line. If omitted, linear estimation is done first.
%
% X ... constraint on L :-
%   - if X is omitted:     no constraint on L
%   - if X is double(4,1): L goes thru point X
%   - if X is double(4,2): L goes thru 3D line spanned by X(:,i)
%
% nonlin_opt ... options for Levenberg-Marquardt. It is comma-separated list
%   of pairs ['option',value]. Possible options are :-
% opt ... options structure with possible fields :-
%   - verbose ... 1 or 0 (default: 0)
%   - niter_term ... maximum number of iterations
%   - rmsstep_term ... terminating step of rms of residuals
%   - lambda_term ... terminating value of lambda (default: 1e10)
%   - lambda_init ... initial value of lambda
% E.g., vgg_line3d_from_lP_nonlin(...,'lambda_init',1e-9,'niter_term',5).
%
% L ... double(4,2), estimated 3D line. Points L(:,i) span the line.
%
% Note: use [] if you want to omit a parameter and use a later one, e.g.
%   vgg_line3d_from_lP_nonlin(s,P,imsize,[],[],'verbose',1,'lam_init',1e-9)
%
% ALGORITHM
% - Minimization is done by Levenberg-Marquardt.
% - 3D line L is parameterized by image lines in the first two images.
%   The positions of these image lines are possibly constrained by X.

% T.Werner, Feb 2002

function L = vgg_line3d_from_lP_nonlin(s,P,imsize,L,X,varargin)

if nargin < 3, imsize = []; end
if nargin < 4, L = []; end
if nargin < 5, X = []; end

if isempty(L)
  L = vgg_line3d_from_lP_lin(s,P,imsize);
end
K = length(P); % number of images
if K<2
  error('Cannot reconstruct 3D line from 1 image');
end
if isempty(X) & K==2 % no need for non-linear minimization
  return
end

% Prepare square root of covariance matrices; now s{k}(:,n) has meaning of 3 homogeneous image points
for k = 1:K
  [us,ss,vs] = svd(s{k},0);
  s{k} = us*sqrt(ss);
end

% Preconditioning
if ~isempty(imsize)
  for k = 1:K
    H = vgg_conditioner_from_image(imsize(:,k));
    P{k} = H*P{k};
    s{k} = H*s{k};
    scale(k) = H(1,1); % save the scales for evaluating objective function
  end
else
  scale = ones(1,K);
end

switch size(X,2)
 case 0
  % Scene line L is unconstrained, having thus 4 DOF.
  % L is parameterized by two image lines in images 1 and 2, each having 2 DOF, as follows:
  %   l1 = l0(1,:) + p(1:2)'*ldelta{1}
  %   l2 = l0(2,:) + p(3:4)'*ldelta{2}
  % where row 4-vector p represents 4 DOF of L.
  for k = 1:2
    l0(k,:) = normx(vgg_wedge(P{k}*L)')';
    ldelta{k} = null(l0(k,:))';
  end

  % optimization
  p = levmarq(@F, {vertcat(P{:}),s,scale,l0,ldelta},...
              @normsolve,...
              [0;0;0;0],...
              varargin{:});
  l = l12_from_p(p,l0,ldelta);

 case 1
  % Scene line L is constrained to intersect the scene point X, having thus 2 DOF.
  % L is parameterized by two image lines in images 1 and 2, each having 1 DOF, as follows:
  %   l1 = l0(1,:) + p(1)*ldelta{1}
  %   l2 = l0(2,:) + p(2)*ldelta{2}
  % where 2-vector p represents 2 DOF of L.
  for k = 1:2
    l0(k,:) = normx(vgg_wedge(P{k}*L)')';
    x = P{k}*X;

    % Since L might not intersect X, move l0(k,:) 'as little as possible' to intersect x.
    l0(k,:) = l0(k,:) - (l0(k,:)*x)/(x'*x).*x';

    Q = null(x')';
    ldelta{k} = null(l0(k,:)*pinv(Q))'*Q;
  end

  % optimization
  p = levmarq(@F, {vertcat(P{:}),s,scale,l0,ldelta},...
              @normsolve,...
              [0;0],...
              varargin{:});
  l = l12_from_p(p,l0,ldelta);

 case 2
  % Scene line L is constrained to intersect the scene line given by 2 points X.
  % This constraint is given by
  %   l1*G*l2' = 0
  % where G is 3x3 rank 2 matrix (analogical in fact to fundamental matrix) and
  %   G = P{1}*M*P{2}'
  % where M is Pluecker matrix of line given by X, M = X(:,1)*X(:,2)'-X(:,2)*X(:,1).
  %
  % This constraint can be written as
  %   p(1:2)*D*p(3:4)' + p(1:2)*d{2}' + d{1}*p(3:4)' + c = 0
  % where D, d, c are given below and 4-vector p are 4 parameters of L.
  %
  % L is parameterized by two image lines in images 1 and 2, each having 2 DOF, as follows:
  %   l1 = l0(1,:) + p(1:2)'*ldelta{1}
  %   l2 = l0(2,:) + p(3:4)'*ldelta{2}
  % where p(1:3) are chosen freely and p(4) is computed from the above formula as
  %   p(4) = -(p(1:2)'*(D(:,1)*p(3)+d{1})+p(3)*d{2}(1)+c)/(p(1:2)'*D(:,2)+d{2}(2)).
  Lpm = X(:,1)*X(:,2)' - X(:,2)*X(:,1)';
  G = P{1}*Lpm*P{2}';
  for k = 1:2
    l0(k,:) = normx(vgg_wedge(P{k}*L)')';
  end

  % As L might not intersect line X, move l0(2,:) 'as little as possible' to enforce l0(1,:)*G*l0(2,:)'==0.
  x = (l0(1,:)*G)';
  l0(2,:) = l0(2,:) - (l0(2,:)*x)/(x'*x).*x';

  for k = 1:2
    ldelta{k} = null(l0(k,:))';
  end

  D = ldelta{1}*G*ldelta{2}';
  d{1} = ldelta{1}*G*l0(2,:)';
  d{2} = ldelta{2}*G'*l0(1,:)';
  c = l0(1,:)*G*l0(2,:)';

  % optimization
  p = levmarq(@F, {vertcat(P{:}),s,scale,l0,ldelta,D,d,c},...
              @normsolve,...
              [0;0;0],...
              varargin{:});
  l = l12_from_p(p,l0,ldelta,D,d,c);

end
if all(~isnan(l(:)))
  L = null([l(1,:)*P{1}; l(2,:)*P{2}]);
end

return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% objective function

% Objective function of Levenberg-Marquardt
function [y,w,J] = F(p,P,s,scale,varargin)
K = length(s);

% l := lines in images 1, 2 from p
l = l12_from_p(p,varargin{:});

% l := reprojection of lines in images 1, 2 to all images
if all(abs(p) < inf)
  [dummy,dummy,L]= svd([l(1,:)*P(1:3,:); l(2,:)*P(4:6,:)],0);
else
  L = inf*ones(4);
end
l = norml(vgg_wedge(reshape(P*L(:,3),[3 K]),reshape(P*L(:,4),[3 K])));

% compute residual function
y = [];
for k = 1:K
  y = [y l(k,:)*s{k}];
end
y = y';

w = [1;1;1] * (1./scale);
w = w(:);

% else, compute also jacobian
if nargout < 2
  return
end
dif = 1e-6;
J = zeros(length(y),length(p));
for i = 1:length(p)
  pdif = p;
  pdif(i) = pdif(i) + dif;
  J(:,i) = (F(pdif,P,s,scale,varargin{:}) - y)/dif;
end
return

% The following function computes lines in the first two images
% from parameters p. Explanation see above.
function l = l12_from_p(p,l0,ldelta,D,d,c)
switch length(p)
 case 4 % unconstrained
  l = [l0(1,:) + p(1:2)'*ldelta{1}
       l0(2,:) + p(3:4)'*ldelta{2}];
 case 2 % going thru X
  l = [l0(1,:) + p(1)*ldelta{1}
       l0(2,:) + p(2)*ldelta{2}];
 case 3 % going thru L
  p(4) = -(p(1:2)'*(D(:,1)*p(3)+d{1})+p(3)*d{2}(1)+c)/(p(1:2)'*D(:,2)+d{2}(2));
  l = [l0(1,:) + p(1:2)'*ldelta{1}
       l0(2,:) + p(3:4)'*ldelta{2}];
end
return

function dp = normsolve(J,Y,w,lambda)
OLDWARN = warning('off');
dp = -( J'*diag(w)*J + lambda*eye(size(J,2)) ) \ ( J'*(Y.*w) );
warning(OLDWARN);
return

function x = normx(x)
if ~isempty(x)
  x = x./(ones(size(x,1),1)*sqrt(sum(x.*x)));
end

function l = norml(l)
% l = norml(l)  Multiplies hyperplane l by scalar so that for each n, norm(l(1:end-1,n))==1.
l = l./(sqrt(sum(l(:,1:end-1).^2,2))*ones(1,size(l,2)));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% a = levmarq(@RES,PARAMS,@NORMSOLVE,a [,opt])  Non-linear least-squares by Levenberg-Marquardt.
%
% Minimizes f(a)'*W*f(a) over a.
%
% @RES ... residual function f called like [e,w,J] = RES(a,PARAMS{:}), where
%    - a ... double(M,1), parameter vector
%    - e ... double(N,1), residual vector
%    - J ... double(N,M), derivative of e wrt a
%    - w ... double(N,1), weights of e; covariance matrix of e is diag(1/e.^2).
%            Use 1 instead of ones(N,1).
% For efficiency, RES should not compute the jacobian if called with two output parameters only.
%
% @NORMSOLVE ... function solving normal equations, called like da = NORMSOLVE(J,e,W,lambda).
% a ... initial parameter vector
% opt ... options structure, see code

function [a,w] = levmarq(RES,PARAMS,NORMSOLVE,a0,varargin)

% options
[opt,rem_opt] = vgg_argparse( { 'niter_term',     +inf,...
                                'drmsrel_term',    0,...
                                'loglambda_term',  6,...
                                'loglambda_init', -4,...
                                'verbose',         0 },...
                              varargin );
if ~isempty(rem_opt), if ~isempty(fieldnames(rem_opt))
  error(['Unknown option(s) ' fieldnames(rem_opt)]);
end, end

% Initial statistics
if opt.verbose
  [e0,w0] = feval(RES,a0,PARAMS{:});
  ssd0 = sum( (e0.*w0).^2 );
  fprintf( '                         [rms=%14.12g] [maxabs=%14.12g]\n',...
          sqrt(ssd0/length(e0)),...
          max(abs(e0.*w0)) );
end

loglambda = opt.loglambda_init;
niter = 0;
while 1

  % Compute actual residual and jacobian
  [e0,w0,J] = feval(RES,a0,PARAMS{:});

  % Update a as a := a0 + da, by finding
  % optimal lambda and solving normal equations for da.
  nfail = 1;
  while (loglambda < opt.loglambda_term)
    niter = niter + 1;

    a = a0 + feval(NORMSOLVE,J,e0,w0,10^loglambda);
    [e,w] = feval(RES,a,PARAMS{:});

    if sum((e.*w).^2) < sum((e0.*w0).^2) % success
      a0 = a;
      loglambda = loglambda - 1;
      break
    end

    if opt.verbose
      fprintf('%4i.%.2i: [loglambda=%3i] [REJECTED]\n',niter,nfail,loglambda);
    end

    loglambda = loglambda + 1;
    nfail = nfail + 1;
  end

  % Print statistic after successful iteration
  ssd0 = sum( (e0.*w0).^2 );
  ssd = sum( (e.*w).^2 );
  if opt.verbose
    fprintf( '%4i   : [loglambda=%3i] [rms=%14.12g] [maxabs=%14.12g] [drmsrel=%4g%%]\n',...
             niter,...
             round(loglambda),...
             sqrt(ssd/length(e)),...
             max(abs(e.*w)),...
             100*(1-sqrt(ssd/ssd0)) );
  end

  % Termination criteria
  test(1) = loglambda <  opt.loglambda_term;
  test(2) = ssd0-ssd  >= opt.drmsrel_term^2*ssd0;
  test(3) = niter     <  opt.niter_term;
  if any(test==0)
    break
  end
end

if opt.verbose
  onoff = {'YES','no'};
  fprintf( ' Levenberg-Marquardt finished succesfully.\n Reason for termination:\n' );
  fprintf( '   lambda  = %s\n', onoff{test(1)+1} );
  fprintf( '   drmsrel = %s\n', onoff{test(2)+1} );
  fprintf( '   niter   = %s\n', onoff{test(3)+1} );
end

return

function print_statistics(niter,loglambda,e0,w0,e,w,opt)
if opt.verbose
  ssd0 = sum( (e0.*w0).^2 );
  ssd = sum( (e.*w).^2 );
  fprintf( '%4i   : [loglambda=%3i] [rms=%14.12g] [maxabs=%14.12g] [drmsrel=%11.5g]\n',...
           niter,...
           round(loglambda),...
           sqrt(ssd/length(e)),...
           max(abs(e.*w)),...
           sqrt(1-ssd/ssd0) );
end
return


clear;
clc;
close all;
format

%% 读取数据

%% 角速度为rad， 加速度为m/s^(2)
load example_ins4.mat

Fs = 100;
N = length(acc);

% 惯导解算, 初始化
p = zeros(3, 1);
v = zeros(3, 1);
q= [1 0 0 0]';

for i=1:N
    [p ,v , q] = ch_nav_equ_local_tan(p, v, q, acc(:,i), gyr(:,i), 1 / Fs, [0, 0, -9.8]');
    h_pos(i,:) = p;
    h_eul(i,:) = ch_q2eul(q);
end

figure;
subplot(2,2,1);
ch_plot_pos3d(h_pos);
subplot(2,2,2);
ch_plot_pos2d(h_pos);
subplot(2,2,3);
ch_plot_att(h_eul);

fprintf("共%d数据，用时:%.3fs\n", N, N/Fs);
fprintf("起始位置:%.3f %.3f, 终点位置%.3f %.3f, 相差:%.3fm\n", h_pos(1,1), h_pos(1,2), h_pos(N,1), h_pos(N,2), norm(h_pos(N,:) - h_pos(1,:)));


%% Copyright (C) 2016 Lagu
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy 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 software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym dawson (@var{x})
%% Symbolic Dawson (scaled imaginary error) function.
%%
%% Example:
%% @example
%% @group
%% syms x
%% dawson (x)
%%   @result{} ans = (sym)
%%             2
%%           -x
%%       √π⋅ℯ   ⋅erfi(x)
%%       ───────────────
%%              2
%% @end group
%% @end example
%% @seealso{dawson, @@sym/erfc, @@sym/erf, @@sym/erfcx, @@sym/erfi, @@sym/erfinv, @@sym/erfcinv}
%% @end defmethod

function y = dawson(x)
  if (nargin ~= 1)
    print_usage ();
  end
  y = elementwise_op ('lambda a: exp(-a**2)*erfi(a)*(sqrt(S(pi))/2)', x);
end

%!test
%! if (exist ('OCTAVE_VERSION', 'builtin'))
%! % dawson missing on Matlab, Issue #742
%! A = dawson([1 2]);
%! B = double(dawson(sym([1 2])));
%! assert(A, B, -eps)
%! end


function phiFaceAverage = tvdMean2D(phi, u, FL)
% This function gets the value of the field variable phi defined
% over the MeshStructure and calculates the TVD average on
% the cell faces, based on the direction of the velocity vector for a uniform mesh.
%
% SYNOPSIS:
%
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% Written by Ali A. Eftekhari
% See the license file

% extract the velocity data
% note: size(ux) = [1:m+1, 1:n] and size(uy) = [1:m, 1:n+1]
ux = u.xvalue;
uy = u.yvalue;

% check the size of the variable and the mesh dimension
Nx = u.domain.dims(1);
Ny = u.domain.dims(2);
dx=repmat(0.5*(u.domain.cellsize.x(1:end-1)+u.domain.cellsize.x(2:end)), 1, Ny);
dy=repmat(0.5*(u.domain.cellsize.y(1:end-1)+u.domain.cellsize.y(2:end))', Nx, 1);

%
phiX_p = zeros(Nx+1,Ny);
phiX_m = zeros(Nx+1,Ny);
phiY_p = zeros(Nx,Ny+1);
phiY_m = zeros(Nx,Ny+1);

% calculate the upstream to downstream gradient ratios for u>0 (+ ratio)
% x direction
dphiX_p = (phi.value(2:Nx+2, 2:Ny+1)-phi.value(1:Nx+1, 2:Ny+1))./dx;
rX_p = dphiX_p(1:end-1,:)./fsign(dphiX_p(2:end,:));
phiX_p(2:Nx+1,:) = phi.value(2:Nx+1, 2:Ny+1)+0.5*FL(rX_p).* ...
    (phi.value(3:Nx+2,2:Ny+1)-phi.value(2:Nx+1, 2:Ny+1));
phiX_p(1, :) = (phi.value(1, 2:Ny+1)+phi.value(2, 2:Ny+1))/2; % left boundary
% y direction
dphiY_p = (phi.value(2:Nx+1, 2:Ny+2)-phi.value(2:Nx+1, 1:Ny+1))./dy;
rY_p = dphiY_p(:,1:end-1)./fsign(dphiY_p(:,2:end));
phiY_p(:,2:Ny+1) = phi.value(2:Nx+1, 2:Ny+1)+0.5*FL(rY_p).* ...
    (phi.value(2:Nx+1,3:Ny+2)-phi.value(2:Nx+1, 2:Ny+1));
phiY_p(:,1) = (phi.value(2:Nx+1,1)+phi.value(2:Nx+1,2))/2; % Bottom boundary

% calculate the upstream to downstream gradient ratios for u<0 (- ratio)
% x direction
rX_m = dphiX_p(2:end,:)./fsign(dphiX_p(1:end-1,:));
phiX_m(1:Nx,:) = phi.value(2:Nx+1, 2:Ny+1)+0.5*FL(rX_m).* ...
    (phi.value(1:Nx, 2:Ny+1)-phi.value(2:Nx+1, 2:Ny+1));
phiX_m(Nx+1,:) = (phi.value(end, 2:Ny+1)+phi.value(end-1, 2:Ny+1))/2; % right boundary
% y direction
rY_m = dphiY_p(:,2:end)./fsign(dphiY_p(:,1:end-1));
phiY_m(:,1:Ny) = phi.value(2:Nx+1, 2:Ny+1)+0.5*FL(rY_m).* ...
    (phi.value(2:Nx+1, 1:Ny)-phi.value(2:Nx+1, 2:Ny+1));
phiY_m(:, Ny+1) = (phi.value(2:Nx+1, end)+phi.value(2:Nx+1, end-1))/2; % top boundary

% calculate the average value
xvalue = (ux>0).*phiX_p+ ...
                        (ux<0).*phiX_m+ ...
                        0.5*(ux==0).*(phi.value(1:Nx+1,2:Ny+1)+phi.value(2:Nx+2,2:Ny+1));
yvalue = (uy>0).*phiY_p+ ...
                        (uy<0).*phiY_m+ ...
                        0.5*(uy==0).*(phi.value(2:Nx+1,1:Ny+1)+phi.value(2:Nx+1,2:Ny+2));
phiFaceAverage=FaceVariable(phi.domain, xvalue, yvalue, []);
end

function phi_out = fsign(phi_in)
% This function checks the value of phi_in and assigns an eps value to the
% elements that are less than or equal to zero, while keeping the signs of
% the nonzero elements
    phi_out = (abs(phi_in)>=eps).*phi_in+eps*(phi_in==0)+eps*(abs(phi_in)<eps).*sign(phi_in);
end


function y = det_inv( varargin )

%DET_INV determinant of the inverse of an SPD matrix.
%   For a square matrix X, DET_INV(X) returns 1.0./DET(X) if X is symmetric
%   (real) or Hermitian (complex) and positive defininte, and +Inf otherwise.
%
%   This function can be used in many convex optimization problems that call
%   for LOG(DET(X)) instead. For example, if the objective function is
%      maximize(logdet(X))
%   then it can be replaced with
%      maximize(-det_inv(X))
%   and the same optimal point will be produced.
%
%   DET_INV(X,p) computes DET_INV(X)^p. p must be a positive real scalar.
%
%   Disciplined convex programming information:
%       DET_INV(X) is convex and nonmonotonic in X; therefore, when used in
%       CVX specifications, its argument must be affine.

persistent P
if isempty( P ),
    P.nargs     = 2;
    P.args      = @det_inv_args;
    P.empty     = 1;
    P.constant  = @det_inv_diag;
    P.diag      = @det_inv_diag;
    P.affine    = @det_inv_aff;
    P.structure = 'psdeig';
end
y = cvx_matrix_op( P, varargin );

function [ X, p ] = det_inv_args( X, p )
if isempty( p ),
    p = 1;
elseif ~( isnumeric(p) && isreal(p) && numel(p)==1 && p>=0 ),
    cvx_throw( 'Second argument must be a positive scalar.' );
end

function y = det_inv_diag( D, p )
y = prod_inv( D, p );

function y = det_inv_aff( X, p )
cvx_begin sdp
    epigraph variable z nonnegative_
    variable Y(n,n) lower_triangular complex_if(X)
    prod_inv( real(diag(Y)), 2*p ) <= z;
    [ diag( D ), Y' ; Y, X ] >= 0;
cvx_end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


% Test file for chebtech/clenshaw.m

function pass = test_clenshaw(varargin)

% Set a tolerance (pref.chebfuneps doesn't matter)
tol = 10*eps;

%%
% Test that a single coefficient is evaluated correctly:
% For a scalar evaluation:
c = sqrt(2);
v = chebtech.clenshaw(0, c);
pass(1) = c == v;

% For a vector evaluation:
x = [-.5 ; 1];
v = chebtech.clenshaw(x, c);
pass(2) = ( all(size(v) == [2, 1]) && all(c == v) );

% For a row vector evaluation with column coefficients:
c = [c, c, c];
v = chebtech.clenshaw(x, c);
pass(3) = ( all(size(v) == [2, 3]) && norm(repmat(c, 2, 1) - v) == 0);

%%
% Test that a vector coefficient is evaluated correctly:
% Some simple data :
c = (5:-1:1).';
x = [-.5 ; -.1 ; 1];

% Scalar coefficient
v = chebtech.clenshaw(x, c);
% Exact values:
vTrue = [3 ; 3.1728 ; 15];
pass(4) = norm(v - vTrue, inf) < tol;

% In vectorised form:
vTrue2 = [0 ; 3.6480 ; 15];
v = chebtech.clenshaw(x, [c, c(end:-1:1)]);
pass(5) = norm(v - [vTrue, vTrue2], inf) < tol;

end


function pass = test_fevalt(pref)
% Test fevalt

if ( nargin == 0 )
    pref = chebfunpref;
end
tol = 1e4*pref.cheb3Prefs.chebfun3eps;

ff = @(x,y,z) cos(x) + sin(x.*y) + sin(z.*x);
f = chebfun3(ff);
seedRNG(42);
x = rand(10,1);
y = rand(10,1);
z = rand(10,1);
F = fevalt(f,x,y,z);

[xx, yy, zz] = ndgrid(x,y,z);
Fexact = ff(xx,yy,zz);
pass(1) = norm(Fexact(:) - F(:)) < tol*vscale(f);

dom = [-1 1 -pi/3 pi/3 -2 0];
f = chebfun3(ff, dom);
x = chebpts(20,dom(1:2));
y = chebpts(20,dom(3:4));
z = chebpts(20,dom(5:6));
F = fevalt(f,x,y,z);

[xx, yy, zz] = ndgrid(x,y,z);
Fexact = ff(xx,yy,zz);
pass(2) = norm(Fexact(:) - F(:)) < tol*vscale(f);

% 'trig' flag + vector inputs
ff = @(x,y,z) sin(pi*(x+y+z));
f = chebfun3(ff, 'trig');
x = linspace(-1, 1, 20)';
y = linspace(-1, 1, 20)';
z = linspace(-1, 1, 20)';
F = fevalt(f,x,y,z);

[xx,yy,zz] = ndgrid(x,y,z);
Fexact = ff(xx,yy,zz);
pass(3) = norm(F(:) - Fexact(:)) < tol*vscale(f);

end


function [e,cnt] = my_normest(S,St,n,tol, maxiter)
% [norm,iter_count] = my_normest( A, At, n, [tol], [maxiter] )
%   estimates the spectral norm of A using the power method,
%   where A is a function handle to compute A(x) = A*x
%   and At is a function handle to compute At(x) = A'*x
%
% Copied from MATLAB's "normest" function, but allows function handles, not just sparse matrices
    if nargin < 4, tol = 1.e-6; end
    if nargin < 5, maxiter = 20; end
    if isempty(St)
        St = S;  % we assume the matrix is symmetric;
    end
    x = ones(n,1);
    cnt = 0;
    e = norm(x);
    if e == 0, return, end
    x = x/e;
    e0 = 0;
    while abs(e-e0) > tol*e && cnt < maxiter
       e0 = e;
       Sx = S(x);
       if nnz(Sx) == 0
          Sx = rand(size(Sx));
       end
       e = norm(Sx);
       x = St(Sx);
       x = x/norm(x);
       cnt = cnt+1;
    end
end


function tifd=friedman(tfr,hat,t,method,trace);
%FRIEDMAN Instantaneous frequency density.
%	TIFD = FRIEDMAN(TFR,HAT,T,METHOD,TRACE) computes the
%	time-instantaneous frequency density (defined by Friedman [1])
%	of a reassigned time-frequency representation.
%
%	TFR   : time-frequency representation, (N,M) matrix.
%	HAT   : complex matrix of the reassignment vectors.
%	T     : the time instant(s)	(default : (1:M)).
%	METHOD: chosen representation	(default : 'tfrrsp').
%	TRACE : if nonzero, the progression of the algorithm is shown
%					(default : 0).
%	TIFD  : time instantaneous-frequency density. When called without
%	        output arguments, FRIEDMAN runs TFRQVIEW.
%
%	WARNING : TIFD is not an energy distribution, but an estimated
%	-------        probability distribution !
%
%	Example :
%	 sig=fmlin(128,0.1,0.4); h=tftb_window(47,'Kaiser');
%	 t=1:2:127; [tfr,rtfr,hat]=tfrrpwv(sig,t,128,h);
%	 friedman(tfr,hat,t,'tfrrpwv',1);
%
%	See also : RIDGES.

%	F. Auger, August 1994, Decembre 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  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 should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
%
%	[1] : D. H. Friedman, "Instantaneous Frequency vs Time : An
%	      Interpretation of the Phase Structure of Speech", Proc. IEEE
%	      ICASSP, pp. 29.10.1-4, Tampa, 1985.

if (nargin<3),
 error('At least 2 parameters required');
end;

[tfrrow,tfrcol]=size(tfr);
[hatrow,hatcol]=size(hat);

if (nargin==2),
 t=1:tfrcol; method='tfrrsp'; trace=0;
elseif (nargin==3),
 method='tfrrsp'; trace=0;
elseif (nargin==4),
 trace=0;
end;

[trow,tcol] = size(t);
if (trow~=1),
 error('T must only have one row');
elseif (tfrrow~=hatrow)|(tfrcol~=hatcol),
 error('tfr and hat must have the same size');
end;

tifd=zeros(tfrrow,tfrcol);
bins=0.5+(0:tfrrow-1);
threshold=sum(sum(tfr))*0.5/(tfrrow*tfrcol);

if trace, fprintf ('\nFriedman distribution: \n'); end;

for j=1:tfrcol,
 if trace, disprog(j,tfrcol,10); end;
 indices=find(tfr(:,j)>threshold);
 if (length(indices)>=1),
  [occurences,trash]=hist(real(hat(indices,j)),bins);
  tifd(:,j)=occurences';
 end;
end;
tifd=tifd/sum(sum(tifd));

method=upper(method);
if nargout==0,
 tfrqview(tifd,[],t,method);
end


function stroud_test30 ( )

%*****************************************************************************80
%
%% TEST30 tests SPHERE_UNIT_**_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_3D_INDEX;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST30\n' );
  fprintf ( 1, '  For integrals on the unit sphere in 3D:\n' );
  fprintf ( 1, '  SPHERE_UNIT_07_3D uses a formula of degree 7.\n' );
  fprintf ( 1, '  SPHERE_UNIT_11_3D uses a formula of degree 11.\n' );
  fprintf ( 1, '  SPHERE_UNIT_14_3D uses a formula of degree 14.\n' );
  fprintf ( 1, '  SPHERE_UNIT_15_3D uses a formula of degree 15.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Unit sphere area = %f\n', sphere_unit_area_nd ( 3 ) );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '    F(X)    S3S07        S3S11         S3S14         S3S15\n' );
  fprintf ( 1, '\n' );

  num = function_3d_num ( );

  for i = 1 : num

    FUNC_3D_INDEX = i;

    result1 = sphere_unit_07_3d ( 'function_3d' );
    result2 = sphere_unit_11_3d ( 'function_3d' );
    result3 = sphere_unit_14_3d ( 'function_3d' );
    result4 = sphere_unit_15_3d ( 'function_3d' );

    fname = function_3d_name ( i );

    fprintf ( 1, '  %s  %12f  %12f  %12f  %12f\n', ...
      fname, result1, result2, result3, result4 );

  end

  return
end


%% Unit Cube
[node,elem] = cubemesh([-1,1,-1,1,-1,1],1);
subplot(1,2,1); showmesh3(node,elem);

%% Lshape domain
% mesh
[node,elem] = cubemesh([-1,1,-1,1,-1,1],1);
[node,elem] = delmesh(node,elem,'x<0 & y<0');
bdFlag = setboundary3(node,elem,'Dirichlet');
% for adaptive mesh refinement, special ordering of elem and HB is needed
[elem,bdFlag,HB] = label3(node,elem,'all',bdFlag);
showmesh3(node,elem);


function [cmap] = gat(nColors, cZero, useWhite)
if nargin < 1
    nColors = 255;
end
if nargin < 2
    cZero = 1;
end
if nargin < 3
    useWhite  = 1;
end

clim = caxis();
if (clim(1)>0 || clim(2)<0)
    cZero = 0;
end
if cZero
    center = round(clim(2)*nColors/(clim(2)-clim(1)));
    N = 2*center;
else
    N = nColors;
    center = round(nColors/2);
end

if nColors == 0
    cmap = [];
    return
end

try
    L = ones(nColors,1)*100;
    a = ones(nColors,1)*0;
    b = ones(nColors,1)*0;

    % RED     100  128  128
    % VIOLET  100  128 -128
    % GREEN     x -128  128
    % BLUE      x -128 -128

    % BLACK => RED => GREEN =>  BLUE => VIOLET

    % BLACK to RED
    nVal = floor(N*5/32);
    Lab1 = [20 20 10];
    Lab2 = [40 100 128];
    step = 1/(nVal-1);
    L(1:nVal) = interp1([0 1]', [Lab1(1) Lab2(1)], [0:step:1]','linear');
    a(1:nVal) = interp1([0 1]', [Lab1(2) Lab2(2)], [0:step:1]','linear');
    b(1:nVal) = interp1([0 1]', [Lab1(3) Lab2(3)], [0:step:1]','linear');
    s = nVal;

    % RED to YELLOW
    nVal = floor(N*6/32);
    Lab1 = [40 100 120];
    Lab2 = [100 -20 128];
    step = 1/(nVal);
    L(s:s+nVal) = interp1([0 1]', [Lab1(1) Lab2(1)], [0:step:1]','pchip');
    a(s:s+nVal) = interp1([0 1]', [Lab1(2) Lab2(2)], [0:step:1]','linear');
    b(s:s+nVal) = interp1([0 1]', [Lab1(3) Lab2(3)], [0:step:1]','linear');
    s = s+nVal;

    % YELLOW to GREEN
    nVal = floor(N*3/32);
    Lab1 = [100 0 128];
    Lab2 = [75 -80 80];
    step = 1/(nVal);
    L(s:s+nVal) = interp1([0 1]', [Lab1(1) Lab2(1)], [0:step:1]','pchip');
    a(s:s+nVal) = interp1([0 1]', [Lab1(2) Lab2(2)], [0:step:1]','linear');
    b(s:s+nVal) = interp1([0 1]', [Lab1(3) Lab2(3)], [0:step:1]','linear');
    s = s+nVal;

    % GREEN to useWhite
    if (useWhite)
        nVal = floor(N*2/32);
        Lab1 = [75 -80 80];
        Lab2 = [100 0 0];
        step = 1/(center-s);
        L(s:center) = interp1([0 1]', [Lab1(1) Lab2(1)], [0:step:1]','linear');
        a(s:center) = interp1([0 1]', [Lab1(2) Lab2(2)], [0:step:1]','linear');
        b(s:center) = interp1([0 1]', [Lab1(3) Lab2(3)], [0:step:1]','linear');
        s = center;
    end

    % HALF BAR ------- ------- ------- ------- ------- ------- ------- -------

    % GREEN to BLUE
    if (~useWhite)
        nVal = floor(N*3/32);
        Lab1 = [75 -80 80];
        Lab2 = [100 -80  0];
        step = 1/(nVal);
        L(s:s+nVal) = interp1([0 1]', [Lab1(1) Lab2(1)], [0:step:1]','linear');
        a(s:s+nVal) = interp1([0 1]', [Lab1(2) Lab2(2)], [0:step:1]','linear');
        b(s:s+nVal) = interp1([0 1]', [Lab1(3) Lab2(3)], [0:step:1]','linear');
        s = s+nVal;
    end

    if cZero
        N = 2*(nColors-center);
    end

    if N > 0
        if (~useWhite)
            nVal = floor(N*2/32);
            Lab1 = [100 -80 0];
            Lab2 = [80 40 -128];
            step = 1/(nVal);
            L(s:s+nVal) = interp1([0 1]', [Lab1(1) Lab2(1)], [0:step:1]','linear');
            a(s:s+nVal) = interp1([0 1]', [Lab1(2) Lab2(2)], [0:step:1]','linear');
            b(s:s+nVal) = interp1([0 1]', [Lab1(3) Lab2(3)], [0:step:1]','linear');
            s = s+nVal;
        end
        % HALF BAR ------- ------- ------- ------- ------- ------- ------- -------

        if (useWhite)
            % useWhite to BLUE
            nVal = floor(N*2/32);
            Lab1 = [100 0 0];
            Lab2 = [80 40 -128];
            step = 1/(nVal);
            L(s:s+nVal) = interp1([0 1]', [Lab1(1) Lab2(1)], [0:step:1]','linear');
            a(s:s+nVal) = interp1([0 1]', [Lab1(2) Lab2(2)], [0:step:1]','linear');
            b(s:s+nVal) = interp1([0 1]', [Lab1(3) Lab2(3)], [0:step:1]','linear');
            s = s+nVal;
        end

        % BLUE to PINK
        nVal = floor(N*3/32);
        Lab1 = [80 40 -128];
        Lab2 = [50 -80 -80];
        step = 1/(nVal);
        L(s:s+nVal) = interp1([0 1]', [Lab1(1) Lab2(1)], [0:step:1]','linear');
        a(s:s+nVal) = interp1([0 1]', [Lab1(2) Lab2(2)], [0:step:1]','linear');
        b(s:s+nVal) = interp1([0 1]', [Lab1(3) Lab2(3)], [0:step:1]','linear');
        s = s+nVal;

        % BLUE to VIOLET
        nVal = floor(N*4/32);
        Lab1 = [50 -80 -80];
        Lab2 = [0 100 -128];
        step = 1/(nVal);
        L(s:s+nVal) = interp1([0 1]', [Lab1(1) Lab2(1)], [0:step:1]','linear');
        a(s:s+nVal) = interp1([0 1]', [Lab1(2) Lab2(2)], [0:step:1]','linear');
        b(s:s+nVal) = interp1([0 1]', [Lab1(3) Lab2(3)], [0:step:1]','linear');
        s = s+nVal;

        % BLUE to VIOLET
        nVal = floor(N*4/32);
        Lab1 = [0  100 -128];
        Lab2 = [40 128 -128];
        step = 1/(nVal);
        L(s:s+nVal) = interp1([0 1]', [Lab1(1) Lab2(1)], [0:step:1]','linear');
        a(s:s+nVal) = interp1([0 1]', [Lab1(2) Lab2(2)], [0:step:1]','linear');
        b(s:s+nVal) = interp1([0 1]', [Lab1(3) Lab2(3)], [0:step:1]','linear');
        s = s+nVal;

        % BLUE to VIOLET
        nVal = floor(N*4/32);
        Lab1 = [40 128 -128];
        Lab2 = [25 10 -20];
        step = 1/(nColors-s);
        L(s:end) = interp1([0 1]', [Lab1(1) Lab2(1)], [0:step:1]','linear');
        a(s:end) = interp1([0 1]', [Lab1(2) Lab2(2)], [0:step:1]','linear');
        b(s:end) = interp1([0 1]', [Lab1(3) Lab2(3)], [0:step:1]','linear');
    end

    %L = [[0:100/(nColors/2-1):100]' ; flipud([0:100/(nColors/2-1):100]')];
    %L = flipud([0:100/(nColors-1):100]');
    cmap = flipud(Lab2RGB(L, a, b )./255);
catch
    cmap = jet(1024);

end
end

function [R, G, B] = Lab2RGB(L, a, b)

var_Y = ( L + 16 ) ./ 116;
var_X = a ./ 500 + var_Y;
var_Z = var_Y - b ./ 200;

pos = (var_Y.^3 > 0.008856);
var_Y(pos) = var_Y(pos).^3;
var_Y(~pos) = (var_Y(~pos) - 16 / 116 ) ./ 7.787;

pos = (var_X.^3 > 0.008856);
var_X(pos) = var_X(pos).^3;
var_X(~pos) = (var_X(~pos) - 16 / 116 ) ./ 7.787;

pos = (var_Z.^3 > 0.008856);
var_Z(pos) = var_Z(pos).^3;
var_Z(~pos) = (var_Z(~pos) - 16 / 116 ) ./ 7.787;

% Observer= 2?, Illuminant= D65
ref_X =  95.047;
ref_Y = 100.000;
ref_Z = 108.883;

X = ref_X .* var_X;
Y = ref_Y .* var_Y;
Z = ref_Z .* var_Z;

var_X = X ./ 100;        % X from 0 to  95.047      (Observer = 2?, Illuminant = D65)
var_Y = Y ./ 100;        % Y from 0 to 100.000
var_Z = Z ./ 100;        % Z from 0 to 108.883

var_R = var_X .*  3.2406 + var_Y .* -1.5372 + var_Z .* -0.4986;
var_G = var_X .* -0.9689 + var_Y .*  1.8758 + var_Z .*  0.0415;
var_B = var_X .*  0.0557 + var_Y .* -0.2040 + var_Z .*  1.0570;

pos = ( var_R > 0.0031308 );
var_R(pos) = 1.055 .* ( var_R(pos) .^ ( 1 / 2.4 ) ) - 0.055;
var_R(~pos) = 12.92 .* var_R(~pos);

pos = ( var_G > 0.0031308 );
var_G(pos) = 1.055 .* ( var_G(pos) .^ ( 1 / 2.4 ) ) - 0.055;
var_G(~pos) = 12.92 .* var_G(~pos);

pos = ( var_B > 0.0031308 );
var_B(pos) = 1.055 .* ( var_B(pos) .^ ( 1 / 2.4 ) ) - 0.055;
var_B(~pos) = 12.92 .* var_B(~pos);

R = max(0,min(var_R .* 255,255));
G = max(0,min(var_G .* 255,255));
B = max(0,min(var_B .* 255,255));

if ((nargout == 1) || (nargout == 0))
    R = [R,G,B];
end

end


function I = mi_mixture_gd_vec(x, y, Ym)
% MI_MIXTURE_GD_VEC Vectorized MI calculation between multiple Gaussian variables
%         and a common discrete variable in bits from Gaussian mixture.
%   I = mi_mixture_gd_vec(x,y,Ym) returns the MI between the (possibly multidimensional)
%   Gaussian variables x and the discrete variable y.
%   size(x) = [Ntrl Nvec Ndim]
%   so each output I(i) = mi_mixture_gd(squeeze(x(:,i,:)), y, Ym);
%
%   y should contain integer values in the range [0 Ym-1] (inclusive).
%
%   biascorrect : true / false option (default true) which specifies whether
%   bias correction should be applied to the esimtated MI.
%   demeaned : false / true option (default false) which specifies whether the
%   input data already has zero mean (true if it has been copula-normalized)
%   See also: MI_MIXTURE_GD, MI_MODEL_GD_VEC

% ensure samples first axis for vectors
if isvector(x)
    x = x(:);
end
if ndims(x)>3
    error('mi_model_gd: input arrays should be 3d')
end
if isvector(y)
    y = y(:);
else
    error('mi_model_gd: only univariate discrete variable supported');
end

Ntrl = size(x,1);
Nvec = size(x,2);
Xdim = size(x,3);

if size(y,1) ~= Ntrl
    error('mi_model_gd: number of trials do not match');
end

I = zeros(Nvec,1);

% y = y-1;
% for vi=1:Nvec
%   I(vi) = mi_model_gd(squeeze(x(:,vi,:)),y,Ym,true,true);
% end
%
% return

% one-hot encoding of Y
Yhot = indexed2boolean(y);

% remove class means
[Xcen class_means] = removeclassmeans(x, Yhot);

% allocate memory for class-conditional entropies and covariances
Ntrl_y = sum(Yhot);
Hcond  = zeros(Nvec,Ym);
Cm     = zeros(Nvec,Xdim,Xdim,Ym);

for vi1=1:Xdim
  % all voxels for this dimension
  x1 = Xcen(:,:,vi1);
  for yi=1:Ym
    tmp = x1(Yhot(:,yi),:);
    Cm(:,vi1,vi1,yi) = sum(tmp.^2);
  end

  for vi2=(vi1+1):Xdim
    x2  = Xcen(:,:,vi2);
    for yi=1:Ym
      tmp = transpose(sum(x1(Yhot(:,yi),:).*x2(Yhot(:,yi),:)));
      Cm(:,vi1,vi2,yi) = tmp;
      Cm(:,vi2,vi1,yi) = tmp;
    end
  end
end

for yi=1:Ym
  Cm(:,:,:,yi) = Cm(:,:,:,yi) / (Ntrl_y(yi) - 1);
  Cm(:,:,:,yi) = vecchol(Cm(:,:,:,yi));
  for vi=1:Xdim
    Hcond(:,yi) = Hcond(:,yi)+log(Cm(:,vi,vi,yi));
  end
end
% class weights
w = Ntrl_y ./ Ntrl;
% normalise entropy properly
Hcond = Hcond + 0.5*Xdim*(log(2*pi)+1);

% mixture entropy via unscented transform
% See:
% Huber, Bailey, Durrant-Whyte and Hanebeck
% "On entropy approximation for Gaussian mixture random vectors"
% http://dx.doi.org/10.1109/MFI.2008.4648062
%
% Goldberger, Gordon, Greenspan
% "An efficient image similarity measure based on approximations of
% KL-divergence between two Gaussian mixtures"
% http://dx.doi.org/10.1109/ICCV.2003.1238387

D = Xdim;
Ds = sqrt(Xdim);
Hmix = zeros(Nvec,1);
chC = Cm;

m = reshape(class_means,[Nvec Xdim Ym]);
for yi=1:Ym
    % vector transpose
    Ps = Ds * permute(chC(:,:,:,yi), [1 3 2]);
    % unscented points for this class
    usc = cat(3,bsxfun(@plus,Ps,m(:,:,yi)), bsxfun(@minus,m(:,:,yi),Ps));

    % class log-likelihoods at unscented points
    log_lik = zeros(Ym,Nvec,2*Xdim);
    for mi=1:Ym
        % demean points
        dx = bsxfun(@minus,usc,m(:,:,mi));
        % gaussian likelihood
        log_lik(mi,:,:) = bsxfun(@minus,norm_innerv(dx, chC(:,:,:,mi)), Hcond(:,mi)) + 0.5*Xdim;
    end
    % log mixture likelihood for these unscented points
    logmixlik = maxstar(log_lik, w);
    % add to entropy estimate
    Hmix = Hmix + w(yi)*sum(logmixlik,2);
end
Hmix = -Hmix/(2*D);

% compute mutual information
I = Hmix - Hcond*w';

% convert to bits
I = I / log(2);

function [Xcen, class_means] = removeclassmeans(X, design)
[Ntrl, Nvec, Ndim] = size(X);
Xcen = X(:,:);
class_means = zeros(Nvec*Ndim,size(design,2));
for k = 1:size(design,2)
  sel = design(:,k);
  tmp = Xcen(sel,:);
  class_means(:,k) = mean(tmp,1);
  Xcen(sel,:) = bsxfun(@minus,tmp,class_means(:,k).');
end
Xcen = reshape(Xcen,[Ntrl Nvec Ndim]);

function Y = indexed2boolean(X)
uX = unique(X);
Y  = false(numel(X),numel(uX));
for k = 1:size(Y,2)
  Y(X==uX(k),k) = true;
end

function w = norm_innerv(x, chC)
% normalised innervations
Nvec = size(chC,1);
w = zeros(Nvec,size(x,3));
for vi=1:Nvec
  m = (squeeze(chC(vi,:,:))')\(squeeze(x(vi,:,:)));
  w(vi,:) = -0.5 *sum(m.*m,1);
end


function r = showrateh(h,err,k,opt,str)
%% SHOWRATEH rate of an err sequence
%
%  r = SHOWRATEH(N,err) finds the number r such that err = N^r and plots the
%  err vs N in loglog scale.
%
%  r = SHOWRATEH(N,err,k) finds the number r such that err(k:end)=N(k:end)^r.
%
%  The function accepts standard plotting properting. For example, r =
%  showrate(N,err,[],'r') will plot the error curve in red.
%
% See also showrate2, showresult, showmesh, showsolution
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

N = 1./h;
if (nargin<=2)
    k = 1; opt = '-*';
end
r = showrate(N,err,k,opt);
h_legend = legend(str,['C_1h^{' num2str(-r) '}'],'LOCATION','Best');
set(h_legend,'FontSize',12);
xlabel('log(1/h)');


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% Tutorial for FAIR: Regularized Parametric Image Registration,  by spline-moment matrix
%
%   - data                 HNSP, Omega=(0,2)x(0,1), level=5, m=[256,128]
%   - viewer               viewImage2D
%   - interpolation        splineInter
%   - distance             SSD
%   - pre-registration     splineTransformation2D, regularized by spline-moment matrix
%   - optimization         Gauss-Newton
% ===============================================================================

clear, close all, help(mfilename);
%%
setup2DHNSPData;  level = 5;  omega = ML{level}.omega; m = ML{level}.m;
p = [8,8];
imgModel('reset','imgModel','splineInter','regularizer','none','theta',0);
[T,R] = imgModel('coefficients',ML{level}.T,ML{level}.R,omega);
distance('reset','distance','SSD');
trafo('reset','trafo','splineTransformation2D','omega',omega,'m',m,'p',p);
w0 = trafo('w0');                        % get starting guess and stopping

% update the regularizer for parametric image registration
hd = prod((omega(2:2:end)-omega(1:2:end))./m);
Mi = @(i) 1e2*toeplitz([96,-54,0,6,zeros(1,p(i)-4)]);
Qi = @(i) toeplitz([120.8,59.55,6,0.05,zeros(1,p(i)-4)])/7;
M1 = kron(speye(p(2)),sparse(Mi(1)));
M2 = kron(sparse(Mi(2)),speye(p(1)));
M  = hd*sparse(kron(speye(2),...
  kron(Qi(2),Mi(1))+2*kron(Mi(2),Mi(1))+kron(Mi(2),Qi(1))));

%% set up elastic regularization matrix
mu     = 2;
lambda = 1;
B = getElasticMatrixNodal(omega,m,mu,lambda);
A = B'*B;
[y,dy] = splineTransformation2D(w0,getNodalGrid(omega,m),'m',m+1);
M = [dy'*A*dy];

%%
% M = speye(length(w0));
alpha = 10;
beta  = 0;
wRef  = w0;

% initialize objective function
xc = getCellCenteredGrid(omega,m);
Rc = imgModel(R,omega,xc);
fctn  = @(wc) PIRobjFctn(T,Rc,omega,m,beta,alpha*M,wRef,xc,wc);
fctn([]);

% initialize plots
FAIRplots('reset','mode','PIR-regularized','omega',omega,'m',m,'fig',1,'plots',1);
FAIRplots('init',struct('Tc',T,'Rc',R,'omega',omega,'m',m));

xc = getCellCenteredGrid(omega,m);
Rc = imgModel(R,omega,xc);

% ----- call Gaus-Newton ------------------------------------
GNoptn = {'maxIter',100,'tolY',1e-4,'tolJ',1e-4,'tolG',1,'solver','backslash'};
[wOpt,his] = GaussNewton(fctn,w0,GNoptn{:},'Plots',@FAIRplots);

% plot iteration history
his.str{1} = sprintf('iteration history PIR: distance=%s, y=%s',distance,trafo);
[ph,th] = plotIterationHistory(his,'J',1:4);
%==============================================================================


% system process model in ecef frame with dcm formulation, Cse is the
% transformation between the sensor and e-frame, with the assumption that
% the sensor is also the body frame, we have Cse=Cbe.
%implement this function as sys_llh_phipsi_v000
function [STM Qd]=sys_ecef_dcm_v001(xyz_imu, Cse, acc, gyro, dt, imutype, modelNo)
% for modelNo=5 and imutype=5
% position   (1-3)
% velocity   (4-6)
% attitude   (7-9)
% acc bias drift
% gyro bias drift
% acc scale factor
% gyro scale factor
% acc turn on bias
% gyro turn on bias
% for modelNo=1 and imutype=5 the acc and gyro turn on bias are removed
WIE_E=7292115e-11;  %Earth rotation rate
g = 9.7803267714; % nominal g on earth surface
R=6317000; % earth average radius
%system disturbance coefs
N=zeros(9,6);
N(7:9,4:6)=-Cse; %attitude
N(4:6,1:3)=Cse; %velocity

%system matrix
A=zeros(9);
acc_e=Cse*acc;

%Position
A(1,4)=1;
A(2,5)=1;
A(3,6)=1;

%Velocity
unit=xyz_imu/norm(xyz_imu,2);
A(4:6,1:3)=-g*R^2/norm(xyz_imu,2)^3*(eye(3)-3*(unit*unit'))-skew([0;0;WIE_E])*skew([0;0;WIE_E]);
A(4:6,4:6)=-2*skew([0;0;WIE_E]);
A(4:6,7:9)=skew(acc_e);

%Attitude
A(7:9,7:9)=-skew([0;0;WIE_E]);
Anav=A;
Nnav=N;
% X(k+1) = ffun[X(k),U(k),V(k)]
% X(k+1) = Ak*X(k)+Gk*Vk

%%%%Imu error model parameters
[Aimu_d, Qimu_d, Cimu, Rimu]=imu_err_model_v001(acc, gyro, dt, imutype, modelNo);

%%%%Combine and discretize nav and imu models
% this discretization can also be accomplished by Loan's matrix exponential
% method, see sys_metric_phipsi_v000.m
Anav_d=eye(9)+dt*Anav;  %Use 1st order taylor series to discretize Anav
Qnav=Nnav*Rimu*Nnav';
Qnav_d=dt/2*(Anav_d*Qnav+Qnav*Anav_d');      %Use trapezoidal rule to discretize Rimu

STM=zeros(9+size(Aimu_d,1));
STM(1:9,1:9)=Anav_d;
STM(1:9,10:end)=Nnav*Cimu*dt;
STM(10:end,10:end)=Aimu_d;

Qd=zeros(9+size(Aimu_d,1));
Qd(1:9,1:9)=Qnav_d;
Qd(10:end,10:end)=Qimu_d;
Qd(1:9,10:end)=Nnav*Cimu*Qimu_d*dt/2;
Qd(10:end,1:9)=Qd(1:9,10:end)';
end


function visualizeDiff(inputA, inputB, diff, hueA, hueB)
% images are inputA and inputB in rgb format or grayscale. Both has
% the same size with values in range [0-255].
% diff image contains any real values, zero means zero difference

numOfPixels = size(inputA,1)*size(inputA,2);

if (size(inputA,3) ~= 1) % image is in RGB format
    % convert it into grayscale
    inputA = rgb2gray(inputA);
end

if (size(inputB,3) ~= 1) % input B is in RGB format
    % convert it into grayscale
    inputB = rgb2gray(inputB)
end

% scale input values from range 0/255 into [0,1]
inputA = double(inputA)/255;
inputB = double(inputB)/255;

% if no output color is defined, use default
if (nargin < 4)
    hueA = pi/3;
    hueB = 4*pi/3;
end

% if difference is not in double values - convert it also
diff = double(diff);
% normalize values of difference into range [-pi/4, pi/4]
diff = diff/max(max(abs(diff)))*pi/4

% here magic begins
%  - Lsh (lightness, saturation, hue) conversion
%  - M represents length of color vector
M = double(reshape(inputA + inputB,numOfPixels,1))*50;
hue = reshape(diff > 0, numOfPixels, 1);
hue = (hue * hueA) - ((hue - 1) * hueB);
saturation = reshape(diff, numOfPixels, 1);

% from hue and saturation extract standard values of Lab color space
L = M .* cos(saturation);
a = M .* abs(sin(saturation)) .* cos(hue);
b = M .* abs(sin(saturation)) .* sin(hue);

% create output image in Lab color format
output(:,:,1) = reshape(L, size(inputA,1), size(inputA,2), 1);
output(:,:,2) = reshape(a, size(inputA,1), size(inputA,2), 1);
output(:,:,3) = reshape(b, size(inputA,1), size(inputA,2), 1);

% convert image into RGB
image = Lab2RGB(output);

% image preview
imshow(image);

end


function y0 = invlink(y,fali)
% inverse link function for locfit.
% y is a vector of raw fitted values.
% fali is the integer [family link] vector from locfit.
% output is the inv. link.

link = fali(2);

switch(link)
    case 3   % identity
        y0 = y;
    case 4   % log
        y0 = exp(y);
    case 5   % logit - should invert carefully!
        y0 = 1 - 1./(1+exp(y));
    case 6   % inverse
        y0 = 1/y;
    case 7   % sqrt
        y0 = y*abs(y);
    case 8   % arcsin
        y0 = sin(y)*sin(y);
    otherwise
        disp('invlink: Unknown link function');
end;


%demoL2iPotts_DecLap
% Reconstruction of a blurred sparse signal from incomplete measurements under
% Laplacian noise using the inverse L2-Potts functional

% create signal
groundTruth = loadSparse('sig1');
n = numel(groundTruth);

% create Gaussian convolution matrix
K = convkernel('gaussian', 51, 5);
Afull = spconvmatrix(K, n);

% select random measurements
idx = sort(randidx(numel(groundTruth), 0.5));
A = Afull(idx, :);

% create blurred and noisy signal (Gaussian noise)
fBlurry = A * groundTruth;
sigma = 0.05;
fNoisy = fBlurry + sigma* randn(size(fBlurry));

% reconstruction
gamma = 0.025;
[u, nSpikes] = minL2iSpars(fNoisy, gamma, A);

% show result
showSparse(fNoisy, u, groundTruth)


function c=ref_fcdgt(f,g,a,M,m_t,m_f,w_t,w_f)
%REF_CDGT  Reference centered DGT
%   Usage:  c=ref_dgtiv(f,g,a,M,c_t,c_f,c_w);
%
%   Linear algebra version of the algorithm. Create big matrix
%   containing all the basis functions and multiply with the transpose.
%
%   For easy work, m_t,m_f,w_t,w_f are all just 0/1 indicator variables.

L=size(f,1);
N=L/a;
b=L/M;

m_t=m_t*.5;
w_t=w_t*floor(a/2);
w_f=w_f*ceil(b/2);

F=zeros(L,M*N);

l=(0:L-1).';

if m_f==0

  for n=0:N-1
    for m=0:M-1
      F(:,M*n+m+1)=exp(2*pi*i*(m*b+w_f)*(l+m_t)/L).*circshift(g,n*a+w_t);
    end;
  end;

else

  for n=0:N-1
    for m=0:M-1
      F(:,M*n+m+1)=exp(2*pi*i*(m*b+.5+w_f)*(l+m_t-n*a)/L).*circshift(g,n*a+w_t);
    end;
  end;

end;

c=F'*f;


function g =  lfmGradientSigmaH3AV(gamma1_p, gamma1_m, sigma2, t1, ...
    t2, preFactor, mode)

% LFMGRADIENTSIGMAH3AV Gradient of the function h_i(z) with respect \sigma.
% FORMAT
% DESC Computes the gradient of the function h_i(z) with respect to the
% length-scale of the input "force", \sigma.
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode: indicates in which way the vectors t1 and t2 must be transposed
% RETURN g : Gradient of the function with respect to \sigma.
%
% COPYRIGHT : Mauricio Alvarez, 2010

% KERN

g = preFactor(1)*lfmavGradientSigmaUpsilonMatrix(gamma1_p,sigma2, t1,t2, mode) ...
    + preFactor(2)*lfmavGradientSigmaUpsilonMatrix(gamma1_m,sigma2, t1,t2, mode);


function R = rand_rotation(varargin)
% generate random rotation matrix

params = inputParser;
params.CaseSensitive = false;

params.addParameter('RotationBound',2*pi,...
    @(x) 0.0<=x && x<=2*pi);

params.parse(varargin{:});

RotationBound = params.Results.RotationBound;

angle = RotationBound*rand - RotationBound/2;
axis  = randn(3,1);
axis  = axis / norm(axis);
R     = axang2rotm([axis' angle]);
end


% ct_beta_study
% study how to adjust beta when pixel size changes in X-ray CT recon.
% results:
% changing nx has neglible effect, as expected
% changing pixel size has minimal effect when fwhm is converted to mm!
% values used CT studies:
% l2b=9, fov=500, fwhm = 1.62 mm
% l2b=9, fov=250, fwhm = 1.63 mm
% so we can  use the same beta regardless of pixel size and get nearly same fwhm
% caution: if other factors change, such as the # of views, may need to revisit!

%nx = 256;
%nx = 128;
nx = 64;
ny = nx;

if ~isvar('R')
	mask = true(nx,ny);
	l2b = 9;
	R = Robject(mask, 'edge_type', 'tight', 'beta', 2^l2b);
end

pixel_size_list = [500/512 250/512 0.25];
for ii=1:length(pixel_size_list)
	pixel_size = pixel_size_list(ii);

	sys = ct_sys('nx', nx, 'ny', ny, 'pixel_size', pixel_size);

	G = Gtomo2_dscmex(sys, 'nthread', 2);

%	psf = qpwls_psf(G, C, 2^l2b, mask, 1);
	psf = qpwls_psf(G, R, 1, mask, 1);
	fwhm = fwhm2(psf);
	printf('fwhm in mm = %g', fwhm * pixel_size)
end