%--------------------------------------------------------------------------
%   [yn,ref_dn,W,en] = lms2(xn,dn,M,mu)
%--------------------------------------------------------------------------
%   功能：
%   LMS自适应滤波器 带通道时延
%--------------------------------------------------------------------------
%   输入:
%           xn  输入信号的序列 列向量
%           dn  所期望的响应序列 列向量
%           M   滤波器阶数
%           mu  收敛因子(步长)
%   输出:
%           W   滤波器的权值矩阵     大小为M * iter
%           en  误差序列（itr * 1） 列向量
%           yn  实际输出序列        列向量
%--------------------------------------------------------------------------
%   例子：
%   暂无
%--------------------------------------------------------------------------
function [yn,ref_dn,W,en] = lms2(xn,dn,M,mu)
if rem(M,2)==0
    disp('请将滤波器阶数为奇数')
    return
end

w_ref = [zeros((M-1)/2,1);1;zeros((M-1)/2,1)];                              %正常延迟信号 延迟(M-1)/2个点
xn = xn(:);
dn =conv(w_ref,dn(:));                                                      %正常信号延迟
ref_dn = dn;
itr = length(xn);
en = zeros(itr,1);                                                          %误差序列，en(k)表示第k次迭代时预期输出与实际输出的误差
W = zeros(M,itr);                                                           %滤波器的权值矩阵（矩阵）
%--------------------------------------------------------------------------
%   迭代计算
%--------------------------------------------------------------------------
for idx = M:itr                                                             %第k次迭代
    x = xn(idx:-1:idx-M+1);                                                 %滤波器M个抽头的输入
    y = W(:,idx-1).'*x;                                                     %滤波器的输出
    en(idx) = dn(idx) - y;                                                  %第k次迭代的误差
    %----------------------------------------------------------------------
    %   滤波器权值计算的迭代式
    %----------------------------------------------------------------------
    W(:,idx) = W(:,idx-1) + 2*mu*en(idx)*x;
end

%--------------------------------------------------------------------------
%   求最优时滤波器的输出序列，r如果没有yn的返回参数可以不要下面的
%--------------------------------------------------------------------------
yn = conv(W(:,end),xn);


function [masks,keepLocs] = maskGaussians( siz, M, width, offset, show )
% Divides a volume into softly overlapping gaussian windows.
%
% Return M^nd masks each of size siz.  Each mask represents a symmetric
% gaussian window, the locations are evenly spaced throughout the array of
% size siz.  For example, if M=2, then along each dimension d the location
% of each mask is either 1/4 or 3/4 of siz(d) and likewise if M=3 that mask
% is at 1/6,3/6, or 5/6 of siz(d). For higher M the locations are:
% 1/2M,3/2M,...,M-1/2M. See examples below to visualize the masks.
%
% The std of each gaussian is set to be equal to the spacing between two
% adjacent masks multiplied by width.  Reducing the widths of the gaussians
% causes there to be less overlap between masks, but if the width is
% reduced too far certain pixels in between masks receive very little
% weight.  A desired property of the masks is that their coverage (the
% total weight placed on the pixel by all the masks) is approximately
% constant.  Typically, we settle for having the coverage be monotonically
% decreasing as we move away from the center.  (In reality the coverage
% oscilates as we move past peaks, it's just that the oscillations tend to
% be small).  The default value of the width is .6, which minimizes overlap
% while still providing good overall coverage. Values lower tend to produce
% noticeable oscillations in coverage.  offset in (-.5,1) controls the
% spacing of the locations.  Essentially, a positive offset moves the
% locations away from the center of the array and a negative offset moves
% the windows away from the center.  Using a positive offset gives better
% coverage to areas near the borders.
%
% USAGE
%  [masks,keepLocs] = maskGaussians( siz, M, [width], [offset], [show] )
%
% INPUTS
%  siz         - dimensions of each mask
%  M           - # mask locations along each dim [either scalar or vector]
%  width       - [.6] widths of the gaussians
%  offset      - [.1] spacing of mask centers; in (-.5,1)
%  [show]      - [0] figure to use for display (no display if==0) (nd<=3)
%
% OUTPUTS
%  masks       - [see above] array of size [siz x M^nd]
%  keepLocs    - logical array of all locs where masks is nonzero
%
% EXAMPLE
%  masks = maskGaussians( 100, 10, .6, -.1, 1 );  %1D
%  masks = maskGaussians( [35 35], 3, .6, .1, 1 );  %2D
%  masks = maskGaussians( [35 35 35], [2 2 4], .6, .1, 1 ); %3D
%
% See also HISTCIMLOC, MASKCIRCLE
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.0
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

nd = length(siz);
if( nargin<3 || isempty(width)); width = .6; end;
if( nargin<4 || isempty(offset)); offset = .1; end;
if( nargin<5 || isempty(show) || nd>3 ); show = 0; end;

%%% uses a cache since this is slow but often called with same inputs.
persistent cache; if( isempty(cache) ); cache=simpleCache('init'); end;
key = [nd siz M width offset];
[found,val] = simpleCache( 'get', cache, key );
if( found ) %%% get masks and keepLocs from cache
  [masks,keepLocs] = deal(val{:});
else %%% create masks and keepLocs
  [M,er] = checkNumArgs( M, [1 nd], 0, 2 ); error(er);
  inds = {':'}; inds = inds(:,ones(1,nd));
  if( offset<=-.5 || offset>=1 ); error('offset must be in (-.5,1)'); end;

  %%% the covariance of each window
  spacing = (siz*(1+2*offset))./M;
  sigmas = spacing * width;
  C = diag(sigmas.^2);

  %%% create each mask
  masks = zeros( [siz,prod(M)] );
  for c=1:prod(M)
    sub = ind2sub2( M, c );
    mus = (sub-.5).* spacing + .5-offset*siz;
    masks(inds{:},c) = filterGauss( siz, mus, C );
  end
  keepLocs = masks>1e-7;

  %%% place into cache
  cache = simpleCache( 'put', cache, key, {masks,keepLocs} );
end;

%%% optionally display
if( show )
  if( nd==1 )
    figure(show); clf; plot( masks );
    figure(show+1); clf; plot(sum( masks,nd+1 ));
    a=axis; a(3)=0; axis(a);
    title('coverage');
 elseif( nd==2)
    figure(show); clf; montage2( masks );
    figure(show+1); clf; im(sum( masks,nd+1));
    title('coverage');
  elseif( nd==3)
    figure(show); clf; montage2( masks );
    figure(show+1); clf; montage2(sum( masks,nd+1) );
    title('coverage');
  end
end;


function nMC=grMinVerCover(E,d)
% Function nMC=grMinVerCover(E,d) solve the minimal vertex cover problem.
% Input parameters:
%   E(m,2) - the edges of graph;
%     1st and 2nd elements of each row is numbers of vertexes;
%     m - number of edges.
%   d(n) (optional) - the weights of vertexes,
%     n - number of vertexes.
%     If we have only 1st parameter E, then all d=1.
% Output parameter:
%   nMC - the list of the numbers of vertexes included
%     in the minimal (weighted) vertex cover.
% Uses the reduction to integer LP-problem.
% Required the Optimization Toolbox v.3.0.1 or over.
% Author: Sergiy Iglin
% e-mail: siglin@yandex.ru
% personal page: http://iglin.exponenta.ru

% ============= Input data validation ==================
if nargin<1,
  error('There are no input data!')
end
[m,n,E] = grValidation(E); % E data validation
if nargin<2, % we may only 1st parameter
  d=ones(n,1); % all weights =1
else
  d=d(:); % reshape to vector-column
  if length(d)<n, % the poor length
    error('The length of the vector d is poor!')
  else
    d=d(1:n); % First n numbers
  end
end

% ============= Parameters of integer LP problem ==========
A=zeros(n,m); % for incidence matrix
A(E(:,1:2)+repmat(([1:m]'-1)*n,1,2))=1; % we fill the incidence matrix
options=optimset('bintprog'); % the default options
options.Display='off'; % we change the output

% ============= We solve the MILP problem ==========
xmin=bintprog(d,-A',-ones(m,1),[],[],[],options);
nMC=find(round(xmin)); % the answer - numbers of vertexes
return


function [node,elem,phiValue,oldNodeIdx,oldNode,oldElemIdx,oldElem] = interfaceadaptivemesh(node,elem,phi)
% INTERFACEMESH bisect the initial mesh to get a mesh fiting the interface
%
%   [node,elem] = INTERFACEADAPTIVEMESH(node,elem,phi,a), where
%    - node and elem represent an initial mesh enclosing the interface;
%    - phi is a level set function defining the interface as follows
%       {p | phi(p)=0} represents the interface
%       {p | phi(p) < 0} represents the interior domain enclosed by the interface
%       {p | phi(p) > 0} rpepresents the exterior domain of the interface.
%
%  Definition
%   Interface element: the element whose vertices are not in the same side
%   of the interface.
%
%   Interface node: the vertices of all the interface elements. Split the
%   set of interface nodes into two parts: interior interface nodes (phi(p)<=0) and
%   exterior interface nodes (phi(p)>=0);
%
%  Example
%   clear all; close all;
%   node = [-1 -1; 1 -1; 1 1; -1 1];
%   elem = [2 3 1; 4 1 3];
%   [node,elem] = interfacemesh(node,elem,@ellipse);
%
%  See also bisect, circle, ellipse.
%
%  Reference: H. Wei, L. Chen, Y. Huang and B. Zheng. Adaptive Mesh
%  Refinement and Superconvergence for Two Dimensional Interface Problems.
%  SIAM Journal on Scientific Computing, 2014.
%
%  Add by Huayi Wei. Based on discussion with Long Chen.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

oldNodeIdx = [];
oldNode = [];
oldElemIdx = [];
oldElem = [];

%% Step 1: Refine mesh such that the set of interface elements is non-empty
N0 = size(node,1);
phiValue = phi(node);

if all(phiValue<0)
    error('The initial mesh is in the interior of the domain the interface surrounds, please use a mesh which can cover the interface!')
end

% uniform bisect the mesh, to make sure there exist at least one mesh node
% which is inside of the interface, namely, phi(p)<0.
while all(phiValue>0)
    [node,elem] = uniformbisect(node,elem);
    N1 = size(node,1);
    phiValue(N0+1:N1) = phi(node(N0+1:N1,:)); % append the phi value
    N0 = N1;
end

display('Finish Step 1: uniform bisect all elements');

%% Step 2: estimate the discrete curvature, then bisect elements with big curvature
% find interface elements, which are elements crossed by the interface and
% restrict the computation to the interface elements
[interfaceElemIdx, ~, isInterfaceElem] = markinterface(elem,phiValue);
interfaceElem = elem(interfaceElemIdx,:);
isInterfaceNode = false(size(node,1),1);
isInterfaceNode(interfaceElem(:))=true;
interfaceNT = size(interfaceElem,1);
T = auxstructure(interfaceElem);
edge = double(T.edge);
edge2elem = T.edge2elem;
clear T;

% case: Fig 2(a) in p6
tmpAngle = ones(interfaceNT,1)*[90,45,45];
angle = accumarray(interfaceElem(:),tmpAngle(:),[size(node,1) 1]);
is360 = (angle == 360);

% case: Fig 2(b) in p6
isInterfaceBEdge = (edge2elem(:,1) == edge2elem(:,2));
interfaceBEdge = edge(isInterfaceBEdge,:);
v2vI = sparse(interfaceBEdge,interfaceBEdge(:,[2,1]),1,size(node,1),size(node,1));
isLinkNode = (v2vI*isInterfaceNode >= 3);

% case: Fig 2(c) in p6
isInteriorNode = phiValue < 0;
turnAngle = zeros(size(node,1),1);
isIn = isInterfaceNode & isInteriorNode & ~is360 & ~isLinkNode;
turnAngle(isIn) = 180 - (360 - angle(isIn));
isOut = isInterfaceNode & ~isInteriorNode & ~is360 & ~isLinkNode;
turnAngle(isOut ) = (360 - angle(isOut)) -180;
v2v = sparse(edge,edge(:,[2,1]),1,size(node,1),size(node,1));
turnAngleTotal = v2v*turnAngle+turnAngle;

% case: size of interface elements should be small enough
ve = node(elem(:,3),:) - node(elem(:,2),:);
L = sqrt(sum(ve.^2,2));
mL = max(L);
isBigSizeElem = ((L > mL*mL) & isInterfaceElem);
% mark nodes for refinement
isBigCurvNode = (is360 | isLinkNode |(abs(turnAngleTotal)> 170));

if any(isBigCurvNode) || any(isBigSizeElem)

    bigCurvElem = interfaceElemIdx(max(isBigCurvNode(interfaceElem),[],2));
    bigSizeElem = find(isBigSizeElem);

    count = 0;
    while  count < 100
        % bisect marked elements
        [node,elem] = bisect(node,elem,[bigCurvElem; bigSizeElem]);

        N1 = size(node,1);
        phiValue(N0+1:N1) = phi(node(N0+1:N1,:));
        N0 = N1;

        % find interface elements and nodes
        interfaceElemIdx = markinterface(elem,phiValue);
        interfaceElem = elem(interfaceElemIdx,:);
        isInterfaceNode = false(size(node,1),1);
        isInterfaceNode(interfaceElem(:))=true;
        T = auxstructure(interfaceElem);
        edge = double(T.edge);
        edge2elem = T.edge2elem;
        clear T;

        % find triangles containing curves with high curvature
        % case: Fig 2(a) in p6
        interfaceNT = size(interfaceElem,1);
        tmpAngle = ones(interfaceNT,1)*[90,45,45];
        angle = accumarray(interfaceElem(:),tmpAngle(:),[size(node,1) 1]);
        is360 = angle == 360;

        % case: Fig 2(b) in p6
        isInterfaceBEdge = edge2elem(:,1) == edge2elem(:,2);
        interfaceBEdge = edge(isInterfaceBEdge,:);
        v2vI = sparse(interfaceBEdge,interfaceBEdge(:,[2,1]),1,size(node,1),size(node,1));
        isLinkNode = (v2vI*isInterfaceNode >= 3);

        % case: Fig 2(c) in p6
        isInteriorNode = (phiValue < 0);
        turnAngle = zeros(size(node,1),1);
        isIn = isInterfaceNode & isInteriorNode & ~is360 & ~isLinkNode;
        turnAngle(isIn) = 180 - (360 - angle(isIn));
        isOut = isInterfaceNode & ~isInteriorNode & ~is360 & ~isLinkNode;
        turnAngle(isOut ) = (360 - angle(isOut)) -180;
        v2v = sparse(edge,edge(:,[2,1]),1,size(node,1),size(node,1));
        turnAngleTotal = v2v*turnAngle+turnAngle;

        % control size of interface elements
        ve = node(interfaceElem(:,3),:) - node(interfaceElem(:,2),:);
        L = sqrt(sum(ve.^2,2));
        bigSizeElem = interfaceElemIdx(L > mL^2);

        isBigCurvNode = is360 | isLinkNode |(abs(turnAngleTotal)> 170);
        if ~any(isBigCurvNode) && ~any(isBigSizeElem)
            break;
        end

        bigCurvElem = interfaceElemIdx(max(isBigCurvNode(interfaceElem),[],2));
    end
end

display('Finish Step 2: bisect interface with big curvature!');

%% Step 3: Update the type of interface triangles
% Two types of triangles in the bisection grids
%  Type A: two edges parallel to coordinates; see Fig 2.6
%  Type B: the longest edge parallel to x or y coordinate; see Fig 2.7

ve = node(elem(:,3),:) - node(elem(:,2),:);
elemType = (abs(ve(:,1)) > eps & abs(ve(:,2)) > eps);
[interfaceElemIdx, ~,isInterfaceElem] = markinterface(elem,phiValue);
T = auxstructure(elem);
neighbor = T.neighbor;
clear T;
isMark = false(size(elem,1),1);
isMark(interfaceElemIdx) = true;
isMark(neighbor(interfaceElemIdx,1)) = true;

t = cumsum(ones(size(elem,1),1));
tt = neighbor(:,1);
isTransitionTypeBElem = isInterfaceElem & isInterfaceElem(tt) & neighbor(tt,1) ~= t & ~elemType;

%% Deal with the transition Type B elements, see Fig 2.7 (c) and (d) and conditions (C1) and (C2)
%
%  case 1: a Type B interface element whose second neighbor is a interface
%          and  Type A element.
idx2 = find(isTransitionTypeBElem & isInterfaceElem(neighbor(:,2)) ...
            & elemType(neighbor(:,2)));
if ~isempty(idx2)
    p1 = node(elem(idx2,1),:);
    p2 = node(elem(idx2,2),:);
    p3 = node(elem(idx2,3),:);
    p4 = node(elem(neighbor(idx2,2),1),:);
    m14 = (p1 + p4)/2;
    m34 = (p3 + p4)/2;
    m23 = (p2 + p3)/2;
    m1 = (m23 + p3)/2;
    m2 = (m23 + p2)/2;
    isNotTB = ((phiValue(elem(idx2,3)) .* phi(m1)) > 0 & ...
               (phi(m14) .* phi(m34) < 0)) | phi(m1).*phi(m2) < 0;
    isTransitionTypeBElem(idx2(isNotTB)) = false;
end

%  case 2: a Type B interface element whose third neighbor is a interface
%          and  Type A element.
idx3 = find(isTransitionTypeBElem & isInterfaceElem(neighbor(:,3)) ...
            & elemType(neighbor(:,3)));
if ~isempty(idx3)
    p1 = node(elem(idx3,1),:);
    p2 = node(elem(idx3,2),:);
    p3 = node(elem(idx3,3),:);
    p4 = node(elem(neighbor(idx3,3),1),:);
    m14 = (p1 + p4)/2;
    m24 = (p2 + p4)/2;
    m23 = (p2 + p3)/2;
    m1 = (m23 + p3)/2;
    m2 = (m23 + p2)/2;
    isNotTB = ((phiValue(elem(idx3,2)).*phi(m2)) > 0 & ...
               (phi(m14).*phi(m24) < 0)) | phi(m1).*phi(m2) < 0;
    isTransitionTypeBElem(idx3(isNotTB)) = false;
end

%  case 3: a Type B interface element whose third neighbor is a interface
%          and  Type B element.
idx4 = find(isTransitionTypeBElem & isInterfaceElem(neighbor(:,2)) ...
          & ~elemType(neighbor(:,2)));
if ~isempty(idx4)
    p2 = node(elem(idx4,2),:);
    p3 = node(elem(idx4,3),:);
    m23 = (p2 + p3)/2;
    m1 = (m23 + p3)/2;
    m2 = (m23 + p2)/2;
    isNotTB =  phi(m1).*phi(m2) < 0;
    isTransitionTypeBElem(idx4(isNotTB)) = false;
end

%  case 4: a Type B interface element whose third neighbor is a interface
%          and  Type B element.
idx5 = find(isTransitionTypeBElem & isInterfaceElem(neighbor(:,3)) ...
          & ~elemType(neighbor(:,3)));
if ~isempty(idx5)
    p2 = node(elem(idx5,2),:);
    p3 = node(elem(idx5,3),:);
    m23 = (p2 + p3)/2;
    m1 = (m23 + p3)/2;
    m2 = (m23 + p2)/2;
    isNotTB =  phi(m1).*phi(m2) < 0;
    isTransitionTypeBElem(idx5(isNotTB)) = false;
end
typeB = (isMark & ~elemType & ~isTransitionTypeBElem);

% bisect some transition type B elements to type A
while any(typeB)

    [node,elem,~, ~,tree] = bisect(node,elem,typeB);
    N1 = size(node,1);
    phiValue(N0+1:N1) = phi(node(N0+1:N1,:));
    N0 = N1;

    elemType = updatetype(elemType,tree(:,[2,3]));

    [interfaceElemIdx, ~,isInterfaceElem] = markinterface(elem,phiValue);

    T = auxstructure(elem);
    neighbor = T.neighbor;
    clear T;
    isMark = false(size(elem,1),1);
    isMark(interfaceElemIdx) = true;
    isMark(neighbor(interfaceElemIdx,1)) = true;

    t = cumsum(ones(size(elem,1),1));
    tt = neighbor(:,1);
    isTransitionTypeBElem = isInterfaceElem & isInterfaceElem(tt) & ...
                            neighbor(tt,1) ~= t & ~elemType;

    %  case 1: a Type B interface element whose second neighbor is a interface
    %          and  Type A element.
    idx2 = find(isTransitionTypeBElem & isInterfaceElem(neighbor(:,2)) & ...
                elemType(neighbor(:,2)));
    if ~isempty(idx2)
        p1 = node(elem(idx2,1),:);
        p2 = node(elem(idx2,2),:);
        p3 = node(elem(idx2,3),:);
        p4 = node(elem(neighbor(idx2,2),1),:);
        m14 = (p1 + p4)/2;
        m34 = (p3 + p4)/2;
        m23 = (p2 + p3)/2;
        m1 = (m23 + p3)/2;
        m2 = (m23 + p2)/2;
        isNotTB = ((phiValue(elem(idx2,3)) .* phi(m1)) > 0 & ...
                   (phi(m14) .* phi(m34) < 0)) | phi(m1).*phi(m2) < 0;
        isTransitionTypeBElem(idx2(isNotTB)) = false;
    end
    %  case 2: a Type B interface element whose third neighbor is a interface
    %          and  Type A element.
    idx3 = find(isTransitionTypeBElem & isInterfaceElem(neighbor(:,3)) & ...
                elemType(neighbor(:,3)));
    if ~isempty(idx3)
        p1 = node(elem(idx3,1),:);
        p2 = node(elem(idx3,2),:);
        p3 = node(elem(idx3,3),:);
        p4 = node(elem(neighbor(idx3,3),1),:);
        m14 = (p1 + p4)/2;
        m24 = (p2 + p4)/2;
        m23 = (p2 + p3)/2;
        m1 = (m23 + p3)/2;
        m2 = (m23 + p2)/2;
        isNotTB = ((phiValue(elem(idx3,2)).*phi(m2)) > 0 & ...
                   (phi(m14).*phi(m24) < 0)) | phi(m1).*phi(m2) < 0;
        isTransitionTypeBElem(idx3(isNotTB)) = false;
    end

    %  case 3: a Type B interface element whose second neighbor is a interface
    %          and  Type B element.
    idx4 = find(isTransitionTypeBElem & isInterfaceElem(neighbor(:,2)) & ...
               ~elemType(neighbor(:,2)));
    if ~isempty(idx4)
        p2 = node(elem(idx4,2),:);
        p3 = node(elem(idx4,3),:);
        m23 = (p2 + p3)/2;
        m1 = (m23 + p3)/2;
        m2 = (m23 + p2)/2;
        isNotTB =  phi(m1).*phi(m2) < 0;
        isTransitionTypeBElem(idx4(isNotTB)) = false;
    end
    %  case 4: a Type B interface element whose third neighbor is a interface
    %          and  Type B element.
    idx5 = find(isTransitionTypeBElem & isInterfaceElem(neighbor(:,3)) & ...
                ~elemType(neighbor(:,3)));
    if ~isempty(idx5)
        p2 = node(elem(idx5,2),:);
        p3 = node(elem(idx5,3),:);
        m23 = (p2 + p3)/2;
        m1 = (m23 + p3)/2;
        m2 = (m23 + p2)/2;
        isNotTB =  phi(m1).*phi(m2) < 0;
        isTransitionTypeBElem(idx5(isNotTB)) = false;
    end
    typeB = (isMark & ~elemType & ~isTransitionTypeBElem);
end

% find interface elements
interfaceElemIdx= markinterface(elem,phiValue);
interfaceElem = elem(interfaceElemIdx,:);
% find interface nodes, which are the vertices of interface elements.
isInterfaceNode = false(size(node,1),1);
isInterfaceNode(interfaceElem(:))=true;
T = auxstructure(elem);
edge = T.edge;
edge2elem = T.edge2elem;
clear T;
% case: Fig 2.7 (b)
isEdge = edge2elem(:,1) ~= edge2elem(:,2);
isEdge = isEdge & (isInterfaceNode(edge(:,1)) | isInterfaceNode(edge(:,2)) |...
    isInterfaceNode(elem(edge2elem(:,1),1)) | isInterfaceNode(elem(edge2elem(:,2),1)));
isEdge = isEdge &  edge2elem(:,3) == 1 & edge2elem(:,4)==1;
isEdge = isEdge & ~elemType(edge2elem(:,1));
isMark = false(size(elem,1),1);
isMark(edge2elem(isEdge,1)) = true;
isMark(edge2elem(isEdge,2)) = true;
[node,elem,~, ~,tree] = bisect(node,elem,isMark);
N1 = size(node,1);
phiValue(N0+1:N1) = phi(node(N0+1:N1,:));
% N0 = N1;
elemType = updatetype(elemType,tree(:,[2,3]));

display('Finish Step 3: transform the type B of  element to type A!');

%% Step 4: Move some interface nodes onto the interface
%
%  Here we just move interface nodes along the shortest edges of all the
%  type A interface elements. For a such kind of edge, we get the
%  intersection points and compute the distances between the two endpoints
%  and the intersection points. An interface node maybe have two moving
%  direction, just choose a direction which move shorter distance, then the
%  qualities of the elements connecting to this node will decrease less.

interfaceElemIdx= markinterface(elem,phiValue);
interfaceElem = elem(interfaceElemIdx,:);
T = auxstructure(interfaceElem);
edge = double(T.edge);
edge2elem = T.edge2elem;
isSpecial = elemType(interfaceElemIdx(edge2elem(:,1))) & ...
    ~elemType(interfaceElemIdx(edge2elem(:,2))) & edge2elem(:,3) == 1;
isSpecial = isSpecial | (~elemType(interfaceElemIdx(edge2elem(:,1))) & ...
    elemType(interfaceElemIdx(edge2elem(:,2))) & edge2elem(:,4) == 1);
isShortEdge = (edge2elem(:,1) ~= edge2elem(:,2)) & ~(edge2elem(:,3) == 1 & ...
               edge2elem(:,4) == 1) & ~isSpecial;
A = node(edge(isShortEdge,1),:);
B = node(edge(isShortEdge,2),:);
h = sqrt(sum((A-B).^2,2));
M = findintersectbisect(phi,A,B);
h1 = sqrt(sum((A-M).^2,2));
h2 = h - h1;
t = sparse(edge(isShortEdge,[1,2]),edge(isShortEdge,[2,1]),[h1./h,h2./h], size(node,1),size(node,1));
t1 = sparse(edge(isShortEdge,[1,2]),edge(isShortEdge,[2,1]),repmat(cumsum(ones(size(M,1),1)),1,2), size(node,1),size(node,1));
[maxt,I] = max(t,[],1);
isIrregularNode = maxt >= 0.5;
idx = find(isIrregularNode);
idx1 = I(idx)+(idx - 1)*size(node,1);

oldNodeIdx = [oldNodeIdx;idx'];
oldNode = [oldNode;node(idx,:)];

node(idx,:) = M(t1(idx1),:);

phiValue(idx) = 0;

display('Finish Step 4: move the nearest nodes to interface!');
% figure
% showmesh(node,elem);
% findelem(node,elem,interfaceElemIdx,'noindex');
% plotcurve(a);

%% Step 5: Edge swap
%
%  We just consider the case that two elements share a common longest edge,
%  whose one or two of its four vertices are moved on the interface. After
%  moving, the qualities of this two elements will change(and most case,
%  the quality will decrease). If edge swapping can improve the qualities,
%  we do edge swapping for this two elements.
%
%  But if the common longest edge of the two element is an interface edge,
%  namely its two endpoints are on the interface, we can't use edge
%  swapping. In this case, we deal with it in Step 7. Just find the element
%  patchs of  the first vertices of these two element, and replace this two
%  vertices by the centroids of the element patchs, respectively.

T = auxstructure(elem);
edge = double(T.edge);
edge2elem = T.edge2elem;
clear T;

leftElemIdx = edge2elem(:,1); % the index of the first neighbor of edge
rightElemIdx = edge2elem(:,2); % the index of the second neighbor of  edge

isIrregularNode = full(isIrregularNode)';
isLongestEdge = (edge2elem(:,1) ~= edge2elem(:,2)) & ...
    edge2elem(:,3) == 1 & edge2elem(:,4)== 1 & ...
    (isIrregularNode(edge(:,1)) | isIrregularNode(edge(:,2)) | ...
    isIrregularNode(elem(leftElemIdx,1)) | isIrregularNode(elem(rightElemIdx,1)));

isCrossLongestEdge = (edge2elem(:,1) ~= edge2elem(:,2)) & ...
    edge2elem(:,3) == 1 & edge2elem(:,4)== 1 & ...
    isIrregularNode(elem(leftElemIdx,1)) & isIrregularNode(elem(rightElemIdx,1)) &...
    ~isIrregularNode(edge(:,1)) & ~isIrregularNode(edge(:,2));

isInterfaceEdge = isIrregularNode(edge(:,1)) & isIrregularNode(edge(:,2));
isInterfaceEdge = isLongestEdge & isInterfaceEdge ;
isInterfaceEdge = isInterfaceEdge & ~isIrregularNode(elem(leftElemIdx,1)) ;
isInterfaceEdge = isInterfaceEdge & ~isIrregularNode(elem(rightElemIdx,1));

isLongestEdge(isInterfaceEdge) = false;

leftElemIdx = edge2elem(isLongestEdge,1);
rightElemIdx = edge2elem(isLongestEdge,2);

leftElem = elem(leftElemIdx,:);
rightElem = elem(rightElemIdx,:);

leftElemNew = [leftElem(:,2), rightElem(:,1),leftElem(:,1)];
rightElemNew = [rightElem(:,2), leftElem(:,1),rightElem(:,1)];

l1 = triangleratio(node,leftElem);
r1 = triangleratio(node,rightElem);

l2 = triangleratio(node,leftElemNew);
r2 = triangleratio(node,rightElemNew);

notNeedSwap = min([l1,r1],[],2) >= min([l2,r2],[],2) & ~isCrossLongestEdge(isLongestEdge);

leftElemNew(notNeedSwap,:) = leftElem(notNeedSwap,:);
rightElemNew(notNeedSwap,:) = rightElem(notNeedSwap,:);

oldElemIdx =[oldElemIdx;leftElemIdx;rightElemIdx];
oldElem = [oldElem;elem([leftElemIdx;rightElemIdx],:)];

elem(leftElemIdx,:) = leftElemNew;
elem(rightElemIdx,:) = rightElemNew;

display('Finish Step 5: edge swap!')

%% Step 6: Mesh smoothing for near interface nodes
% q = minangle(node,elem);
% [minq,I] = min(q);

T = auxstructure(elem);
% edge = double(T.edge);
neighbor = double(T.neighbor);
clear T;

isInterfaceNode = (msign(phiValue) == 0);

badElem = find(~isInterfaceNode(elem(:,1)) & isInterfaceNode(elem(:,2)) ...
              & isInterfaceNode(elem(:,3)));
v1 = node(elem(badElem,3),:) - node(elem(badElem,2),:);
v2 = node(elem(badElem,1),:) - node(elem(badElem,3),:);
v3 = node(elem(badElem,2),:) - node(elem(badElem,1),:);
badElem = badElem(sum(v2.^2,2)+sum(v3.^2,2) < sum(v1.^2,2)-eps);
moveNodeIdx = elem(badElem,1);
if ~isempty(moveNodeIdx)
   N = size(node,1);
   NT = size(elem,1);
   v2t = sparse(elem,[(1:NT)',(1:NT)',(1:NT)'],1,N,NT);
   area = simplexvolume(node,elem);
   bc = (node(elem(:,1),:) + node(elem(:,2),:) + node(elem(:,3),:))/3;
   oldNodeIdx = [oldNodeIdx;moveNodeIdx];
   oldNode = [oldNode;node(moveNodeIdx,:)];
   ba = v2t(moveNodeIdx,:)*area;
   node(moveNodeIdx,:) = v2t(moveNodeIdx,:)*([area,area].*bc)./[ba,ba];
end
% case:
badElem2 = find(~isInterfaceNode(elem(:,2)) & isInterfaceNode(elem(:,1)) ...
              & isInterfaceNode(elem(:,3)) & ~elemType(neighbor(:,1)));
v1 = node(elem(badElem2,3),:) - node(elem(badElem2,2),:);
v2 = node(elem(badElem2,1),:) - node(elem(badElem2,3),:);
v3 = node(elem(badElem2,2),:) - node(elem(badElem2,1),:);
badElem2 = badElem2(sum(v2.^2,2)+sum(v3.^2,2)<sum(v1.^2,2)-eps);
% case:
badElem3 = find(~isInterfaceNode(elem(:,3)) & isInterfaceNode(elem(:,1)) ...
              & isInterfaceNode(elem(:,2)) & ~elemType(neighbor(:,1)));
v1 = node(elem(badElem3,3),:) - node(elem(badElem3,2),:);
v2 = node(elem(badElem3,1),:) - node(elem(badElem3,3),:);
v3 = node(elem(badElem3,2),:) - node(elem(badElem3,1),:);
badElem3 = badElem3(sum(v2.^2,2)+sum(v3.^2,2)<sum(v1.^2,2)-eps);
moveNodeIdx = [elem(badElem2,2);elem(badElem3,3)];
if ~isempty(moveNodeIdx)
   if ~exist('v2t','var')
       N = size(node,1);
       NT = size(elem,1);
       v2t = sparse(elem,[(1:NT)',(1:NT)',(1:NT)'],1,N,NT);
   end
   area = simplexvolume(node,elem);
   bc = (node(elem(:,1),:) + node(elem(:,2),:) + node(elem(:,3),:))/3;
   isMany = sum(v2t(moveNodeIdx,:),2)<=6;
   moveNodeIdx = moveNodeIdx(isMany);
   oldNodeIdx = [oldNodeIdx;moveNodeIdx];
   oldNode = [oldNode;node(moveNodeIdx,:)];
   ba = v2t(moveNodeIdx,:)*area;
   node(moveNodeIdx,:) = v2t(moveNodeIdx,:)*([area,area].*bc)./[ba,ba];
end
display('Finish Step 6: We got it finally!');
%
% q = minangle(node,elem);
% [minq,I] = min(q);


function D = sqdistance(A, B, M)
% Square Euclidean or Mahalanobis distances between all sample pairs
% A: d x n1 data matrix
% B: d x n2 data matrix
% M: d x d  Mahalanobis matrix
% D: n1 x n2 pairwise square distance matrix
% Written by Michael Chen (sth4nth@gmail.com). July 2009.

if nargin == 1
    A = bsxfun(@minus,A,mean(A,2));
    S = full(sum(A.^2,1));
    D = full((-2)*(A'*A));
    D = bsxfun(@plus,D,S);
    D = bsxfun(@plus,D,S');
elseif nargin == 2
    m = (sum(A,2)+sum(B,2))/(size(A,2)+size(B,2));
    A = bsxfun(@minus,A,m);
    B = bsxfun(@minus,B,m);
    D = full((-2)*(A'*B));
    D = bsxfun(@plus,D,full(sum(B.^2,1)));
    D = bsxfun(@plus,D,full(sum(A.^2,1)'));
elseif nargin == 3
    m = (sum(A,2)+sum(B,2))/(size(A,2)+size(B,2));
    A = bsxfun(@minus,A,m);
    B = bsxfun(@minus,B,m);
    D = full((-2)*(A'*M*B));
    D = bsxfun(@plus,D,full(sum(B.*(M*B),1)));
    D = bsxfun(@plus,D,full(sum(A.*(M*A),1)'));
end


function [ result ] = so3_exp( x )
% compute the exponential mapping of R^3 to SO(3)
% use sparse matrix
	N = size(x,1);
	%if N == 3
        theta=norm(x);
        if theta==0
            result=eye(3);
        else
            omega =x/theta;
            result=eye(3,3) + sin(theta) * skew(omega) + (1 - cos(theta))*skew(omega)^2;
        end

    %else
        % todo, compute exp() of R^6 to SE(3)?
    %    m = (N-3)/3;
    %    result=speye(3+m);
    %    result(1:3,1:3)=Exp( x(1:3));
    %end
end


function test_example_phase_reset

% MEM 4gb
% WALLTIME 00:10:00

%
%% Simulate an oscillatory signal with phase resetting
%
%% # Narrow-band oscillation
%
% The following code simulates a clean oscillating signal with a phase reset at time zero.
%
clear all
close all

f = 5;
t = (-1000:1:1000)/1000;

figure
for i=1:10 % repeat 10 times
s = nan(size(t));
s(t<0 ) = cos(2*pi*f*t(t<0 ) + 2*pi*rand(1)); % random phase
s(t>=0) = cos(2*pi*f*t(t>=0)               ); % constant phase
s = 0.2*s+i;
d(i,:) = s; % remember the signal on each repetition
plot(t, s);
hold on;
end
axis auto

figure;
plot(t, mean(d,1))

%
%% # Broad-band oscillation
%
% The following code also includes a small "random walk" in the phase, i.e. the signal is a little bit broad-band and over time there is some phase dispersion. At t=0 there is still a phase reset. The phase dispersion causes the average ERF again to disappear some time following the phase reset.
%
clear all
close all
f = 5;
t = (-2000:1:5000)/1000;

figure
for i=1:10 % repeat 10 times

p = 2*pi*f*t; % linear phase increase
p(t<0) = p(t<0) + 2*pi*rand(1); % random phase prior to TMS pulse
p = p + cumsum(2*pi*(rand(size(p))-0.5)*0.01); % random walk
p = p - p(t==0);

s = cos(p);
s = 0.2*s+i;
d(i,:) = s; % remember the signal on each repetition
plot(t, s);
hold on;
end
axis auto

figure;
plot(t, mean(d,1))


clear; close all;

%% 10MHz incident signal;
fs = 1000e6;
f = 10e6;
wavelength = fs/f;
sig = wavemaker(3.5, f, fs);
figure; subplot(3,1,1);
plot(sig, 'LineWidth', 1); title(strcat('input signal, wavelength =',num2str(wavelength),' data pts'));
axis([0,3300,-2,2])

%% Sparse signal;
N = 3000;                    % N : length of signal
s = zeros(N,1);
k = [50:(50+wavelength*0.1) 500:(500+wavelength*0.6) 1200:(1200+wavelength*1.6) 2200:(2200+wavelength*4.6)];
s(k) = 1;
subplot(3,1,2);
plot(s, 'LineWidth', 1); title('distribution: objective function');
axis([0,3300,0,2])

%% convoluted signal
y = conv(sig,s);
subplot(3,1,3);plot(y, 'LineWidth', 1);hold on;
plot(abs(hilbert(y)), 'LineWidth', 1);
title('convoluted signal between input and districution');
xlim([0,3300])

% My addition
vX = s(:);
vH = sig(:);
vY = y(:);

save('UserData', 'vX', 'vY', 'vH');
% End of my addition

function x = wavemaker(nCycles, fc, fs)
% function to generate wave packet;
nSample = round(fs / fc * nCycles);
ts      = 1 / fs;
T       = ts * nSample;
t_max   = ts * (nSample-1);
t       = 0: ts: t_max;

x       = sin( 2 * pi * fc .* t);

x = x.*hanning(nSample)';
end


function fim = fftconv2(im,f)
% function fim = fftconv2(im,f)
%
% Convolution using FFT.
%
% David R. Martin <dmartin@eecs.berkeley.edu>
% March 2003

% wrap the filter around the origin and pad with zeros
padf = zeros(size(im));
r = floor(size(f,1)/2);
padf(1:r+1,1:r+1) = f(r+1:end,r+1:end);
padf(1:r,end-r+1:end) = f(r+2:end,1:r);
padf(end-r+1:end,1:r) = f(1:r,r+2:end);
padf(end-r+1:end,end-r+1:end) = f(1:r,1:r);

% magic
fftim = fft2(im);
fftf = fft2(padf);
fim = real(ifft2(fftim.*fftf));


function lp=dir_lpdf(x,a)
%DIR_LPDF   Log probability density function of uniform Dirichlet
%           distribution
%
%       Description:
%       LP = DIR_LPDF(X, A) returns the lpdf of Dirichlet distribution
%       with A at X

% Copyright (c) 2000 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

lp=gammaln(sum(a))-sum(gammaln)+sum(log(x).^(a-1));


function [ n_data, f, s, p, cdf ] = negative_binomial_cdf_values ( n_data )

%*****************************************************************************80
%
%% NEGATIVE_BINOMIAL_CDF_VALUES returns values of the negative binomial CDF.
%
%  Discussion:
%
%    Assume that a coin has a probability P of coming up heads on
%    any one trial.  Suppose that we plan to flip the coin until we
%    achieve a total of S heads.  If we let F represent the number of
%    tails that occur in this process, then the value of F satisfies
%    a negative binomial PDF:
%
%      PDF(F,S,P) = Choose ( F from F+S-1 ) * P**S * (1-P)**F
%
%    The negative binomial CDF is the probability that there are F or
%    fewer failures upon the attainment of the S-th success.  Thus,
%
%      CDF(F,S,P) = sum ( 0 <= G <= F ) PDF(G,S,P)
%
%    In Mathematica, the function can be evaluated by:
%
%      Needs["Statistics`DiscreteDistributions`]
%      dist = NegativeBinomialDistribution [ s, p ]
%      CDF [ dist, f ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    F C Powell,
%    Statistical Tables for Sociology, Biology and Physical Sciences,
%    Cambridge University Press, 1982.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer F, the maximum number of failures.
%
%    Output, integer S, the number of successes.
%
%    Output, real P, the probability of a success on one trial.
%
%    Output, real CDF, the probability of at most F failures
%    before the S-th success.
%
  n_max = 27;

  cdf_vec = [ ...
     0.6367187500000000E+00, ...
     0.3632812500000000E+00, ...
     0.1445312500000000E+00, ...
     0.5000000000000000E+00, ...
     0.2265625000000000E+00, ...
     0.6250000000000000E-01, ...
     0.3437500000000000E+00, ...
     0.1093750000000000E+00, ...
     0.1562500000000000E-01, ...
     0.1792000000000000E+00, ...
     0.4096000000000000E-01, ...
     0.4096000000000000E-02, ...
     0.7047000000000000E-01, ...
     0.1093500000000000E-01, ...
     0.7290000000000000E-03, ...
     0.9861587127990000E+00, ...
     0.9149749500510000E+00, ...
     0.7471846521450000E+00, ...
     0.8499053647030009E+00, ...
     0.5497160941090026E+00, ...
     0.2662040052146710E+00, ...
     0.6513215599000000E+00, ...
     0.2639010709000000E+00, ...
     0.7019082640000000E-01, ...
     0.1000000000000000E+01, ...
     0.1990000000000000E-01, ...
     0.1000000000000000E-03 ];

  f_vec = [ ...
     4,  3,  2, ...
     3,  2,  1, ...
     2,  1,  0, ...
     2,  1,  0, ...
     2,  1,  0, ...
    11, 10,  9, ...
    17, 16, 15, ...
     9,  8,  7, ...
     2,  1,  0 ];

  p_vec = [ ...
     0.50E+00, ...
     0.50E+00, ...
     0.50E+00, ...
     0.50E+00, ...
     0.50E+00, ...
     0.50E+00, ...
     0.50E+00, ...
     0.50E+00, ...
     0.50E+00, ...
     0.40E+00, ...
     0.40E+00, ...
     0.40E+00, ...
     0.30E+00, ...
     0.30E+00, ...
     0.30E+00, ...
     0.30E+00, ...
     0.30E+00, ...
     0.30E+00, ...
     0.10E+00, ...
     0.10E+00, ...
     0.10E+00, ...
     0.10E+00, ...
     0.10E+00, ...
     0.10E+00, ...
     0.10E-01, ...
     0.10E-01, ...
     0.10E-01 ];

  s_vec = [ ...
    4, 5, 6, ...
    4, 5, 6, ...
    4, 5, 6, ...
    4, 5, 6, ...
    4, 5, 6, ...
    1, 2, 3, ...
    1, 2, 3, ...
    1, 2, 3, ...
    0, 1, 2 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    f = 0;
    s = 0;
    p = 0.0;
    cdf = 0.0;
  else
    f = f_vec(n_data);
    s = s_vec(n_data);
    p = p_vec(n_data);
    cdf = cdf_vec(n_data);
  end

  return
end


function d = ddiff ( d1, d2 )

%*****************************************************************************80
%
%% DDIFF returns the signed distance to a region that is the difference of two regions.
%
%  Discussion:
%
%    The author comments that this is not the true signed distance function for
%    the difference set, in particular, around corners.
%
%  Licensing:
%
%    (C) 2004 Per-Olof Persson.
%    See COPYRIGHT.TXT for details.
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, real D1, D2, the signed distances to region 1 and 2.
%
%    Output, real D, the signed distance to the region formed by
%    removing from region 1 its intersection with region 2.
%
  d = max ( d1, -d2 );

  return
end


function a = bab_eigen_right ( n, alpha, beta )

%*****************************************************************************80
%
%% BAB_EIGEN_RIGHT returns the right eigenvectors of the BAB matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real ALPHA, BETA, the parameters.
%
%    Output, real A(N,N), the right eigenvector matrix.
%
  a = zeros ( n, n );

  for i = 1: n
    for j = 1 : n
      angle = ( i * j ) * pi / ( n + 1 );
      a(i,j) = sqrt ( 2.0 / ( n + 1 ) ) * sin ( angle );
    end
  end

  return
end


% Test file for trigtech/prolong.m

function pass = test_prolong(pref)

if ( nargin < 1 )
    pref = trigtech.techPref();
end

testclass = trigtech();

F = @(x) exp(sin(pi*x));
f = testclass.make(F, [], pref);

% Odd prolongation.
k = 101;
go = prolong(f, k);
x = testclass.trigpts(k);
pass(1) = size(go,1) == k  && norm(go.values - F(x), inf) < 10*vscale(go)*eps;

% Even prolongation.
k = 100;
ge = prolong(f, k);
x = testclass.trigpts(k);
pass(2) = size(ge,1) == k  && norm(ge.values - F(x), inf) < 10*vscale(ge)*eps;

% Odd restriction from odd length f.
k = 89;
g = prolong(go, k);
x = testclass.trigpts(k);
pass(3) = size(g,1) == k  && norm(g.values - F(x), inf) < 10*vscale(g)*eps;

% Even restriction from odd length f.
k = 88;
g = prolong(go, k);
x = testclass.trigpts(k);
pass(4) = size(g,1) == k  && norm(g.values - F(x), inf) < 10*vscale(g)*eps;

% Odd restriction from even length f.
k = 89;
g = prolong(ge, k);
x = testclass.trigpts(k);
pass(5) = size(g,1) == k  && norm(g.values - F(x), inf) < 10*vscale(g)*eps;

% Even restriction from even length f.
k = 88;
g = prolong(ge, k);
x = testclass.trigpts(k);
pass(6) = size(g,1) == k  && norm(g.values - F(x), inf) < 10*vscale(g)*eps;

% Restriction to length 1.
g = prolong(f, 1);
pass(7) = size(g,1) == 1;

% Array-valued tests.
F = @(x) exp(sin(pi*x));
f = testclass.make(@(x) [F(x), -F(x)], [], pref);
k = 101;
g = prolong(f, k);
x = testclass.trigpts(k);
values = g.coeffs2vals(g.coeffs);
pass(8) = size(g,1) == k && norm(values - [F(x), -F(x)], inf) < ...
    100*max(vscale(g)*eps);

g = prolong(f, 1);
pass(9) = size(g,1) == 1;

% Same length test
g = prolong(f, size(f,1));
fvalues = f.coeffs2vals(f.coeffs);
gvalues = g.coeffs2vals(g.coeffs);
pass(10) = all(fvalues(:) == gvalues(:));

% Values test.
v = [1 2 3];
f = testclass.make(v, [], pref);
g = prolong(f, 5);
values = g.coeffs2vals(g.coeffs);
pass(11) = norm(values - repmat([1 2 3], 5, 1), inf) < ...
   10*max(vscale(g)*eps);

end


function b = r8pbu_mxv ( n, mu, a, x )

%*****************************************************************************80
%
%% R8PBU_MXV multiplies a R8PBU matrix times a vector.
%
%  Discussion:
%
%    The R8PBU storage format is for a symmetric positive definite band matrix.
%
%    To save storage, only the diagonal and upper triangle of A is stored,
%    in a compact diagonal format that preserves columns.
%
%    The diagonal is stored in row MU+1 of the array.
%    The first superdiagonal in row MU, columns 2 through N.
%    The second superdiagonal in row MU-1, columns 3 through N.
%    The MU-th superdiagonal in row 1, columns MU+1 through N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer MU, the number of superdiagonals in the matrix.
%    MU must be at least 0 and no more than N-1.
%
%    Input, real A(MU+1,N), the R8PBU matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(N), the result vector A * x.
%

%
%  Multiply X by the diagonal of the matrix.
%
  b(1:n) = a(mu+1,1:n) .* x(1:n);
%
%  Multiply X by the superdiagonals of the matrix.
%
  for i = mu : -1 : 1
    for j = mu+2-i : n
      ieqn = i + j - mu - 1;
      b(ieqn) = b(ieqn) + a(i,j) * x(j);
      b(j) = b(j) + a(i,j) * x(ieqn);
    end
  end

  return
end


function madReadings = removeOLMADx5(readings)
%This function removes outliers from the data set using the median
%absolute deviation method with a x5 factor. The formula used is
%MAD = Median { | y(i) - m | / 0.6745 }

med = median(readings); %get median value of readings
x = abs((readings - med)); %get absolute value of each reading minus median value
MADx5 = median(x/.6745)*5; %get value of x array and multiply it by 5, result is outlier factor
j = 1; %variable for non-outlier array
madReadings = zeros(1,length(readings));%allocate array for storing readings with outliers removed

for i = 1:length(readings) %this loop removes outliears
    if readings(i) <= (med + MADx5) && readings(i) >= (med - MADx5)
        madReadings(j) = readings(i);%if not an outlier add to array
        j = j + 1;
    end

end

madReadings((j):length(madReadings)) = [];%delete empty array items


function mi = BF_MutualInformation(v1,v2,r1,r2,numBins)
% BF_MutualInformation  Mutual information between two data vectors using bin counting.
%
% Mutual information computed using a histogram-based, bin-counting method.
%
%---INPUTS:
% v1, the first input vector
% v2, the second input vector
% r1, the bin-partitioning method for the first input vector, v1
% r2, the bin-partitioning method for the second input vector, v2
% numBins, the number of bins to partition each vector into.
%
% NB: r1 and r2 can also be two-component vectors, that specify a custom range
%     for binning
%
%---OUTPUT:
% mi, the mutual information computed between v1 and v2

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
%% Check inputs and set defaults:
% ------------------------------------------------------------------------------
% by default, take a range equal to the range of the vectors
if nargin < 3 || isempty(r1)
    r1 = 'range';
end
if nargin < 4 || isempty(r2)
    r2 = 'range';
end
% use 10 bins for the histograms (=100 bins in 2d)
if nargin < 5 || isempty(numBins)
    numBins = 10;
end
% ------------------------------------------------------------------------------

if length(v1) ~= length(v2)
    error('input vectors must be the same length');
end

N = length(v1); % length of vectors

% Make sure both column vectors
if size(v1,2) > size(v1,1), v1 = v1'; end
if size(v2,2) > size(v2,1), v2 = v2'; end

% Create histograms:
% (i) in v1
edgesi = SUB_GiveMeEdges(r1,v1,numBins);
[ni, bini] = histc(v1, edgesi);

% (ii) in v2
edgesj = SUB_GiveMeEdges(r2,v2,numBins);
[nj, binj] = histc(v2, edgesj);

% ------------------------------------------------------------------------------
%% Create a joint histogram:
% ------------------------------------------------------------------------------

% We have the edges in each dimension: edgesi, and edgesj
histxy = zeros(numBins);
for i2 = 1:numBins
    for j2 = 1:numBins
        histxy(i2,j2) = sum(bini==i2 & binj==j2);
    end
end

% Normalize counts to probabilities
p_i = ni(1:numBins)/N;
p_j = nj(1:numBins)/N;
p_ij = histxy/N;
p_ixp_j = p_i*p_j';
summe = (p_ixp_j > 0 & p_ij > 0);

% Do a matrix-sum mutual information calculation:
if any(summe(:) == 1)
    mi = sum(p_ij(summe).*log(p_ij(summe)./p_ixp_j(summe)));
else
    fprintf(1,['The histograms aren''t catching any points?? ' ...
            'Perhaps due to an inappropriate custom range for binning the data...\n']);
    mi = NaN; return
end

% ------------------------------------------------------------------------------
% ------------------------------------------------------------------------------
function edges = SUB_GiveMeEdges(r,v,nbins)
    EE = 1E-6; % this small addition gets lost in the last bin
    if strcmp(r,'range')
        edges = linspace(min(v),max(v)+EE,nbins+1);

    elseif strcmp(r,'quantile') % bin edges based on quantiles
        edges = quantile(v,linspace(0,1,nbins+1));
%             edges(1) = edges(1) - 0.1;
        edges(end) = edges(end) + EE;
%             edges = sort(unique(edges)); % in case you have many repeated values -- will bias MI calculation
    elseif length(r)==2 % a two-component vector
        edges = linspace(r(1),r(2)+EE,nbins+1);
    else
        error('Unknown partitioning method ''%s''',r);
    end
end
% ------------------------------------------------------------------------------
end


function cdf = burr_cdf ( x, a, b, c, d )

%*****************************************************************************80
%
%% BURR_CDF evaluates the Burr CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument of the CDF.
%
%    Input, real A, B, C, D, the parameters of the PDF.
%    0 < B,
%    0 < C.
%
%    Output, real CDF, the value of the CDF.
%
  if ( x <= a )

    cdf = 0.0;

  else

    cdf = 1.0 / ( 1.0 + ( b / ( x - a ) )^c  )^d;

  end

  return
end


% fits quadratic to y = f(x) and outputs yq = f(xq)
% x = nsamps x 1
% y = nsamps x nt
% xq = nsampsq x 1
% yq = nsampsq x nt
function yq = fitQuad(x,y,xq)

X    = [ones(numel(x),1) x x.^2];
XT   = (X'*X)\X';

a    = XT * y;

XQ   = [ones(numel(xq),1) xq xq.^2];
yq   = XQ * a;


function B = barycentric_coordinates(P,varargin)
  % BARYCENTRIC_COORDINATES Computes barycentric coordinates of point p in
  % simplex (v1,v2,v3)
  %
  % B = barycentric_coordinates(P,V1,V2,V3, ...)
  %
  % Inputs:
  %   P  #P by dim list of query point locations
  %   V1  #P by dim list of simplex corner locations
  %   V2  #P by dim list of simplex corner locations
  %   ...
  % Outputs:
  %   B  #P by dim+1 list of barycentric coordinates
  %

  % SHOULD BE USING VARGIN TO ACCEPT ARBITRARY DIMENSION INPUT
  for varg = varargin
    assert(size(P,1) == size(varg{1},1), 'All inputs should be same length');
    assert(size(P,2) == size(varg{1},2), 'All inputs should be same dimension');
  end

  function v = volume(ad,bd,cd)
    r =[bd(:,2).*cd(:,3)-bd(:,3).*cd(:,2), ...
        bd(:,3).*cd(:,1)-bd(:,1).*cd(:,3), ...
        bd(:,1).*cd(:,2)-bd(:,2).*cd(:,1)];
    v = -sum(ad.*r,2)./6;
  end

  n = size(P,1);
  switch numel(varargin)
  case 4
    V1 = varargin{1};
    V2 = varargin{2};
    V3 = varargin{3};
    V4 = varargin{4};
    V1P = V1-P;
    V2P = V2-P;
    V3P = V3-P;
    V4P = V4-P;
    %T = bsxfun(@plus,(1:n)',(0:3)*n);
    A1 = volume(V2P,V4P,V3P);
    A2 = volume(V1P,V3P,V4P);
    A3 = volume(V1P,V4P,V2P);
    A4 = volume(V1P,V2P,V3P);
    A  = volume(V1-V4,V2-V4,V3-V4);
    if size(P,2)>3 && max(abs(sum([A1 A2 A3 A4],2)-A))>1e-14
      warning('Possibly negative coordinates. Not supported in dim~=3');
    end
    B = bsxfun(@rdivide,[A1 A2 A3 A4],A);
  case 3
    V1 = varargin{1};
    V2 = varargin{2};
    V3 = varargin{3};
    A1 = doublearea([ P;V2;V3],[1:n;n+[1:n;n+(1:n)]]');
    A2 = doublearea([V1; P;V3],[1:n;n+[1:n;n+(1:n)]]');
    A3 = doublearea([V1;V2; P],[1:n;n+[1:n;n+(1:n)]]');
    A  = doublearea([V1;V2;V3],[1:n;n+[1:n;n+(1:n)]]');
    if size(P,2)>2 && max(abs(sum([A1 A2 A3],2)-A))>1e-14
      warning('Possibly negative coordinates. Not supported in dim~=2');
    end
    B = bsxfun(@rdivide,[A1 A2 A3],A);
  case 2
    V1 = varargin{1};
    V2 = varargin{2};
    A1 = edge_lengths([P;V2],[1:n;n+[1:n]]');
    A2 = edge_lengths([V1;P],[1:n;n+[1:n]]');
    A = edge_lengths([V1;V2],[1:n;n+[1:n]]');
    if size(P,2)>1 && max(abs(sum([A1 A2],2)-A))>1e-14
      warning('Possibly negative coordinates. Not supported in dim~=1');
    end
    B = bsxfun(@rdivide,[A1 A2],A);
  otherwise
    error(sprintf('%d-simplices not supported',numel(varargin)));
  end
end


% Kernel Normalized Least-Mean-Square algorithm with Coherence Criterion
%
% C. Richard, J.C.M. Bermudez, and P. Honeine, "Online Prediction of Time
% Series Data With Kernels," IEEE Transactions on Signal Processing,
% vol. 57, no. 3, pp. 1058-1067, March 2009,
% http://dx.doi.org/10.1109/TSP.2008.2009895
%
% Remark: memories are initialized empty in this implementation
%
% This file is part of the Kernel Adaptive Filtering Toolbox for Matlab.
% https://github.com/steven2358/kafbox/

classdef knlms < kernel_adaptive_filter

    properties (GetAccess = 'public', SetAccess = 'private') % parameters
        eta = .5; % step size
        mu0 = .95; % coherence criterion threshold
        eps = 1E-2; % regularization
        kerneltype = 'gauss'; % kernel type
        kernelpar = 1; % kernel parameter
    end

    properties (GetAccess = 'public', SetAccess = 'private') % variables
        dict = []; % dictionary
        modict = []; % modulus of the dictionary elements
        alpha = []; % expansion coefficients
    end

    methods
        function kaf = knlms(parameters) % constructor
            if (nargin > 0) % copy valid parameters
                for fn = fieldnames(parameters)'
                    if ismember(fn,fieldnames(kaf))
                        kaf.(fn{1}) = parameters.(fn{1});
                    end
                end
            end
        end

        function y_est = evaluate(kaf,x) % evaluate the algorithm
            if size(kaf.dict,1)>0
                k = kernel(kaf.dict,x,kaf.kerneltype,kaf.kernelpar);
                y_est = k'*kaf.alpha;
            else
                y_est = zeros(size(x,1),1);
            end
        end

        function train(kaf,x,y) % train the algorithm
            if size(kaf.dict,2)==0 % initialize
                k = kernel(x,x,kaf.kerneltype,kaf.kernelpar);
                kaf.dict = x;
                kaf.modict = sqrt(k);
                kaf.alpha = 0;
            else
                k = kernel(kaf.dict,x,kaf.kerneltype,kaf.kernelpar);
                kx = kernel(x,x,kaf.kerneltype,kaf.kernelpar);
                C = k./(sqrt(kx)*kaf.modict); % coherence
                if (max(C) <= kaf.mu0) % coherence criterion
                    kaf.dict = [kaf.dict; x]; % order increase
                    kaf.modict = [kaf.modict; sqrt(kx)];
                    kaf.alpha = [kaf.alpha; 0]; % reserve spot
                end
            end

            k = kernel(kaf.dict,x,kaf.kerneltype,kaf.kernelpar);
            kaf.alpha = kaf.alpha + ... % update coefficients
                kaf.eta / (kaf.eps + k'*k) * (y - k'*kaf.alpha) * k;
        end
    end
end


function [a,t]=v_rotqr2ax(q)
%V_ROTQR2AX converts a real quaternion to the corresponding rotation axis and angle
% Inputs:
%
%     Q(4,1)   real-valued quaternion (with magnitude = 1)
%
% Outputs:
%
%     A(3,1)   Unit vector in the direction of the rotation axis.
%     T        Rotation angle in radians (in range 0 to 2pi)
%
% In the quaternion representation of a rotation, and q(1) = cos(t/2)
% where t is the angle of rotation in the range 0 to 2pi
% and q(2:4)/sin(t/2) is a unit vector lying along the axis of rotation
% a positive rotation about [0 0 1] takes the X axis towards the Y axis.
%
%      Copyright (C) Mike Brookes 2007-2018
%      Version: $Id: v_rotqr2ax.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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
a=q(2:4);
m=sqrt(a'*a);
t=2*atan2(m,q(1));      % avoids problems if unnormalized
if m>0
    a=a/m;
else
    a=[0 0 1]';
end
if ~nargout
    v_rotqr2ro(q); % plot a rotated cube
end


function value = r4_normal_01_cdf_inverse ( p )

%*****************************************************************************80
%
%% R4_NORMAL_01_CDF_INVERSE inverts the standard normal CDF.
%
%  Discussion:
%
%    The result is accurate to about 1 part in 10**7.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 December 2004
%
%  Author:
%
%    Original FORTRAN77 version by Michael Wichura.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Michael Wichura,
%    The Percentage Points of the Normal Distribution,
%    Algorithm AS 241,
%    Applied Statistics,
%    Volume 37, Number 3, pages 477-484, 1988.
%
%  Parameters:
%
%    Input, real P, the value of the cumulative probability densitity function.
%    0 < P < 1.  If P is not in this range, an "infinite" value is returned.
%
%    Output, real VALUE, the normal deviate value with the property that
%    the probability of a standard normal deviate being less than or
%    equal to the value is P.
%
  a = [ 3.3871327179, 50.434271938, 159.29113202, 59.109374720 ];
  b = [ 1.0, 17.895169469, 78.757757664, 67.187563600 ];
  c = [ 1.4234372777, 2.7568153900, 1.3067284816, 0.17023821103 ];
  const1 = 0.180625;
  const2 = 1.6;
  d = [ 1.0, 0.73700164250, 0.12021132975 ];
  e = [ 6.6579051150, 3.0812263860, 0.42868294337, 0.017337203997 ];
  f = [ 1.0, 0.24197894225, 0.012258202635 ];
  split1 = 0.425;
  split2 = 5.0;

  if ( p <= 0.0 )
    value = -r4_huge ( );
    return
  end

  if ( 1.0 <= p )
    value = r4_huge ( );
    return
  end

  q = p - 0.5;

  if ( abs ( q ) <= split1 )

    r = const1 - q * q;
    value = q * r4poly_value ( 4, a, r ) / r4poly_value ( 4, b, r );

  else

    if ( q < 0.0 )
      r = p;
    else
      r = 1.0 - p;
    end

    if ( r <= 0.0 )
      value = -1.0;
      error ( 'R4_NORMAL_CDF_INVERSE - Fatal error!' );
    end

    r = sqrt ( -log ( r ) );

    if ( r <= split2 )

      r = r - const2;

      value = r4poly_value ( 4, c, r ) / r4poly_value ( 3, d, r );

    else

      r = r - split2;

      value = r4poly_value ( 4, e, r ) / r4poly_value ( 3, f, r );

    end

    if ( q < 0.0 )
      value = -value;
    end

  end

  return
end


function DecayMatrix = getDecayMatrix(EchoTimes,T2vals)
    DecayMatrix = zeros(length(EchoTimes),length(T2vals));
    for j = 1:length(T2vals)
        DecayMatrix(:,j) = exp(-EchoTimes/T2vals(j))';
    end
end


function [q] = rotMatToQuat(R)

[r,c] = size( R );
R=R';

if( r ~= 3 | c ~= 3 )
    fprintf( 'R must be a 3x3 matrix\n\r' );
    return;
end

Rxx = R(1,1); Rxy = R(1,2); Rxz = R(1,3);
Ryx = R(2,1); Ryy = R(2,2); Ryz = R(2,3);
Rzx = R(3,1); Rzy = R(3,2); Rzz = R(3,3);

w = sqrt( trace( R ) + 1 ) / 2;

% check if w is real. Otherwise, zero it.
if( imag( w ) > 0 )
     w = 0;
end

x = sqrt( 1 + Rxx - Ryy - Rzz ) / 2;
y = sqrt( 1 + Ryy - Rxx - Rzz ) / 2;
z = sqrt( 1 + Rzz - Ryy - Rxx ) / 2;

[~, i ] = max( [w,x,y,z] );

if( i == 1 )
    x = ( Rzy - Ryz ) / (4*w);
    y = ( Rxz - Rzx ) / (4*w);
    z = ( Ryx - Rxy ) / (4*w);
end

if( i == 2 )
    w = ( Rzy - Ryz ) / (4*x);
    y = ( Rxy + Ryx ) / (4*x);
    z = ( Rzx + Rxz ) / (4*x);
end

if( i == 3 )
    w = ( Rxz - Rzx ) / (4*y);
    x = ( Rxy + Ryx ) / (4*y);
    z = ( Ryz + Rzy ) / (4*y);
end

if( i == 4 )
    w = ( Ryx - Rxy ) / (4*z);
    x = ( Rzx + Rxz ) / (4*z);
    y = ( Ryz + Rzy ) / (4*z);
end

q = [x; y; z; w];

end


function angle = Compute_Angle(xy_long)

x1 = xy_long(:, 1);
y1 = xy_long(:, 2);
K1 = -(y1(2)-y1(1))/(x1(2)-x1(1));
angle = atan(K1)*180/pi;


function value = givens_determinant ( n )

%*****************************************************************************80
%
%% GIVENS_DETERMINANT returns the determinant of the GIVENS matrix.
%
%  Discussion:
%
%    Since a formula for the eigenvalues is known, we compute the
%    determinant as the product of those values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real VALUE, the determinant.
%
  value = 1.0;

  for i = 1 : n
    angle = ( 2 * i - 1 ) * pi / ( 4 * n );
    value = value * 0.5 / ( cos ( angle ) )^2;
  end

  return
end


function index = randIndex(maxIndex,len)
%INDEX = RANDINDEX(MAXINDEX,LEN)
%   randomly, non-repeatedly select LEN integers from 1:MAXINDEX

if len > maxIndex
	index = [];
	return
end

index = zeros(1,len);
available = 1:maxIndex;
rs = ceil(rand(1,len).*(maxIndex:-1:maxIndex-len+1));
for p = 1:len
	while rs(p) == 0
		rs(p) = ceil(rand(1)*(maxIndex-p+1));
	end
	index(p) = available(rs(p));
	available(rs(p)) = [];
end


function [k,v]=findpeaks(x,m,w)
%FINDPEAKS finds peaks with optional quadratic interpolation [K,V]=(X,M,W)
%
%  Inputs:  X        is the input signal (does not work with UInt datatype)
%           M        is mode:
%                       'q' performs quadratic interpolation
%                       'v' finds valleys instead of peaks
%           W        is the width tolerance; a peak will be eliminated if there is
%                    a higher peak within +-W samples
%
% Outputs:  K        are the peak locations in X (fractional if M='q')
%           V        are the peak amplitudes: if M='q' the amplitudes will be interpolated
%                    whereas if M~='q' then V=X(K).

% Outputs are column vectors regardless of whether X is row or column.
% If there is a plateau rather than a sharp peak, the routine will place the
% peak in the centre of the plateau. When the W input argument is specified,
% the routine will eliminate the lower of any pair of peaks whose separation
% is <=W; if the peaks have exactly the same height, the second one will be eliminated.
% All peak locations satisfy 1<K<length(X).
%
% If no output arguments are specified, the results will be plotted.
%

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

if nargin<2
    m=' ';
end
nx=length(x);
if any(m=='v')
    x=-x(:);        % invert x if searching for valleys
else
    x=x(:);        % force to be a column vector
end
dx=x(2:end)-x(1:end-1);
r=find(dx>0);
f=find(dx<0);

if length(r)>0 & length(f)>0    % we must have at least one rise and one fall
    dr=r;
    dr(2:end)=r(2:end)-r(1:end-1);
    rc=repmat(1,nx,1);
    rc(r+1)=1-dr;
    rc(1)=0;
    rs=cumsum(rc); % = time since the last rise

    df=f;
    df(2:end)=f(2:end)-f(1:end-1);
    fc=repmat(1,nx,1);
    fc(f+1)=1-df;
    fc(1)=0;
    fs=cumsum(fc); % = time since the last fall

    rp=repmat(-1,nx,1);
    rp([1; r+1])=[dr-1; nx-r(end)-1];
    rq=cumsum(rp);  % = time to the next rise

    fp=repmat(-1,nx,1);
    fp([1; f+1])=[df-1; nx-f(end)-1];
    fq=cumsum(fp); % = time to the next fall

    k=find((rs<fs) & (fq<rq) & (floor((fq-rs)/2)==0));   % the final term centres peaks within a plateau
    v=x(k);

    if any(m=='q')         % do quadratic interpolation
        b=0.5*(x(k+1)-x(k-1));
        a=x(k)-b-x(k-1);
        j=(a>0);            % j=0 on a plateau
        v(j)=x(k(j))+0.25*b(j).^2./a(j);
        k(j)=k(j)+0.5*b(j)./a(j);
        k(~j)=k(~j)+(fq(k(~j))-rs(k(~j)))/2;    % add 0.5 to k if plateau has an even width
    end

    % now purge nearby peaks

    if nargin>2
        j=find(k(2:end)-k(1:end-1)<=w);
        while any(j)
            j=j+(v(j)>=v(j+1));
            k(j)=[];
            v(j)=[];
            j=find(k(2:end)-k(1:end-1)<=w);
        end
    end
else
    k=[];
    v=[];
end
if any(m=='v')
    v=-v;    % invert peaks if searching for valleys
end
if ~nargout
    if any(m=='v')
        x=-x;    % re-invert x if searching for valleys
        ch='v';
    else
        ch='^';
    end
    plot(1:nx,x,'-',k,v,ch);
end


function determ = conference_determinant ( n )

%*****************************************************************************80
%
%% CONFERENCE_DETERMINANT returns the determinant of the CONFERENCE matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.  N-1 must be an odd prime,
%    or a power of an odd prime.
%
%    Output, real DETERM, the determinant.
%
  if ( mod ( n - 1, 4 ) == 1 )
    determ = - sqrt ( ( n - 1 )^n );
  else
    determ = + sqrt ( ( n - 1 )^n );
  end

  return
end


%
% Polynomial subtraction  h = f - g
%
% Syntax:  >> h = PolynomialPlus(f,g)
%     or:  >> h = pplus(f,g)
%
%   Input:    f --- (string or numeric) polynomial one
%             g --- (string or numeric) polynomial two
%
%  Output:    h --- (string) difference f-g
%
%  Example:  >> PolynomialMinus('5*x^2*y+8','3*x^2*y+2')
%
%            ans =
%
%            6 + 2*x^2*y


function [x,fVal,exitCode]=divRectOpt(f,lowerBounds,upperBounds,options)
%%DIVRECTOPT The dividing rectangles optimization algorithm. This is a
%           derivative-free global optimization algorithm where the
%           multidimensional search space can be bounded. This algorithm
%           tries to minimize a given function of a vector parameter.
%
%INPUTS: f A handle to the function over which the minimization is to be
%          performed. The function [fVal,violatesConstraints]=f(x) takes
%          the NX1 xvector and returns the real scalar function value
%          fVal. The parameter violatesConstraints is used to mark
%          forbidden areas, for example, where the function might not be
%          defined. violatesConstraints=false if there are not constraint
%          violations and it is true if there is a constraint violation.
% lowerBounds,upperBounds NX1 vectors of lower bounds defining lower and
%          upper bounds on the components of x at the optimal points. This
%          defines the search space. The elements must be finite and
%          cannot be NaNs.
%  options A structure where all elements are optional. The elements set
%          options that affect the algorithm and its convergence. Default
%          values are used if a field is missing. Possible fields in the
%          structure are:
%          algorithm (default 0) This selects which of the two direct
%                    optimization algorithms should be posed. Possible
%                    values are
%                    0: Use the algorithm of [1], which is probably
%                       better for functions with many local minima.
%                    1: Use the algorithm of [2], which favors local
%                       search and thus might be better for functions
%                       with few local minima.
%             maxDiv (default 5000) The maximum number of divisions of
%                    the hyperrectangles allowed. This must be >0.
%            maxIter (default 100) The maximum number of iterations.
%           maxFEval (default 500) The maximum number of function
%                     evaluations.
%            epsilon (default 1e-4) The Jones' factor. It is the epsilon
%                    term in Definitions 3.1 and 4.1 in [1]. It affects
%                    the convergence rate of the algorithm by avoiding
%                    oversampling near points with low function values
%                    and biasing the sampling towards a global search.
%                    If a positive value of epsilon is specified, then
%                    the same value is used for all iterations. In [1],
%                    it is suggested that epsilon be set to the desired
%                    solution accuracy (in f) for good convergence,
%                    though other convergence conditions are discussed in
%                    [2]. If a negative value is specified, then epsilon
%                    is adaptively changed using the formula
%                    epsilon = max(1.D-4*abs(fMin),abs(epsilon)).
%                    The magnitude of epislon must be between 0 and 1,
%                    not inclusive of 1, though 0 tends to work.
%         epsilonAbs (default 0) Definitions 3.1 and 4.1 in [1] involve
%                    comparisons to fMin-epsilon*abs(fMin), where fMin is
%                    the current minimum function value found. If
%                    epsilonAbs is nonzero, then the comparison is
%                    actually taken with respect to
%                    min(fMin-epsilon*abs(fMin),epsilonAbs)
%                    epsilonAbs must be positive.
%       volumeRelTol (default 1e-10) Convergence is declared if the
%                    volume of the search region is this fraction of the
%                    original volume (must be between 0 and 1)
%        sigmaRelTol (default 0) Convergence is declared if the hypercube
%                    measure is smaller than this value. The notion of
%                    the hypercube measure sigma(S) is discussed in [2].
%            fGlobal If the actual minimum value of f is known, even
%                    though the value of x providing that minimum is
%                    unknown, then it can be specified. Otherwise, this
%                    parameter is not used.
%      fGlobalRelTol (default 1e-12 if fGlobal is provided) This is only
%                    used if fGlobal is provided. Convergence is declared
%                    if
%                    (fMin - fGlobal)/max(1,abs(fGlobal)) < fGlobalRelTol
%
%OUTPUTS: x The NX1 optimal point found.
%      fVal The value of the function at the optimal point found.
%  exitCode A return value indicating the status of the algorithm on
%          termination. Possible values are:
%          1 Function terminated, because  the number of function
%            evaluations done is larger than maxFEval.
%          2 Function terminated, because the number of iterations equals
%            maxIter.
%          3 The function value found is within fGlobalRelTol of the
%            global optimum given in fGlobal.
%          4 The volume of the hyperrectangle with fVal at its center is
%            less than volumeRelTol times the volume of the original
%            hyperrectangle.
%          5 The measure of the hyperrectangle with fVal at its center is
%            less than sigmaRelTol.
%
%Global optimization algorithms such as this are very good at handling
%functions with many local minima. Often, one might want to use a global
%optimization algorithm to get an initial estimate close to the global
%solution and then do a few iterations of, for example, Newton's method,
%to find the global optimum.
%
%This function is an interface to the DIRECT library that is part of the
%NLOpt set of nonlinear optimization routines available at [4], which
%implements [1] and [2].  Though the aforementioned code online provides
%a Matlab interface, it was decided to include the dividing rectangles
%algorithm without the rest of the library, in part because some of the
%other functions include weak copyleft provisions in the form of the LGPL.
%Additionally, the implementation above did not allow one to specify the
%maximum number of hyperrectangle divisions (it was statically fixed) nor
%did it use the memory allocation routines in Matlab that mex files are
%supposed to (but do not have to) use. Thus, the code was slightly
%modified (but is still included with the rest of the third party code).
%
%Though [1] and [2] use a number of exmaple, functions, they formulae are
%not explicitely given. Rather, one must consult a few references, which
%can be tedious to obtain. On the other hand, numrous exmaples functions
%used in [1] are given explicitely in [3]. Examples of the algorithm are
%thus:
% %Branin's RCOS function
% %Note that the deal function is used to make an anonymous function have
% %two outputs.
% f=@(x)deal((x(2)-(5/(4*pi^2))*x(1)^2+(5/pi)*x(1)-6)^2+10*(1-(1/(8*pi)))*cos(x(1))+10,0);
% lowerBounds=[-5;10];
% upperBounds=[0;15];
% [x,fVal,exitCode]=divRectOpt(f,lowerBounds,upperBounds)
%The globally optimal value is given in [3] as 0.397887357729739. The
%returned fVal is close.
%
% %Six-Hump Camel
% f=@(x)deal((x(1)^2*(4-2.1*x(1)^2+x(1)^4/3)+x(1)*x(2)+x(2)^2*(-4+4*x(2)^2)),0);
% lowerBounds=[-3;-2];
% upperBounds=[3;2];
% [x,fVal,exitCode]=divRectOpt(f,lowerBounds,upperBounds)
%The globally optimal value is given in [3] as -1.0316284535. The returned
%fVal is close.
%
% %Two-Dimensional Schubert
% f=@(x)deal(sum((1:5).*cos(((1:5)+1)*x(1)+(1:5)))*sum((1:5).*cos(((1:5)+1)*x(2)+(1:5))),0)
% lowerBounds=[-10;-10];
% upperBounds=[10;10];
% options.maxFEval=5000;
% options.maxIter=200;
% [x,fVal,exitCode]=divRectOpt(f,lowerBounds,upperBounds,options)
%Here, increasing the number of function evaluations and iterations is
%important to getting a solution that is close to the optimal value as
%this function has 760 local minima (and 18 global minima). The global
%minimum is -186.730908831024 as per [3].
%
%REFERENCES:
%[1] D. R. Jones, C. D. Peritunen, and B. E. Stuckman, "Lipschitzian
%    optimization without the Lipschitz constant," Journal of Optimization
%    Theory and Application, vol. 79, no. 1, pp. 157-181, Oct. 1993.
%[2] J. M. Gablonsky and C. T. Kelley, "A locally-biased form of the
%    DIRECT algorithm," Journal of Global Optimization, vol. 21, no. 1,
%    pp. 27-37, Sep. 2001.
%[3] M. Bj�kman and K. Holmstr�m, "Global optimization using the DIRECT
%    algorithm in Matlab," The Electronic International Journal Advanced
%    Modeling and Optimization, vol. 1, no. 2, pp. 17-37, 1999.
%    [Online]. Available: http://camo.ici.ro/journal/v1n2.htm
%[4] S. G. Johnson. (2014, 20 May) The NLopt nonlinear-optimization
%    package. [Online]. Available: http://ab-initio.mit.edu/nlopt
%
%October 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

error('This function is only implemented as a mexed C or C++ function. Please run CompileCLibraries.m to compile the function for use.')

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


%LAPLACIAN_PERRINX   Compute surface Laplacian of EEG data.
% [surf_lap,G,H] = laplacian_perrinX(data,x,y,z[,leg_order,smoothing]);
%
% INPUTS    :
%      data : EEG data (can be N-D, but first dimension must be electrodes)
%     x,y,z : x,y,z coordinates of electrode positions (e.g., [EEG.chanlocs.X])
%
% (optional inputs)
% leg_order : order of Legendre polynomial (default is 20 [40 for >100 electrodes])
% smoothing : G smoothing parameter (lambda), set to 1e-5 by default
%
%
% OUTPUTS   :
%  surf_lap : the surface Laplacian (second spatial derivative)
% (optional outputs)
%       G,H : G and H matrices
%
%  This is an implementation of algorithms described by
%    Perrin, Pernier, Bertrand, and Echallier (1989). PubMed #2464490

% mikexcohen@gmail.com

function [surf_lap,G,H] = laplacian_perrinX(data,x,y,z,varargin) % vararg order: leg_order,smoothing

numelectrodes = numel(x);

if nargin<4
    help laplacian_perrinX
    error('Read help file!')
end

%% compute G and H matrices

% initialize
G=zeros(numelectrodes);
H=zeros(numelectrodes);
cosdist=zeros(numelectrodes);

% default parameters for +/- 100 electrodes
if numelectrodes>100
    m=3; leg_order=40;
else
    m=4; leg_order=20;
end

if numel(varargin)>0 && ~isempty(varargin{1})
    leg_order=varargin{1};
end

% scale XYZ coordinates to unit sphere
[junk,junk,spherical_radii] = cart2sph(x,y,z);
maxrad = max(spherical_radii);
x = x./maxrad;
y = y./maxrad;
z = z./maxrad;

for i=1:numelectrodes
    for j=i+1:numelectrodes
        cosdist(i,j) = 1 - (( (x(i)-x(j))^2 + (y(i)-y(j))^2 + (z(i)-z(j))^2 ) / 2 );
    end
end
cosdist = cosdist+cosdist' + eye(numelectrodes);

% compute Legendre polynomial
legpoly = zeros(leg_order,numelectrodes,numelectrodes);
for ni=1:leg_order
    temp = legendre(ni,cosdist);
    legpoly(ni,:,:) = temp(1,:,:);
end

% precompute electrode-independent variables
twoN1  = 2*(1:leg_order)+1;
gdenom = ((1:leg_order).*((1:leg_order)+1)).^m;
hdenom = ((1:leg_order).*((1:leg_order)+1)).^(m-1);

for i=1:numelectrodes
    for j=i:numelectrodes

        g=0; h=0;

        for ni=1:leg_order
            % compute G and H terms
            g = g + (twoN1(ni)*legpoly(ni,i,j)) / gdenom(ni);
            h = h - (twoN1(ni)*legpoly(ni,i,j)) / hdenom(ni);
        end
        G(i,j) =  g/(4*pi);
        H(i,j) = -h/(4*pi);
    end
end

% mirror matrix
G=G+G'; H=H+H';

% correct for diagonal-double
G = G-eye(numelectrodes)*G(1)/2;
H = H-eye(numelectrodes)*H(1)/2;

%% compute laplacian

% reshape data to electrodes X time/trials
orig_data_size = squeeze(size(data));
if any(orig_data_size==1)
    data=data(:);
else
    data = reshape(data,orig_data_size(1),prod(orig_data_size(2:end)));
end

% smoothing constant
if numel(varargin)==2
    smoothing=varargin{2};
else
    smoothing=1e-5;
end

% add smoothing constant to diagonal
% (change G so output is unadulterated)
Gs = G + eye(numelectrodes)*smoothing;

% compute C matrix
GsinvS = sum(inv(Gs));
dataGs = data'/Gs;
C      = dataGs - (sum(dataGs,2)/sum(GsinvS))*GsinvS;

% compute surface Laplacian (and reshape to original data size)
surf_lap = reshape((C*H')',orig_data_size);

%% end


        	% Stergerea spatiul de lucru si a figurii curente
	clear; clf;
			% Introducerea punctelor definite
			% -	vectorul x cu abscisele punctelor
	x=0:5;
            % -	vectorul y cu ordonatele punctelor
	y=[0 0.5 4.5 0 2 0];
			% Generarea vectorului xx cu pasul fin
	xx=0:0.1:5;
            % Reprezentarea grafica cu subplot-uri
	h=subplot(2,2,1)
            % Precizarea positiei si marimii celulei de figura
    set(h,'position',[0.05,0.55,0.4,0.4])
            % Se reprezinta cu *-uri negre punctele date
	plot(x,y,'ko');
	hold on
		    % Interpolarea liniara
            % si reprezentarea functiilor obtinute
	yy=interp1(x,y,xx,'linear');
	plot(xx,yy,'r','LineWidth',1.5);
            % Personalizrea reprezentarii grafice
	grid on;
	title('LINEAR');

		    % Interpolarea de tip nearest
	h=subplot(2,2,2)
	set(h,'position',[0.5,0.55,0.4,0.4])
    plot(x,y,'ko');
	hold on
	yy=interp1(x,y,xx,'nearest');
	plot(xx,yy,'g','LineWidth',1.5);
	grid on;
	title('NEAREST');

			% Interpolarea de tip spline
	h=subplot(2,2,3)
    set(h,'position',[0.05,0.05,0.4,0.4])
 	plot(x,y,'ko');
	hold on
	yy=interp1(x,y,xx,'spline');
	plot(xx,yy,'m','LineWidth',1.5);
	grid on;
	title('SPLINE')

            % Interpolarea de tip cubic
	h=subplot(2,2,4)
    set(h,'position',[0.5,0.05,0.4,0.4])
    plot(x,y,'ko');
	hold on
	yy=interp1(x,y,xx,'cubic');
	plot(xx,yy,'b','LineWidth',1.5);
	grid on;
	title('CUBIC')


function [J, grad] = linearRegCostFunction(X, y, theta, lambda)
%LINEARREGCOSTFUNCTION Compute cost and gradient for regularized linear
%regression with multiple variables
%   [J, grad] = LINEARREGCOSTFUNCTION(X, y, theta, lambda) computes the
%   cost of using theta as the parameter for linear regression to fit the
%   data points in X and y. Returns the cost in J and the gradient in grad

% Initialize some useful values
m = length(y); % number of training examples

% You need to return the following variables correctly
J = 0;
grad = zeros(size(theta));

% ====================== YOUR CODE HERE ======================
% Instructions: Compute the cost and gradient of regularized linear
%               regression for a particular choice of theta.
%
%               You should set J to the cost and grad to the gradient.
%
tempTheta = theta;
tempTheta(1) = 0;

J = (1 / (2*m) ) * sum(((X * theta)-y).^2) + (lambda / (2 * m))*sum(tempTheta.^2);
temp = X * theta;
error = temp - y;
grad = (1 / m) * (X' * error) + (lambda/m)*tempTheta;

% =========================================================================

grad = grad(:);

end


clear, close all
        % Determinarea numarului de noduri pe unitatea de lungime
nrnod=50;
        % Generarea unei retele pe domeniul considerat
[x,y]=meshgrid(0:1/(nrnod-1):2);
        % Determinarea numarului de noduri pe o linoe
N=length(x);
        % Initializarea matricelor V si Vp
V=zeros(N); Vp=V;
        % Fixarea erorii maxime impuse
erormax=0.0001;
        % Initializarea contorului de cicluri
nrp=1;
        % Ciclul repetitiv de calcul al potentialului
while nrp<3 | abs(max(max(V(2:N-1,2:N-1)-...
        Vp(2:N-1,2:N-1))))>erormax
        % - memorarea valorilor de la iteratia precedenta
    Vp=V;
        % - recalcularea potentialului in nodurile considerate
    V=V+del2(V);
	    % - impunerea conditiilor de frontiera
    V(N,:)=0; V(:,1)=0; V(:,N)=0; V(1,:)=1;
        % - incremenatarea contorului de cicluri
	nrp=nrp+1;
end;
        % Reprezentarea liniilor echipotentiale selectate
[c,h]=contour(x,y,V,[1,0.8,0.4,0.25,0.005]);
        % Etichetarea liniilor echipotentiale
clabel(c,h)
        % Setarea axelor patrate si personalizarea graficii
axis square
grid
xlabel('x')
ylabel('y')


function [xs,ys] = plot_gmm_1D(Priors,Mu,Sigmas)
%PLOT_GMM Summary of this function goes here
%   Detailed explanation goes here

D = size(Mu,1);
K = size(Mu,2);

[Y,I] = max(Mu);
Max = Y + 4.0*sqrt(Sigmas(I));
[Y,I] = min(Mu);
Min = Y - 4.0*sqrt(Sigmas(I));
nbSamples = 401;
xs=linspace(Min,Max, nbSamples);
ys = zeros(1,nbSamples);

for k=1:K
    ys=ys+(Priors(k).*gaussPDF(xs,Mu(k),Sigmas(k)))';
end

end


%  Figure 3.44      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
%  Example 3.35
clf;
t=[0 .1 .2 .3 .4 .5 1 1.5 2 2.5 3 4 10];
y=[0 .005 .034 .085 .140 .215 .510 .7 .817 .890 .932 .975 .9999];
dy=ones(1,13)-y;
A=-1.37;     % adjust A & alpha to get best fit
alpha=1.;     % to the data by trial and error
axis([0 6 -2 1]);
fity=-A*exp(-alpha*t);
dyl=log10(dy);
fityl=log10(fity);
plot(t,dyl,'o',t,fityl,'-');
title('Fig. 3.44 Plot of log10[y(\infty) - y(t)] versus t');
ylabel('log|1 - y(t)|');
xlabel('Time (sec)');
nicegrid;


disp('Fitting AR model to AR-5 process data with known coefficients');

secs=5;
ns=128;
t=[1/ns:1/ns:secs];
N=length(t);
noise_var=1^2;
noise=sqrt(noise_var)*randn(1,N);
a_true=[-1.8517,1.3741,0.1421,-0.6852,0.3506];
y=filter(1,[1,a_true],noise);
y=y(1:N);
figure
plot(t,y);
xlabel('Seconds');
title('Sample of AR-5 process');

total_variance=std(y)^2;
signal_variance=total_variance-noise_var;
snr=sqrt(signal_variance)/sqrt(noise_var);
disp(sprintf('SNR=%1.3f',snr));

disp('For model order p=5');
ar=spm_ar (y,5);

disp(' ');
disp('True coefficients');
disp(a_true);

disp(' ');
disp('Estimated coefficients');
disp(ar.a_mean');

for p=1:10,
    disp(sprintf('Now fitting model with p=%d coefficients',p));
    ar=spm_ar (y,p,0);
    logev(p)=ar.fm;
end
logev=logev-min(logev);
figure
bar(logev);
ylabel('Log Evidence');
xlabel('Model order');


function [fittedCircle, circleNormal] = fitCircle3d(pts)
%FITCIRCLE3D Fit a 3D circle to a set of points.
%
%   [FITTEDCIRCLE, CIRCLENORMAL] = fitCircle3d(PTS)
%
%   Example
%     % points on a 2d circle with noise
%     nop = randi([5 50],1,1);
%     radius = randi([5 25],1,1);
%     points2d = circleToPolygon([0 0 radius], nop);
%     points2d(1,:) = [];
%     points2d = points2d + rand(size(points2d));
%     points2d(:,3)=rand(length(nop),1);
%     % apply random rotation and translation
%     [theta, phi] = randomAngle3d;
%     theta = rad2deg(theta);
%     phi = rad2deg(phi);
%     tfm = eulerAnglesToRotation3d(phi, theta, 0);
%     trans = randi([-250 250],3,1);
%     tfm(1:3,4)=trans;
%     points3d = awgn(transformPoint3d(points2d, tfm),1);
%     % fit 3d circle
%     [fittedCircle, circleNormal] = fitCircle3d(points3d);
%     % plot 3d points and 3d circle
%     figure('Color','w'); hold on; axis equal tight; view(3);
%     xlabel('X');ylabel('Y');zlabel('Z');
%     drawPoint3d(points3d)
%     drawCircle3d(fittedCircle, 'k')
%     drawVector3d(fittedCircle(1:3), circleNormal*fittedCircle(4))
%
%   See also
%   circle3dOrigin, circle3dPosition, circle3dPoint, intersectPlaneSphere
%   drawCircle3d, drawCircleArc3d, drawEllipse3d
%
% ------
% Authors: oqilipo, David Legland
% created: 2017-05-09

% Mean of all points
meanPoint = mean(pts,1);

% Center points by subtracting the meanPoint
centeredPoints = pts - repmat(meanPoint,size(pts,1),1);

% Project 3D data to a plane
[~,~,V]=svd(centeredPoints);
tfmPoints = transformPoint3d(centeredPoints, V');

% Fit a circle to the points in the xy-plane
circleParamter = CircleFitByTaubin(tfmPoints(:,1:2));
center2d = circleParamter(1:2);
radius=circleParamter(3);
center3d = transformPoint3d([center2d, 0], [inv(V'), meanPoint']);
circleNormal = V(:,3)';
[theta, phi, ~] = cart2sph2(circleNormal);
fittedCircle = [center3d radius rad2deg(theta) rad2deg(phi) 0];

end

% Circle Fit (Taubin method)
% version 1.0 (2.24 KB) by Nikolai Chernov
% http://www.mathworks.com/matlabcentral/fileexchange/22678
function Par = CircleFitByTaubin(XY)

%--------------------------------------------------------------------------
%
%     Circle fit by Taubin
%      G. Taubin, "Estimation Of Planar Curves, Surfaces And Nonplanar
%                  Space Curves Defined By Implicit Equations, With
%                  Applications To Edge And Range Image Segmentation",
%      IEEE Trans. PAMI, Vol. 13, pages 1115-1138, (1991)
%
%     Input:  XY(n,2) is the array of coordinates of n points x(i)=XY(i,1), y(i)=XY(i,2)
%
%     Output: Par = [a b R] is the fitting circle:
%                           center (a,b) and radius R
%
%     Note: this fit does not use built-in matrix functions (except "mean"),
%           so it can be easily programmed in any programming language
%
%--------------------------------------------------------------------------

n = size(XY,1);      % number of data points

centroid = mean(XY);   % the centroid of the data set

%     computing moments (note: all moments will be normed, i.e. divided by n)

Mxx = 0; Myy = 0; Mxy = 0; Mxz = 0; Myz = 0; Mzz = 0;

for i=1:n
    Xi = XY(i,1) - centroid(1);  %  centering data
    Yi = XY(i,2) - centroid(2);  %  centering data
    Zi = Xi*Xi + Yi*Yi;
    Mxy = Mxy + Xi*Yi;
    Mxx = Mxx + Xi*Xi;
    Myy = Myy + Yi*Yi;
    Mxz = Mxz + Xi*Zi;
    Myz = Myz + Yi*Zi;
    Mzz = Mzz + Zi*Zi;
end

Mxx = Mxx/n;
Myy = Myy/n;
Mxy = Mxy/n;
Mxz = Mxz/n;
Myz = Myz/n;
Mzz = Mzz/n;

%    computing the coefficients of the characteristic polynomial

Mz = Mxx + Myy;
Cov_xy = Mxx*Myy - Mxy*Mxy;
A3 = 4*Mz;
A2 = -3*Mz*Mz - Mzz;
A1 = Mzz*Mz + 4*Cov_xy*Mz - Mxz*Mxz - Myz*Myz - Mz*Mz*Mz;
A0 = Mxz*Mxz*Myy + Myz*Myz*Mxx - Mzz*Cov_xy - 2*Mxz*Myz*Mxy + Mz*Mz*Cov_xy;
A22 = A2 + A2;
A33 = A3 + A3 + A3;

xnew = 0;
ynew = 1e+20;
epsilon = 1e-12;
IterMax = 20;

% Newton's method starting at x=0

for iter=1:IterMax
    yold = ynew;
    ynew = A0 + xnew*(A1 + xnew*(A2 + xnew*A3));
    if abs(ynew) > abs(yold)
       disp('Newton-Taubin goes wrong direction: |ynew| > |yold|');
       xnew = 0;
       break;
    end
    Dy = A1 + xnew*(A22 + xnew*A33);
    xold = xnew;
    xnew = xold - ynew/Dy;
    if (abs((xnew-xold)/xnew) < epsilon), break, end
    if (iter >= IterMax)
        disp('Newton-Taubin will not converge');
        xnew = 0;
    end
    if (xnew<0.)
        fprintf(1,'Newton-Taubin negative root:  x=%f\n',xnew);
        xnew = 0;
    end
end

%  computing the circle parameters

DET = xnew*xnew - xnew*Mz + Cov_xy;
Center = [Mxz*(Myy-xnew)-Myz*Mxy , Myz*(Mxx-xnew)-Mxz*Mxy]/DET/2;

Par = [Center+centroid , sqrt(Center*Center'+Mz)];

end    %    CircleFitByTaubin


function stats = optMyopic(returns, gamma, rf, accuracy)
%OPTMYOPIC: Calculate the optimal portfolio weight through the Myopic
%strategy or alternatively the Buy-and-Hold strategy
%
%   STATS = OPTMYOPIC(returns, ...) retrieves the optimal portfolio weight
%   using the myopic strategy. A grid search is used, based on the input
%   variable 'returns'. At any grid point, the conditional expectation is
%   calculated. In the end, the portfolio weight with the highest
%   conditional expectation is selected.
%   Alternatively, one can use this function to calculate the optimal
%   portfolio with the buy-and-hold strategy. For an investor with
%   investment horizon of 'K' periods, define the cumulative returns over
%   the past 'K' periods and use K*rf instead of rf.
%
%   Note that it is optimal to invest myopically when there are not state
%   variables and when the future states and returns are independent
%   (conditional on the state variables), in continuous time. Moreover,
%   long-term investors should invest myopically when returns are identical
%   and independent distributed.
%
%   'returns'	:   is a 'n-by-1' vector. One could use realized returns or
%                   simulated path returns (see also example).
%
%   The following assumptions are relevant:
%   -   A tradeoff is made between the input 'returns' and the risk free rate 'rf'
% 	-   The input consists only of the (simulated) returns.
%   -   Weights are restricted between 0 and 1.
%
%   STATS = OPTMYOPIC(returns, 'gamma', ...) allows you to specify an own
%   risk aversion parameter 'gamma'.
%       'gamma'   :     risk-aversion constant, initially set to 5.
%
%   STATS = OPTMYOPIC(returns, gamma, 'rf', ...) allows you to specify the
%   risk free rate, relevant for your optimization problem.
%       'rf'      :     the risk free rate constant, initial set to 0.
%
%   STATS = OPTMYOPIC(returns, gamma, rf, 'accuracy') allows you to extend
%   the accuracy of the grid, initially set to 0.01.
%       'accuracy':     the single step length in the grid
%
%   STATS = OPTMYOPIC(...) returns the structure 'stats' consisting of:
%       'stats.optWeight'   :  the optimal portfolio weight (in risky asset)
%       'stats.optCU'       :  optimal conditional expectation
%
%   NB:     -   For three assets we need a two dimensional grid, which
%               leads appearantly to the curse of dimensionality. There are
%               methods that solve this problem, however such solutions are
%               beyond the scope of this code.
%
%  Semin Ibisevic (2012)
%  $Date: 02/05/2012$
%
% -------------------------------------------------------------------------
% References
%
% B.F. Diris. Portfolio Management. Econometric Institute, 2012. Lecture
% FEM21010.
% -------------------------------------------------------------------------

[n, m] = size(returns);
if m >= n,  error('optMyopic:InvalidInput','The number of rows should be sufficiently larger than the number of columns'); end
if m > 1,   error('optMyopic:InvalidInput','The input should consist only of one column. Note: to calculate the portfolio weights using the Buy-and-Hold strategy, use cumulative returns over the past k periods instead.'); end

if nargin < 1, error('optMyopic:InsufficientParameters','Input variable is necessary'); end
if nargin < 2, gamma = 5; end   % default risk aversion
if nargin < 3, rf = 0; end      % default risk free rate
if nargin < 4, accuracy = 0.01; end

% One-dimensional grid
grid = 0.00:accuracy:1.00;

% init
U = zeros(n,size(grid,2));
CU = zeros(1,size(grid,2));
CUopt = -10^(9);
gridopt = 0;

% -------------------------------------------------------------------------

% pick portfolio weight from the grid
wIter = 1;
for w = grid

   % define the realized utility value for all i = 1,...,n
   U(:,wIter) = (1/(1-gamma)) * ( w*exp( returns ) + (1-w)*exp( rf ) ).^(1-gamma);

   % approximate the conditional expected utlity for grid point w
   CU(wIter) = (1/n)*sum( U(:,wIter) );

   % update
   if CU(wIter) > CUopt
       CUopt = CU(wIter);
       gridopt = w;
   end

   wIter = wIter + 1;

end

stats.optWeight = gridopt;
stats.optCU = CUopt;


function peakVals = ode_fun ( bVals, kVals )

%*****************************************************************************80
%
%% ODE_FUN finds peak values of an ODE over a mesh of parameter values.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 March 2011
%
%  Author:
%
%    John Burkardt.
%
%  Parameters:
%
%    Input, real bVals(M), a vector of B values.
%
%    Input, real kVals(N), a vector of K values.
%
%    Output, real peakVals(M,N), a vector containing the maximum value of
%    the ODE solution over a fixed time interval, for every combination
%    of B and K values.
%

%
%  Form a grid of all pairs of K and B:
%
  [ kGrid, bGrid ] = meshgrid ( bVals, kVals );
%
%  Define an array to hold the results, and initialize it to NAN.
%
  peakVals = nan ( size ( kGrid ) );
%
%  Solve the ODE for every pair of K and B values.  (M is fixed at 5.)
%
  m = 5.0;

  for ij = 1 : numel(kGrid)
%
%  Solve the ODE over the time interval 0 <= T <= 25, with
%  initial conditions X(0) = 0, X'(0) = 1.
%
    [ T, Y ] = ode45 ( @(t,y) ode_system ( t, y, m, bGrid(ij), kGrid(ij) ), ...
      [0, 25],  [0, 1] );
%
%  Retrieve the maximum value achieved by this solution.
%
    peakVals(ij) = max ( Y(:,1) );

  end

  return
end


function [min_width, min_angle] = minimumCaliperDiameter(points)
%MINIMUMCALIPERDIAMETER Minimum caliper diameter of a set of points
%
%   WIDTH = minimumCaliperDiameter(POINTS)
%   Computes the minimum width of a set of points. As polygons and
%   polylines are represented as point lists, this function works also for
%   polygons and polylines.
%
%   [WIDTH THETA] = minimumCaliperDiameter(POINTS)
%   Also returns the direction of minimum width. The direction corresponds
%   to the horizontal angle of the edge that minimizes the width. THETA is
%   given in radians, between 0 and PI.
%
%
%   Example
%   % Compute minimal caliper diameter, and check coords of rotated points
%   % have expected extent
%     points = randn(30, 2);
%     [width theta] = minimumCaliperDiameter(points);
%     points2 = transformPoint(points, createRotation(-theta));
%     diff = max(points2) - min(points2);
%     abs(width - diff(2)) < 1e-10
%     ans =
%         1
%
%   References
%   Algorithms use rotating caliper. Implementation was based on that of
%   Wikipedia:
%   http://en.wikipedia.org/wiki/Rotating_calipers
%
%   See also
%   polygons2d, convexHull, orientedBox
%

% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-04-08,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

% first, compute convex hull of the polygon
inds = convhull(points(:,1), points(:,2));
hull = points(inds, :);

% if first and last points are the same, remove the last one
if inds(1) == inds(end)
    hull = hull(1:end-1, :);
end

% number of hull vertices
nV = size(hull, 1);

% default values
rotated_angle = 0;
min_width = inf;
min_angle = 0;

% avoid degenerated cases
if nV < 3
    return;
end

[tmp, p_a] = min(hull(:, 2)); %#ok<ASGLU>
[tmp, p_b] = max(hull(:, 2)); %#ok<ASGLU>

caliper_a = [ 1 0];    % Caliper A points along the positive x-axis
caliper_b = [-1 0];    % Caliper B points along the negative x-axis

while rotated_angle < pi
    % compute the direction vectors corresponding to each edge
    ind_a2 = mod(p_a, nV) + 1;
    vector_a = hull(ind_a2, :) - hull(p_a, :);

    ind_b2 = mod(p_b, nV) + 1;
    vector_b = hull(ind_b2, :) - hull(p_b, :);

    % Determine the angle between each caliper and the next adjacent edge
    % in the polygon
    angle_a = vectorAngle(caliper_a, vector_a);
    angle_b = vectorAngle(caliper_b, vector_b);

    % Determine the smallest of these angles
    minAngle = min(angle_a, angle_b);

    % Rotate the calipers by the smallest angle
    caliper_a = rotateVector(caliper_a, minAngle);
    caliper_b = rotateVector(caliper_b, minAngle);

    rotated_angle = rotated_angle + minAngle;

    % compute current width, and update opposite vertex
    if angle_a < angle_b
        line = createLine(hull(p_a, :), hull(ind_a2, :));
        width = distancePointLine(hull(p_b, :), line);
        p_a = mod(p_a, nV) + 1;

    else
        line = createLine(hull(p_b, :), hull(ind_b2, :));
        width = distancePointLine(hull(p_a, :), line);
        p_b = mod(p_b, nV) + 1;

    end

    % update minimum width and corresponding angle if needed
    if width < min_width
        min_width = width;
        min_angle = rotated_angle;
    end

end


% calculate error between an ellipse and a point
% expected input form : ellipse = [x0 y0 A B alpha], point = [x y]
function error = calcEllipseCost(ellipse, point)

% Initial setting
x = point(2);   y = point(1);
x0 = ellipse(1);    y0 = ellipse(2);
a = ellipse(4);     b = ellipse(3);
alpha = ellipse(5);
Q = [cos(alpha), sin(alpha); -sin(alpha), cos(alpha)];  % inverse of rotation matrix

% E(x',y') = abs(x'^2/a^2 + y'^2/b^2 - 1)
v = Q*[x-x0;y-y0];
v(1) = v(1)/a;
v(2) = v(2)/b;
error = abs(1-v'*v);

end


function [accepted, rejected] = fun_metropolis_hastings(param_init, iterations, data)

x = param_init;

accepted = [];
rejected = [];

for i = 1:iterations
    x_new = fun_transition_model(x);
    x_lik = fun_log_likelihood(x, data);
    x_new_lik = fun_log_likelihood(x_new, data);

    if fun_acceptance(x_lik + log(fun_prior(x)), x_new_lik + log(fun_prior(x_new)))
        x = x_new;
        accepted = [accepted x_new(1)];
    else
        rejected = [rejected x_new(1)];
    end
end