function [yIntervalEst,PInvIntervalEst,yFwdPred,PInvFwdPred,yFwdEnd,PInvFwdEnd]=FPInfoIntervalSmoother(yFwdPred,PInvFwdPred,yFwdPrev,PInvFwdPrev,N,z,R,H,F,Q,u,hasLastPred)
%%FPINFOINTERVALSMOOTHER Update the Fraser-Potter information smoother
%                smoother over a sliding or growing interval given a new
%                measurement. The smoothed estimates over the entire
%                interval are available. If one already computed the
%                predicted state at the current time, it can be provided
%                rather than being recomputed. This can slide an interval
%                of estimates fixed at length N or grow an interval until
%                it is length N. See the example below for how one can grow
%                the interval until it is a desired length.
%
%INPUTS: yFwdPred The xDimXN set of predicted information states. If the
%                 interval is is increasing in size (has not yet reached
%                 the desired N), then the last dimension is NCur-1 in
%                 size, where NCur is the current length of z, unless
%                 hasLastPred=true, in which case it is NCur in length
%                 with the final prediction already computed. If
%                 hasLastPred=false, then the final column of xFwdPred is
%                 the state predicted to the time-step PRIOR to the current
%                 one. Otherwise, if hasLastPred=true, then the final
%                 column is the state predicted to the time of the latest
%                 measurement in z. This cannot be an empty matrix.
%     PInvFwdPred The xDimXxDimXN set of predicted inverse covariance
%                 matrices associated with yFwdPred. The size and meaning
%                 of the final dimension is the same as for yFwdPred.
%        yFwdPrev The xDimX1 posterior information state estimate at the
%                 time of the second to latest measurement in z. When
%                 calling this function repeatedly, this can be the yFwdEnd
%                 output from the last function call. If hasLastPred=true,
%                 then an empty matrix can be passed in place of this
%                 input.
%     PInvFwdPrev The xDimXxDim inverse covariance matrix associate with
%                 yFwdPrev. When calling this function repeatedly, this can
%                 be the PInvFwdEnd output from the last function call. If
%                 hasLastPred=true, then an empty matrix can be passed in
%                 place of this input.
%               N The positive scalar integer desired length of the
%                 smoothed interval; N>=2. If omitted or an empty matrix is
%                 passed, then N is set to the number of columns of z
%                 (NCur).
%               z The zDimXN set of measurements over the interval. z(:,N)
%                 is the latest measurement. If the interval is expanding,
%                 then z is zDimXNCur in length.
%               R The zDimXzDimXN set of measurement covariance matrices
%                 associated with the values in z. If the interval is
%                 expanding, then the last dimension is NCur in size. If
%                 all the matrices are the same, then a single zDimXzDim
%                 matrix can be passed.
%               H The xDimXxDimXN set of measurement matrices associated
%                 with the values in z. If the interval is expanding, then
%                 the last dimension is NCur in size. If all the matrices
%                 are the same, then a single xDimXzDim matrix can be
%                 passed.
%               F The xDimXxDimX(N-1) set of state transition matrices over
%                 the interval. F(:,:,N-1) is the state transition matrix
%                 to the time of the latest measurerment in z. If the batch
%                 is expanding, then the last dimension is NCur-1 in size.
%                 If all F are the same, then a single xDimXxDim matrix can
%                 be passed.
%               Q The xDimXxDimX(N-1) set of process noise covariance
%                 matrices. the meaning of the third dimension is the same
%                 as for F. If all Q are the same, then a single xDimXxDim
%                 matrix can be passed.
%               u The xDimX(N-1) set of state transition control inputs,
%                 where the second dimension has the same meaning as the
%                 third dimensions of F. . If an empty matrix is passed,
%                 then it is assumed that all control inputs are zero. If
%                 aall the u vectors are the same, then a single xDimX1
%                 vector can be passed.
%     hasLastPred This boolean value indicates whether yFwdPred and
%                 PInvFwdPred contain the prediction to the time of the
%                 latest measurement in z. The default if omitted or an
%                 empty matrix is passed is false.
%
%OUTPUTS: yIntervalEst The xDimXN set of smoothed information state
%                      estimates over the interval. The oldest estimate is
%                      yIntervalEst(:,1)
%                      and the newest yIntervalEst(:,N). If the interval is
%                      expanding, the second dimension will be size NCur.
%      PInvIntervalEst The xDimXxDimXN set of inverse covariance matrices
%                      associated with those in yIntervalEst. If the
%                      interval is expanding, the third dimension will be
%                      size NCur.
% yFwdPred,PInvFwdPred The xDimXN set of predicted information states and
%                      the associated xDimXxDimX(N-1) set of predicted
%                      inverse covariance matrices. If the interval is
%                      expanding, the final dimensions will be length
%                      NCur.
%   yFwdEnd,PInvFwdEnd The xDimX1 and xDimXxDim posterior information state
%                      and its inverse covariance matrix at the latest
%                      time.
%
%The Kalman smoothing technique utilizing forward and backwards filters is
%described in [1]. The use with information filters is related.
%
%EXAMPLE:
%Given an uninformative prior, the information smoother is initialized with
%the output of an information filter at the first step. Then, as more
%measurements are obtained, the batch length increases until it reached
%length N. After that, the interval slides. We also run an information
%filter and show that not only does the smoothed estimate at the end of the
%interval (the most smoothed) outperform the information filter, but that
%the estimate at the start of the interval is the same as the real-time
%information filter output. The measurement error reduction factor (MERF)
%and the NEES of the estimates (converted to target states) of the
%information filter and the information  smoother are compared. The MERF of
%the smoother is lower (better) and the NEES of both are close to 1,
%indicating estimator consistency.
% T=1;%Sample period.
% numRuns=500;
% N=4;%The length of the interval considered.
% numSteps=100;
%
% %Parameters for the noise process dynamics and measurement.
% H=[eye(2,2),zeros(2,2)];
% zDim=size(H,1);
% xDim=size(H,2);
% R=diag([40;40]);
% SR=chol(R,'lower');
%
% %Statistics for the initial state.
% x0Mean=[1e3;0;75;-50];
% x0Cov=diag([1e3^2;100^2;25^2;25^2]);
% x0S=chol(x0Cov,'lower');
%
% %Parameters for the state dynamics.
% q=processNoiseSuggest('PolyKal-ROT',9.8,1);
% F=FPolyKal(T,xDim,1);
% Q=QPolyKal(T,xDim,1,q);
% SQ=chol(Q,'lower');
%
% %The initial uninformative state.
% yInit=zeros(xDim,1);
% PInvInit=zeros(xDim,xDim,1);
%
% xTrue=zeros(xDim,numSteps+N-1,numRuns);
% z=zeros(zDim,numSteps+N-1,numRuns);
% ySmoothed=zeros(xDim,numSteps,numRuns);
% PInvSmoothed=zeros(xDim,xDim,numSteps,numRuns);
% yInfo=zeros(xDim,numSteps,numRuns);
% PInvInfo=zeros(xDim,xDim,numSteps,numRuns);
% yInfoAlt=zeros(xDim,numSteps,numRuns);
% PInvInfoAlt=zeros(xDim,xDim,numSteps,numRuns);
% for curRun=1:numRuns
%     %Draw the initial state.
%     xTrue(:,1,curRun)=x0Mean+x0S*randn(xDim,1);
%     %Get the first measurement.
%     z(:,1,curRun)=H*xTrue(:,1,curRun)+SR*randn(zDim,1);
%
%     %Get the states and measurements for the rest of the steps plus extra
%     %steps so that the Kalman filter and smoother cover the same range.
%     for curStep=2:(numSteps+N-1)
%         xTrue(:,curStep,curRun)=F*xTrue(:,curStep-1,curRun)+SQ*randn(xDim,1);
%         z(:,curStep,curRun)=H*xTrue(:,curStep,curRun)+SR*randn(zDim,1);
%     end
%
%     %Initialize the information filters and the information smoother with a
%     %single measurement.
%     [yUpdate,PInvUpdate]=infoFilterUpdate(yInit,PInvInit,z(:,1,curRun),R,H);
%     yFwdPred=yInit;
%     PInvFwdPred=PInvInit;
%     yFwdPrev=yUpdate;
%     PInvFwdPrev=PInvUpdate;
%
%     yInfo(:,1,curRun)=yUpdate;
%     PInvInfo(:,:,1,curRun)=PInvUpdate;
%
%     yInfoAlt(:,1,curRun)=yUpdate;
%     PInvInfoAlt(:,:,1,curRun)=PInvUpdate;
%
%     %Continue with the rest of the measurements.
%     for curStep=2:numSteps
%         zSpan=z(:,max(curStep-N+1,1):curStep,curRun);
%
%         [yIntervalEst,PInvIntervalEst,yFwdPred,PInvFwdPred,yFwdPrev,PInvFwdPrev]=FPInfoIntervalSmoother(yFwdPred,PInvFwdPred,yFwdPrev,PInvFwdPrev,N,zSpan,R,H,F,Q);
%         if(curStep>=N)
%             ySmoothed(:,curStep-N+1,curRun)=yIntervalEst(:,1);
%             PInvSmoothed(:,:,curStep-N+1,curRun)=PInvIntervalEst(:,:,1);
%         end
%         yInfoAlt(:,curStep,curRun)=yIntervalEst(:,end);
%         PInvInfoAlt(:,:,curStep,curRun)=PInvIntervalEst(:,:,end);
%
%         [yPred,PInvPred]=infoFilterDiscPred(yInfo(:,curStep-1,curRun),PInvInfo(:,:,curStep-1,curRun),F,Q);
%         [yUpdate,PInvUpdate]=infoFilterUpdate(yPred,PInvPred,z(:,curStep,curRun),R,H);
%         yInfo(:,curStep,curRun)=yUpdate;
%         PInvInfo(:,:,curStep,curRun)=PInvUpdate;
%     end
%
%     %Now, go N-1 steps more with the smoother, so that the information
%     %filter estimates and the information smoother estimates cover the same
%     %range of values.
%     for curStep=(numSteps+1):(numSteps+N-1)
%         zSpan=z(:,(curStep-N+1):curStep,curRun);
%         [yIntervalEst,PInvIntervalEst,yFwdPred,PInvFwdPred,yFwdPrev,PInvFwdPrev]=FPInfoIntervalSmoother(yFwdPred,PInvFwdPred,yFwdPrev,PInvFwdPrev,N,zSpan,R,H,F,Q);
%         ySmoothed(:,curStep-N+1,curRun)=yIntervalEst(:,1);
%         PInvSmoothed(:,:,curStep-N+1,curRun)=PInvIntervalEst(:,:,1);
%     end
% end
%
% %The maximum absolute difference between the standard information filter
% %estimate and the estimate at the front of the infomration smoother batch.
% %This should be 0.
% maxInfoDiff=max(abs(yInfo(:)-yInfoAlt(:)))
%
% %Get the target states. We shall skip the first one which is not completely
% %observable.
% xSmoothed=zeros(xDim,numSteps-1,numRuns);
% PSmoothed=zeros(xDim,xDim,numSteps-1,numRuns);
% xEst=zeros(xDim,numSteps-1,numRuns);
% PEst=zeros(xDim,xDim,numSteps-1,numRuns);
% for curRun=1:numRuns
%     for curStep=2:numSteps
%         xSmoothed(:,curStep-1,curRun)=PInvSmoothed(:,:,curStep,curRun)\ySmoothed(:,curStep,curRun);
%         PSmoothed(:,:,curStep-1,curRun)=inv(PInvSmoothed(:,:,curStep,curRun));
%
%         xEst(:,curStep-1,curRun)=PInvInfo(:,:,curStep,curRun)\yInfo(:,curStep,curRun);
%         PEst(:,:,curStep-1,curRun)=inv(PInvInfo(:,:,curStep,curRun));
%     end
% end
%
% %Limit the period plotted to the range where the target state is fully
% %observable and is common to the smoother and the filter.
% xTrue=xTrue(:,2:numSteps,:);
% z=z(:,2:numSteps,:);
% posTrue=xTrue(1:2,:,:);
%
% MERFSmoothed=calcMERF(posTrue,z,xSmoothed(1:2,:,:),0,true);
% MERFInfo=calcMERF(posTrue,z,xEst(1:2,:,:),0,true);
% NEESSmoothed=calcNEES(xTrue,xSmoothed,PSmoothed);
% NEESInfo=calcNEES(xTrue,xEst,PEst);
%
% figure(1)
% clf
% hold on
% plot(2:numSteps,MERFInfo,'-k','linewidth',4)
% plot(2:numSteps,MERFSmoothed,'--r','linewidth',2)
% h1=xlabel('Step');
% h2=ylabel('MERF');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
% legend('Information Filter','Information Smoother')
%
% figure(2)
% clf
% hold on
% plot(2:numSteps,NEESInfo,'-k','linewidth',4)
% plot(2:numSteps,NEESSmoothed,'--r','linewidth',2)
% h1=xlabel('Step');
% h2=ylabel('NEES');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
% legend('Information Filter','Information Smoother','location','southeast')
%
%REFERENCES:
%[1] D. C. Fraser and J. E. Potter, "The optimal linear smoother as a
%    combination of two optimum linear filters," IEEE Transactions on
%    Automatic Control, pp. 387-390, Aug. 1969.
%
%August 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xDim=size(yFwdPred,1);
NCur=size(z,2);

if(nargin<12||isempty(hasLastPred))
    hasLastPred=false;
end

if(isempty(N))
    %Assume that the batch is already the desired size if the batch length
    %is not specified.
    N=NCur;
end

NFEnd=NCur-1;

if(size(F,3)==1)
    F=repmat(F,[1,1,NFEnd]);
end

if(size(Q,3)==1)
    Q=repmat(Q,[1,1,NFEnd]);
end

if(nargin<11||isempty(u))
    u=zeros(xDim,NFEnd);
elseif(size(u,2)==1)
    u=repmat(u,[1,NFEnd]);
end

if(size(H,3)==1)
    H=repmat(H,[1,1,NCur]);
end

if(size(R,3)==1)
    R=repmat(R,[1,1,NCur]);
end

if(hasLastPred==false)
    [yFwdPredEnd,PInvFwdPredEnd]=infoFilterDiscPred(yFwdPrev,PInvFwdPrev,F(:,:,NFEnd),Q(:,:,NFEnd),u(:,NFEnd));

    if(NCur==N&&size(yFwdPred,2)==N)
        %If the interval is sliding.
        yFwdPred=[yFwdPred(:,2:N),yFwdPredEnd];
        PInvFwdPred=cat(3,PInvFwdPred(:,:,2:N),PInvFwdPredEnd);
    else%The interval is growing; NCur is one more than the current length
        %of yFwdPred.
        yFwdPred(:,NCur)=yFwdPredEnd;
        PInvFwdPred(:,:,NCur)=PInvFwdPredEnd;
    end
end

%Run the information filter backwards.
yRev=zeros(xDim,NCur);
PInvRev=zeros(xDim,xDim,NCur);

[yRev(:,NCur),PInvRev(:,:,NCur)]=infoFilterUpdate(zeros(xDim,1),zeros(xDim,xDim),z(:,NCur),R(:,:,NCur),H(:,:,NCur));
for curStep=(NCur-1):-1:1
    [yRevPred,PInvRevPred]=infoFilterDiscPredRev(yRev(:,curStep+1),PInvRev(:,:,curStep+1),F(:,:,curStep),Q(:,:,curStep),u(:,curStep));
    [yRev(:,curStep),PInvRev(:,:,curStep)]=infoFilterUpdate(yRevPred,PInvRevPred,z(:,curStep),R(:,:,curStep),H(:,:,curStep));
end

PInvIntervalEst=PInvFwdPred+PInvRev;
yIntervalEst=yFwdPred+yRev;

if(nargout>4)
    yFwdEnd=yIntervalEst(:,end);
    PInvFwdEnd=PInvIntervalEst(:,:,end);
end
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.


classdef DehomogenizingSingularitiesTest < handle

    properties (Access = private)
        mesh
        orientation
        levelSet
    end

    properties (Access = private)
        testName
        meshSize
        singularitiesData
        xmin
        xmax
        ymin
        ymax
        widthH
        widthW
        nCells
    end

    methods (Access = public)

        function obj = DehomogenizingSingularitiesTest(cParams)
            obj.init(cParams);
            obj.createMesh();
            obj.createOrientation();
            obj.dehomogenize();
        end

        function passed = hasPassed(obj)
            d = load(obj.testName);
            ls = obj.levelSet;
            errI = zeros(numel(ls),1);
            for iCell = 1:numel(ls)
                lSI = d.levelSet{iCell};
                errI(iCell) = norm(ls{iCell} - lSI)/norm(lSI);
            end
            itIs = sum(errI) < 1e-12;
            passed = itIs;
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.testName = cParams.testName;
            %obj.meshSize = 0.00521;
            obj.meshSize = 0.09;%0.0221;%0.09;%0.0221;%0.0521 %0.0221;0.0921
            obj.nCells   = [60 62];%linspace(60,62,40);%45;   %45
            obj.xmin = 0.5;
            obj.xmax = 2.0;
            obj.ymin = 0.25;
            obj.ymax = 1.75;
            obj.singularitiesData = [0.32,-0.8];
            obj.widthH = 0.87;
            obj.widthW = 0.87;
        end

        function createMesh(obj)
            h = obj.meshSize;
            xv = obj.xmin:h:obj.xmax;
            yv = obj.ymin:h:obj.ymax;
            [X,Y] = meshgrid(xv,yv);
            s.coord(:,1) = X(:);
            s.coord(:,2) = Y(:);
            s.connec = delaunay(s.coord);
            m = Mesh(s);
            obj.mesh = m;
        end

        function createOrientation(obj)
            m = obj.mesh;
            s1 = obj.singularitiesData(:,1);
            s2 = obj.singularitiesData(:,2);
            x1 = m.coord(:,1);
            x2 = m.coord(:,2);
            v(:,1) = cos(pi*(x1 + s1*x2));
            v(:,2) = cos(pi*(x2 + s2*x1));
            beta = atan2(v(:,2),v(:,1));
            alpha = beta/2;
            obj.orientation(:,1) = cos(alpha);
            obj.orientation(:,2) = sin(alpha);
        end

        function plotOrientation(obj)
            figure()
            x = obj.mesh.coord(:,1);
            y = obj.mesh.coord(:,2);
            t  = obj.orientation;
            ct = cos(t(:,1));
            st = sin(t(:,1));
            quiver(x,y,ct,st)
        end

        function s = createLevelSetCellParams(obj)
            I        = ones(size(obj.mesh.coord,1),1);
            s.type   = 'rectangleInclusion';
            s.widthH = obj.widthH*I;
            s.widthV = obj.widthW*I;
            s.ndim   = 2;
        end

        function dehomogenize(obj)
            s.nCells             = obj.nCells;
            s.cellLevelSetParams = obj.createLevelSetCellParams();
            s.mesh               = obj.mesh;
            s.theta              = atan2(obj.orientation(:,2),obj.orientation(:,1));
            d = Dehomogenizer(s);
            ls = d.compute();
            d.plot();
            obj.levelSet = ls;
        end

    end

end


function a = r8col_sortr_a ( m, n, a, key )

%*****************************************************************************80
%
%% R8COL_SORTR_A ascending sorts one column of an R8COL, adjusting all entries.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), an unsorted M by N array.
%
%    Input, integer KEY, the column in which the "key" value
%    is stored.  On output, column KEY of the array will be
%    in nondecreasing order.
%
%    Output, real A(M,N), rows of the array have been shifted in such
%    a way that column KEY of the array is in nondecreasing order.
%
  if ( m <= 0 )
    return
  end

  if ( key < 1 || n < key )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8COL_SORTR_A - Fatal error!\n' );
    fprintf ( 1, '  The value of KEY is not a legal column index.\n' );
    fprintf ( 1, '  KEY = %d\n', key );
    fprintf ( 1, '  N = %d\n', n );
    error ( 'R8COL_SORTR_A - Fatal error!' );
  end
%
%  Initialize.
%
  i = 0;
  indx = 0;
  isgn = 0;
  j = 0;
%
%  Call the external heap sorter.
%
  while ( 1 )

    [ indx, i, j ] = sort_heap_external ( m, indx, isgn );
%
%  Interchange the I and J objects.
%
    if ( 0 < indx )

      a = r8row_swap ( m, n, a, i, j );
%
%  Compare the I and J objects.
%
    elseif ( indx < 0 )

      if ( a(i,key) < a(j,key) )
        isgn = -1;
      else
        isgn = +1;
      end

    elseif ( indx == 0 )

      break

    end

  end

  return
end


classdef MaF15 < PROBLEM
% <multi/many> <real> <large/none>
% Inverted LSMOP8

%------------------------------- Reference --------------------------------
% R. Cheng, M. Li, Y. Tian, X. Zhang, S. Yang, Y. Jin, and X. Yao, A
% benchmark test suite for evolutionary many-objective optimization,
% Complex & Intelligent Systems, 2017, 3(1): 67-81.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties(Access = private)
        sublen;	% Number of variables in each subcomponent
        len;    % Cumulative sum of lengths of variable groups
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            % Parameter setting
            if isempty(obj.M); obj.M = 3; end
            if isempty(obj.D); obj.D = 20*obj.M; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = [ones(1,obj.M-1),10.*ones(1,obj.D-obj.M+1)];
            obj.encoding = ones(1,obj.D);
            % Calculate the number of variables in each subcomponent
            nk = 2;
            c  = 3.8*0.1*(1-0.1);
            for i = 1 : obj.M-1
                c = [c,3.8.*c(end).*(1-c(end))];
            end
            obj.sublen = floor(c./sum(c).*(obj.D-obj.M+1)/nk);
            obj.len    = [0,cumsum(obj.sublen*nk)];
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            [N,D] = size(PopDec);
            M     = obj.M;
            nk    = 2;
            PopDec(:,M:D) = (1+repmat(cos((M:D)./D*pi/2),N,1)).*PopDec(:,M:D) - repmat(PopDec(:,1)*10,1,D-M+1);
            G = zeros(N,M);
            for i = 1 : 2 : M
                for j = 1 : nk
                    G(:,i) = G(:,i) + Griewank(PopDec(:,obj.len(i)+M-1+(j-1)*obj.sublen(i)+1:obj.len(i)+M-1+j*obj.sublen(i)));
                end
            end
            for i = 2 : 2 : M
                for j = 1 : nk
                    G(:,i) = G(:,i) + Sphere(PopDec(:,obj.len(i)+M-1+(j-1)*obj.sublen(i)+1:obj.len(i)+M-1+j*obj.sublen(i)));
                end
            end
            G      = G./repmat(obj.sublen,N,1)./nk;
            PopObj = (1+G+[G(:,2:end),zeros(N,1)]).*(1-fliplr(cumprod([ones(N,1),cos(PopDec(:,1:M-1)*pi/2)],2)).*[ones(N,1),sin(PopDec(:,M-1:-1:1)*pi/2)]);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = UniformPoint(N,obj.M);
            R = 1 - R./repmat(sqrt(sum(R.^2,2)),1,obj.M);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            if obj.M == 2
                R = obj.GetOptimum(100);
            elseif obj.M == 3
                a = linspace(0,pi/2,10)';
                R = {1-sin(a)*cos(a'),1-sin(a)*sin(a'),1-cos(a)*ones(size(a'))};
            else
                R = [];
            end
        end
    end
end

function f = Griewank(x)
    f = sum(x.^2,2)./4000 - prod(cos(x./repmat(sqrt(1:size(x,2)),size(x,1),1)),2) + 1;
end

function f = Sphere(x)
    f = sum(x.^2,2);
end


% Check whether a graph is weighted, i.e edges have weights.
%
% INPUTS: edge list, m x 3, m: number of edges, [node 1, node 2, edge weight]
% OUTPUTS: Boolean variable, 0 or 1
%
% GB: last updated, Sep 23, 2012

function S=isWeighted(el)

S=true;

if numel( find(el(:,3)==1) ) == size(el,1); S=false; end


function [A_or,C_or,S_or,P_or,srt,srt_val] = order_ROIs(A,C,S,P,srt)

% ordering of the found components based on their maximum temporal
% activation and their size (through their l_inf norm)
% you can also pre-specify the ordering sequence

nA = full(sqrt(sum(A.^2)));
nr = length(nA);
A = A/spdiags(nA(:),0,nr,nr);
C = spdiags(nA(:),0,nr,nr)*C;
mA = sum(A.^4).^(1/4);
%sA = sum(A);
mC = max(C,[],2);
if ~exist('srt', 'var')||isempty(srt)
    [srt_val,srt] = sort(mC.*mA','descend');
end
A_or = A(:,srt);
C_or = C(srt,:);

if nargin < 4
    P_or = [];
else
    P_or = P;
    if isfield(P,'gn'); P_or.gn=P.gn(srt); end
    if isfield(P,'b'); P_or.b = cellfun(@times,P.b(srt),num2cell(nA(srt)'),'UniformOutput',false); end
    if isfield(P,'c1'); P_or.c1 = cellfun(@times,P.c1(srt),num2cell(nA(srt)'),'UniformOutput',false); end
    if isfield(P,'neuron_sn'); P_or.neuron_sn=num2cell(nA(srt)'.*cell2mat(P.neuron_sn(srt))); end
end

if nargin < 3 || isempty(S)
    S_or = [];
else
    S = spdiags(nA(:),0,nr,nr)*S;
    S_or = S(srt,:);
end


function Loglike = Loglike_MixHP(Seqs, model, alg)

% given responsibility, calcuate the expected number of sequence belonging
% to the k-th cluster
EX = model.R;

% update parameters of Hawkes processes (mu_k, A_k), k=1,...,N
% initialize
A = model.beta;%random('exp', model.beta);
mu = sqrt(pi/2)*model.b;%random('rayl', model.b);

tmp1 = A(:)./(model.beta(:));
tmp1(isnan(tmp1)) = 0;
tmp1(isinf(tmp1)) = 0;

tmp2 = mu(:).^2./(2*model.b(:).^2);
tmp2(isnan(tmp2)) = 0;
tmp2(isinf(tmp2)) = 0;

tmp3 = log(mu(:));
tmp3(isnan(tmp3)) = 0;
tmp3(isinf(tmp3)) = 0;

Loglike = sum(tmp1)+ sum(tmp2)-sum(tmp3);

% E-step: evaluate the responsibility using the current parameters
for c = 1:length(Seqs)

    Time = Seqs(c).Time;
    Event = Seqs(c).Mark;
    Tstart = Seqs(c).Start;

    if isempty(alg.Tmax)
        Tstop = Seqs(c).Stop;
    else
        Tstop = alg.Tmax;
        indt = Time < alg.Tmax;
        Time = Time(indt);
        Event = Event(indt);
    end

    N = length(Time);
    % calculate the integral decay function in the log-likelihood function
    G = Kernel_Integration(Tstop - Time, model);

    for i = 1:N

        ui = Event(i);
        ti = Time(i);

        lambdai = mu(ui,:)+eps;
        pii = lambdai;

        if i>1
            tj = Time(1:i-1);
            uj = Event(1:i-1);

            gij = Kernel(ti-tj, model);
            auiuj = A(uj, :, :, ui);
            pij = repmat(gij, [1,1,model.K,1]).* auiuj;

            tmp = sum(sum(pij,1),2);
            lambdai = lambdai + tmp(:)';

        end

        LL = LL+log(lambdai);

    end
    LL = LL - (Tstop-Tstart).*sum(mu);
    tmp = sum(sum(repmat(G, [1,1,model.K]).*sum(A(Event,:,:,:),4),2),1);
    LL = LL - tmp(:)';

    Loglike = Loglike - EX(c,:)*LL(:);

end

Loglike = -Loglike;


function S = sub(A, i, j)
%SUB     Principal submatrix.
%        SUB(A,i,j) is A(i:j,i:j).
%        SUB(A,i)  is the leading principal submatrix of order i,
%        A(1:i,1:i), if i>0, and the trailing principal submatrix
%        of order ABS(i) if i<0.

if nargin == 2
   if i >= 0
      S = A(1:i, 1:i);
   else
      n = min(size(A));
      S = A(n+i+1:n, n+i+1:n);
   end
else
   S = A(i:j, i:j);
end


function [d dt pred] = bfs_in_mbgl(A,u)
% BFS_IN_MBGL Reimplement the BFS function with MatlabBGL visitors.
%
% [d dt pred] = bfs_in_mbgl(A,u)
%
% See BFS
%
% Example:
%    load ../graphs/bfs_example.mat
%    d = bfs_in_mbgl(A,1)

ip_d = ipdouble(-ones(num_vertices(A),1));
ip_dt = ipdouble(-ones(num_vertices(A),1));
ip_pred = ipdouble(zeros(1,num_vertices(A)));

ip_time = ipdouble(1);

    function time_discover_vertex(u)
        ip_dt(u) = ip_time(1);
        ip_time(1) = ip_time(1) + 1;
    end

    function distance_tree_edge(ei,u,v)
        ip_d(v) = ip_d(u)+1;
    end

    function pred_tree_edge(ei,u,v)
        ip_pred(v) = u;
    end

vis_distance = struct('tree_edge', @distance_tree_edge);
vis_time = struct('discover_vertex', @time_discover_vertex);
vis_pred = struct('tree_edge', @pred_tree_edge);

vis = combine_visitors(vis_distance, vis_time, vis_pred);

ip_d(u) = 0;
ip_dt(u) = ip_time(1);
breadth_first_search(A,u,vis);

d = double(ip_d);
dt = double(ip_dt);
pred = double(ip_pred);

% end the function
end


function toms743_test03 ( nbits, xmin, xmax, n )

%*****************************************************************************80
%
%% TOMS743_TEST03 tests WAPR(X) when X is the argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Andrew Barry, S. J. Barry,
%    Patricia Culligan-Hensley.
%    This MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, integer NBITS, the number of bits in the mantissa.
%
%    Input, real XMIN, XMAX, the range.
%
%    Input, integer N, the number of equally spaced values
%    in the range at which arguments are to be chosen.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TOMS743_TEST03\n' );
  fprintf ( 1, '  Input X is the argument.\n' );

  ifmt = 1;
  l = 0;

  if ( xmax < xmin )
    temp = xmin;
    xmin = xmax;
    xmax = temp;
  end

  dx = ( xmax - xmin ) / n;
  xmin = xmin - dx;

  if ( xmax <= 0.0 )
    iw = 1;
    fprintf ( 1, '  Both branches of the W function will be checked.\n' );
  else
    iw = 0;
    fprintf ( 1, '  Wp has been selected (maximum x is > 0)\n' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Results for Wp(x):\n' );

  if ( ifmt == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, '   Offset x    W(x) (WAPR)' );
    fprintf ( 1, '     W(x) (BISECT)  Digits Correct\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '     x     W(x) (WAPR)' );
    fprintf ( 1, '     W(x) (BISECT)  Digits Correct\n' );
  end
  fprintf ( 1, '\n' );

  for i = 1 : n + 1

    x = xmin + i * dx;
    [ w, nerror ] = wapr ( x, 0, l );

    if ( nerror == 1 )

      fprintf ( 1, '  The value of X = %g is out of range.\n', x );

    else

      [ we, ner ] = bisect ( x, 0, l );

      if ( ner == 1 )
        fprintf ( 1, '\n' );
        fprintf ( 1, ' BISECT did not converge for x = %g\n', x );
        fprintf ( 1, '  Try reducing NBITS.\n' );
      end

      if ( w == we )
        nd = floor ( log10 ( 2.0 ^ nbits ) + 0.5 );
      else
        nd = floor ( log10 ( abs ( we / ( w - we ))) + 0.5 );
      end

      fprintf ( 1, '%17.8g      %17.8g      %17.8g      %3d\n', x, w, we, nd );

    end

  end

  if ( iw == 1 )

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Results for Wm(x):\n' );

    if ( ifmt == 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, '   Offset x    W(x) (WAPR)' );
      fprintf ( 1, '     W(x) (BISECT)  Digits Correct\n' );
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, '     x     W(x) (WAPR)' );
      fprintf ( 1, '     W(x) (BISECT)  Digits Correct\n' );
    end

    for i = 1 : n + 1

      x = xmin + i * dx;
      [ w, nerror ] = wapr ( x, 1, l );

      if ( nerror == 1 )

        fprintf ( 1, '  The value of X = %g is out of range.\n', x );

      else

        [ we, ner ] = bisect ( x, 1, l );

        if ( ner == 1 )
          fprintf ( 1, '\n' );
          fprintf ( 1, ' BISECT did not converge for x = %g\n', x );
          fprintf ( 1, '  Try reducing NBITS.\n' );
        end

        if ( w == we )
          nd = log10 ( 2.0 ^ nbits ) + 0.5;
        else
          nd = log10 ( abs ( we / ( w - we ))) + 0.5;
        end

        fprintf ( 1, '%17.8g      %17.8g      %17.8g      %3d\n', x, w, we, nd );

      end

    end

  end

  return
end


%% Copyright (C) 2014-2019, 2022 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy is free software; you can redistribute it and/or modify
%% it under the terms of the GNU General Public License as published
%% by the Free Software Foundation; either version 3 of the License,
%% or (at your option) any later version.
%%
%% This software is distributed in the hope that it will be useful,
%% but WITHOUT ANY WARRANTY; without even the implied warranty
%% of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See
%% the GNU General Public License for more details.
%%
%% You should have received a copy of the GNU General Public
%% License along with this software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defun  vpasolve (@var{e})
%% @defunx vpasolve (@var{e}, @var{x})
%% @defunx vpasolve (@var{e}, @var{x}, @var{x0})
%% Numerical solution of a symbolic equation.
%%
%% Variable-precision numerical solution of the equation @var{e}
%% for variable @var{x} using initial guess of @var{x0}.
%%
%% Example:
%% @example
%% @group
%% syms x
%% eqn = exp(x) == x + 2;
%% vpasolve(eqn, x, 0.1)
%%   @result{} (sym) 1.1461932206205825852370610285214
%% @end group
%% @end example
%%
%% Systems of equations are supported:
%% @example
%% @group
%% syms x y
%% eqns = [y*exp(x) == 16; x^5 == x + x*y^2]
%%   @result{} eqns = (sym 2×1 matrix)
%%
%%       ⎡     x       ⎤
%%       ⎢  y⋅ℯ  = 16  ⎥
%%       ⎢             ⎥
%%       ⎢ 5      2    ⎥
%%       ⎣x  = x⋅y  + x⎦
%% @end group
%%
%% @group
%% vpasolve(eqns, [x; y], [1; 1])
%%   @result{} (sym 2×1 matrix)
%%
%%       ⎡1.7324062670465659633407456995303⎤
%%       ⎢                                 ⎥
%%       ⎣2.8297332667835974266598942031498⎦
%% @end group
%% @end example
%%
%% Complex roots can be found but you must provide a complex initial
%% guess:
%% @example
%% @group
%% vpasolve(x^2 + 2 == 0, x, 1i)
%%   @result{} (sym) 1.4142135623730950488016887242097⋅ⅈ
%% @end group
%% @end example
%%
%% @seealso{vpa}
%% @end defun

function r = vpasolve(e, x, x0)

  if (nargin < 1 || nargin > 3)
    print_usage ();
  end

  if (nargin < 3)
    x0 = sym(0);
  end
  if (nargin < 2)
    x = symvar(e, 1);
  end

  n = digits();

  % everything to column vector
  e = e(:);
  x = x(:);
  x0 = x0(:);

  if (isscalar (x0)) && (numel (x) >= 2)
    x0 = x0 * ones (size (x));
  end

  % IPC cannot pass double matrices yet
  if (isnumeric (x0) && (numel (x0) > 1))
    x0 = num2cell (x0);
  end

  cmd = {
      '(e, x, x0, n) = _ins'
      'import mpmath'
      'mpmath.mp.dps = n'
      'r = nsolve(e, x, x0)'
      'return r' };
  r = pycall_sympy__ (cmd, sym(e), x, x0, n);

end

%!test
%! syms x
%! vpi = vpa(sym(pi), 64);
%! e = tan(x/4) == 1;
%! q = vpasolve(e, x, 3.0);
%! w = q - vpi ;
%! assert (double(w) < 1e-30)

%!test
%! syms x
%! vpi = vpa(sym(pi), 64);
%! e = tan(x/4) == 1;
%! q = vpasolve(e, x);
%! w = q - vpi;
%! assert (double(w) < 1e-30)
%! q = vpasolve(e);
%! w = q - vpi;
%! assert (double(w) < 1e-30)

%!test
%! % very accurate pi
%! syms x
%! e = tan(x/4) == 1;
%! m = digits(256);
%! q = vpasolve(e, x, 3);
%! assert (double(abs(sin(q))) < 1e-256)
%! digits(m);

%!test
%! % very accurate sqrt 2
%! syms x
%! e = x*x == 2;
%! m = digits(256);
%! q = vpasolve(e, x, 1.5);
%! assert (double(abs(q*q - 2)) < 1e-256)
%! digits(m);

%!test
%! % very accurate sqrt pi
%! % (used to fail https://github.com/sympy/sympy/issues/8564)
%! syms x
%! e = x*x == sym(pi);
%! m = digits(256);
%! q = vpasolve(e, x, 3);
%! assert (double(abs(sin(q*q))) < 1e-256)
%! digits(m);

%!test
%! syms x
%! r = vpasolve(x^2 + 2 == 0, x, 1i);
%! assert (double (imag(r)^2 - 2), 0, 1e-32)
%! assert (double (real(r)^2), 0, 1e-32)
%! r = vpasolve(x^2 + 2 == 0, x, -3i + 5);
%! assert (double (imag(r)^2 - 2), 0, 1e-32)
%! assert (double (real(r)^2), 0, 1e-32)

%!test
%! % system
%! syms x y
%! f = 3*x^2 - 2*y^2 - 1;
%! g = x^2 - 2*x + y^2 + 2*y - 8;
%! r = vpasolve([f; g], [x; y], sym([-1; 1]));
%! assert (isa (r, 'sym'))
%! assert (numel (r) == 2)

%!test
%! % system, double guess
%! syms x y
%! f = 3*x^2 - 2*y^2 - 1;
%! g = x^2 - 2*x + y^2 + 2*y - 8;
%! r = vpasolve([f; g], [x; y], [-1.1 1.2]);

%!test
%! % system, double guess
%! syms x y
%! f = 3*x^2 - 2*y^2 - 1;
%! g = x^2 - 2*x + y^2 + 2*y - 8;
%! r1 = vpasolve([f; g], [x; y], [-1.1]);
%! r2 = vpasolve([f; g], [x; y], [-1.1 -1.1]);
%! assert (isequal (r1, r2))

%!test
%! % system, more eqns than unknowns
%! syms x y
%! eqns = [x^3 - x - y == 0; y*exp(x) == 16; log(y) + x == 4*log(sym(2))];
%! r = vpasolve (eqns, [x; y], [1; 1]);
%! A = subs (lhs (eqns), [x; y], r);
%! err = A - [0; 16; 4*log(sym(2))];
%! assert (double (err), zeros (size (err)), 1e-31)


function pass = test_sample( )
% Test diskfun sample() command

tol = 100*chebfunpref().cheb2Prefs.chebfun2eps;

% Function to test
f = diskfun(@(x,y) sin(pi*x.*y));

% Ensure the matrix of sampled values is correct.
[m,n] = length(f);
[nn,mm] = size(sample(f));
pass(1) = (m == mm) && (n == nn);

% Sample on fixed grids of various sizes to make sure the right size output
% is given.
m = 120;
n = 121;
[nn,mm] = size(sample(f, m, n));
pass(2) = (m == mm) && (n == nn);

m = 121;
n = 120;
[nn,mm] = size(sample(f, m, n));
pass(3) = (m == mm) && (n == nn);

% Check samples are correct.
% m even and n odd
m = 30;
n = 2*20+1;
cp = chebpts(n);
[t,r] = meshgrid(trigpts(m, [-pi, pi]), cp((n+1)/2:end));
F = f(t,r, 'polar');
G = sample(f, m, (n+1)/2);
pass(4) = norm(F(:) - G(:), inf) < tol;
[U, D, V] = sample(f, m, (n+1)/2);
G = U * D * V.';
pass(5) = norm(F(:) - G(:), inf) < tol;

% m odd and n odd
m = 31;
n = 2*20+1;
cp = chebpts(n);
[t,r] = meshgrid(trigpts(m, [-pi, pi]), cp((n+1)/2:end));
F = f(t,r, 'polar');
G = sample(f, m, (n+1)/2);
pass(6) = norm(F(:) - G(:), inf) < tol;
[U, D, V] = sample(f, m, (n+1)/2);
G = U * D * V.';
pass(7) = norm(F(:) - G(:), inf) < tol;

% m odd and n even
m = 31;
n = 2*20-1;
cp = chebpts(n);
[t,r] = meshgrid(trigpts(m, [-pi, pi]), cp((n+1)/2:end));
F = f(t,r, 'polar');
G = sample(f, m, (n+1)/2);
pass(8) = norm(F(:) - G(:), inf) < tol;
[U, D, V] = sample(f, m, (n+1)/2);
G = U * D * V.';
pass(9) = norm(F(:) - G(:), inf) < tol;

% m even and n even
m = 30;
n = 2*20-1;
cp = chebpts(n);
[t,r] = meshgrid(trigpts(m, [-pi, pi]), cp((n+1)/2:end));
F = f(t,r, 'polar');
G = sample(f, m, (n+1)/2);
pass(10) = norm(F(:) - G(:), inf) < tol;
[U, D, V] = sample(f, m, (n+1)/2);
G = U * D * V.';
pass(11) = norm(F(:) - G(:), inf) < tol;

% Sample should return all ones for the function 1.
f = diskfun(@(x,y) 1 + 0*x);
F = sample(f, 128, 128);
pass(12) = norm(F(:) - 1, inf) < tol;

% Check that errors are caught
try
    F = sample(f, 0, 20);
    pass(13) = false;
catch ME
    pass(13) = strcmp(ME.identifier, 'CHEBFUN:DISKFUN:sample:inputs');
end

try
    F = sample(f, 20, 0);
    pass(14) = false;
catch ME
    pass(14) = strcmp(ME.identifier, 'CHEBFUN:DISKFUN:sample:inputs');
end

end


function prob = nmix2row(prob)
%NMIX2ROW  Convert Mixed Nonlinear Constraints to Nonlinear Row bounds
%   prob = nmix2row(prob)
%
%   Use -1 for <=, 0 for =, and 1 for >= in vector e

%   Copyright (C) 2012 Jonathan Currie (I2C2)

%Assign common vars
rhs = prob.nlrhs;
e = prob.nle;

%Transpose as Required
if(size(rhs,2) > 1)
    rhs = rhs';
end
if(size(e,2) > 1)
    e = e';
end
if(xor(isempty(rhs),isempty(e)))
    error('You must supply both nlrhs and nle!');
end
if(length(rhs) ~= length(e))
    error('nlrhs and nle are not the same length!');
end
if(any(e < -1) || any(e > 1))
    error('The vector e must only contain values -1, 0 or 1');
end

%Defaults
ncon = length(e);
prob.cl = -Inf(ncon,1);
prob.cu = Inf(ncon,1);

%Indices
ieq = e == 0;
icl = (e ==  1 | ieq);
icu = (e == -1 | ieq);

%Fill In
prob.cl(icl) = rhs(icl);
prob.cu(icu) = rhs(icu);

%Remove unused fields
prob.nlrhs = []; prob.nle = [];


function a2 = r8cmat_to_r8mat ( lda, m, n, a1 )

%*****************************************************************************80
%
%% R8CMAT_TO_R8MAT transfers data from an R8CMAT to an R8MAT.
%
%  Discussion:
%
%    An R8CMAT is an MxN array of R8's, stored with a leading dimension LD,
%    accessible as a vector:
%      (I,J) -> (I+J*LD).
%    or as a doubly-dimensioned array, if declared A(LD,N):
%      (I,J) -> A(I,J)
%
%    An R8MAT is an MxN array of R8's,
%    accessible as a vector:
%      (I,J) -> (I+J*M).
%    or as a doubly-dimensioned array, if declared A(M,N):
%      (I,J) -> A(I,J)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer LDA, the leading dimension of A1.
%    M <= LDA.
%
%    Input, integer M, the number of rows of data.
%    M <= LDA.
%
%    Input, integer N, the number of columns of data.
%
%    Input, real A1(LDA,N), the M by N matrix to be copied.
%
%    Output, real A2(M,N), a copy of the
%    information in the MxN submatrix of A1.
%
      a2(1:m,1:n) = a1(1:m,1:n);

      return
      end


function [gx] = g_goNogo(x,P,u,in)
% generates the probability of 'go' choice: P(go) = sig(V/temperature+bias)
% function [gx] = g_goNogo(x,P,u,in)
% IN:
%   - x: value of the 'go' option
%   - P: log-temperature and bias
%   - u: [useless]
%   - in: [useless]
% OUT:
%   - gx: P(go) = sig(V/temperature+bias)

gx = 1./(1+exp(P-x));


clc; clear all; close all;

dd = 'C:\Users\Doyeunlee\Desktop\Analysis\rawdata\convert\';
filelist = 'dslim_multigrasp_MI';

%% Band topography
[cnt, mrk, mnt]=eegfile_loadMatlab([dd filelist]);

% band power
band= [4 8];
ival=[0 45000];
[b,a]= butter(5, band/cnt.fs*2);
cnt_flt= proc_filt(cnt, b, a);

epo = cntToEpo(cnt_flt, mrk, ival);

spec= proc_spectrum(epo, [4 8]);

% H = scalpPlot(mnt, spec);
figure('Name', 'CSP Patterns');
plotCSPatterns(csp_fv, mnt, csp_w, csp_fv.y)

plotCSPatterns(fv, mnt, W, la);


function [permuteMatrix, nNonRepeats, nUniqueDirs, nUniqueMeasurements] = dtiBootGetPermMatrix(bvecs, bvals)
%
% [permuteMatrix, nNonRepeats, nUniqueDirs, nUniqueMeasurements] = dtiBootGetPermMatrix(bvecs, bvals)
%
%
% HISTORY:
% 2007.06.07 RFD wrote it

n = min(size(bvals,2),size(bvecs,2));
bv = [bvecs(:,1:n).*repmat(bvals(:,1:n),[3 1])];
nMeasurements = size(bv,2);
permuteMatrix = cell(nMeasurements,1);
for(ii=1:nMeasurements)
    dist1 = sqrt((bv(1,:)-bv(1,ii)).^2+(bv(2,:)-bv(2,ii)).^2+(bv(3,:)-bv(3,ii)).^2);
    dist2 = sqrt((bv(1,:)+bv(1,ii)).^2+(bv(2,:)+bv(2,ii)).^2+(bv(3,:)+bv(3,ii)).^2);
    permuteMatrix{ii} = unique([find(dist1<1e-3) find(dist2<1e-3)]);
end
numPerms = cellfun('length',permuteMatrix);
nNonRepeats = sum(numPerms==1);
m = zeros(nMeasurements,max(numPerms));
for(ii=1:nMeasurements)
    m(ii,1:length(permuteMatrix{ii})) = permuteMatrix{ii};
end
nUniqueMeasurements = size(unique(m,'rows'),1);
if(any(bvals<max(bvals)*0.01))
    nUniqueDirs = nUniqueMeasurements-1;
else
    nUniqueDirs = nUniqueMeasurements;
end

return


function rot = rotation(T)
% convertes orthogonal rank 2 tensors into rotations
%
% Syntax
%   rot = rotation(T)
%
% Input
%  T - orthogonal rank 2 @tensor
%
% Output
%  rot - @rotation
%

rot = rotation.nan(length(T));

switch T.rank

  case 2
    for i = 1:length(T)
      rot(i) = mat2quat(T.M(:,:,i));
    end

    rot = rot .* sign(det(T));

  otherwise
    error('Tensor needs to have rank 2!')
end


function C4 = C4f(epsi, C4x)
%C4F  Evaluate C_4
%
%   C4 = C4F(EPSI, C4X) evaluates C_{4,l} in the expansion for the area
%   (Eq. (65) expressed in terms of n and epsi) using the coefficient
%   vector C4X.  K2 is a K x 1 array.  C4X is a 1 x 15 array.  C4 is a K x
%   5 array.

  nC4 = 6;
  nC4x = size(C4x, 2);
  j = nC4x;
  C4 = zeros(length(epsi), nC4);
  for k = nC4 : -1 : 1
    t = C4(:, k);
    for i = nC4 - k : -1 : 0
      t = epsi .* t + C4x(j);
      j = j - 1;
    end
    C4(:, k) = t;
  end
  mult = ones(length(epsi), 1);
  for k = 2 : nC4
    mult = mult .* epsi;
    C4(:, k) = C4(:, k) .* mult;
  end
end


%% Copyright (C) 2016 Lagu
%% Copyright (C) 2019 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy is free software; you can redistribute it and/or modify
%% it under the terms of the GNU General Public License as published
%% by the Free Software Foundation; either version 3 of the License,
%% or (at your option) any later version.
%%
%% This software is distributed in the hope that it will be useful,
%% but WITHOUT ANY WARRANTY; without even the implied warranty
%% of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See
%% the GNU General Public License for more details.
%%
%% You should have received a copy of the GNU General Public
%% License along with this software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym divisors (@var{x})
%% Get divisors of integer.
%%
%% Example:
%% @example
%% @group
%% x = sym(150);
%% y = divisors(x)
%%   @result{} y = (sym) [1  2  3  5  6  10  15  25  30  50  75  150]  (1×12 matrix)
%% @end group
%% @end example
%% @end defmethod

%% Reference: http://docs.sympy.org/dev/modules/ntheory.html

function y = divisors(x)
  if (nargin ~= 1)
    print_usage ();
  end
  y = pycall_sympy__ ('return S(divisors(_ins[0])),', x);
  y = cell2sym(y);
end

%!test
%! assert( isequal( divisors(sym(150)), divisors(sym(-150)) ))


%% dev
% Below is a demonstration of the features of the |dev| function

%%
clear; close all; clc;

%% Syntax
% |Ad=dev(A);|

%% Description
% Computes the deviatoric part of the tensor A.
%
% See also |sph|.

%% Examples
%

A=rand(3,3)

Ad=dev(A)

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>
%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function [Torr] = mbar2Torr(mbar)
% Convert pressure from millibars to torr.
% Chad Greene 2012
Torr = mbar*0.750062;


function [cum_ret, cumprod_ret, daily_ret, daily_portfolio]...
    = olmar2(fid, data, varargins, opts)
% This program starts the OLMAR-2 algorithm
%
% function [cum_ret, cumprod_ret, daily_ret, daily_portfolio] ....
%           = olmar2_start(fid, data, varargins, opts)
%
% cum_ret: a number representing the final cumulative wealth.
% cumprod_ret: cumulative return until each trading period
% daily_ret: individual returns for each trading period
% daily_portfolio: individual portfolio for each trading period
%
% data: market sequence vectors
% fid: handle for write log file
% varargins: variable parameters
% opts: option parameter for behvaioral control
%
% Example: [cum_ret, cumprod_ret, daily_ret, daily_portfolio] ...
%          = olmar2_start(fid, data, {10, 0.5, 0}, opts);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is part of OLPS: http://OLPS.stevenhoi.org/
% Original authors: Bin LI, Steven C.H. Hoi
% Contributors:
% Change log:
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Extract the parameters
epsilon = varargins{1}; % reversion parameter \epsilon
alpha = varargins{2};       % Alpha size
tc = varargins{3};      % transaction cost fee rate

% Run the OLMAR-2 algorithm
[cum_ret, cumprod_ret, daily_ret, daily_portfolio]...
    = olmar2_run(fid, data, epsilon, alpha, tc, opts);

end


function test_ft_connectivitysimulation

% WALLTIME 00:10:00
% MEM 2gb
% DEPENDENCY ft_connectivitysimulation

cfg = [];
cfg.method      = 'ar';
cfg.nsignal     = 32;
cfg.ntrials     = 100;
cfg.triallength = 2;
cfg.fsample     = 500;

% test 'ar' method
cfg.params = zeros(cfg.nsignal,cfg.nsignal,cfg.ntrials);
cfg.params(14,21,42) = 0.5;
cfg.noisecov = 0.5*eye(cfg.nsignal);
dataout = ft_connectivitysimulation(cfg);

return
%% test linear_mix method => wait for issue to be solved
cfg.mix = 0.5*ones(32,2);
cfg.delay = 50*ones(32,2);

dataout = ft_connectivitysimulation(cfg);


function [D,Tc,res,dD,dTc,dRes,d2Phi] = distance(varargin)
% NP, JH 2006/11/25
%
% the distance function is phrased as
% D(y) = D(R,T,y) = phi(res(y))
%
% hence
% D   = phi(res(y))
% dD  = dPhi(res(y)) * dRes(y)
% d2D = res(y)' * d2Phi(res(y)) * dRes(y) + stuff we don't consider
%
% example SSD:
%   phi = hd/2 res'*res,  dPhi = hd * res', d2Phi = hd
%   res = T(y) - R,       dRes = dT

D     = [];
Tc    = [];
res   = [];
dD    = [];
dTc   = [];
dRes  = [];
d2Phi = [];
%
% -----------------------------------------------------------------------------
persistent PARA

if nargout>0 & nargin == 0,            % return PARA
    D  = PARA;
    return;
end;

% clear PARA
if isstr(varargin{1}) & strcmp(varargin{1},'clear'),
    PARA = [];
    return;
end;
% set PARA
if isstr(varargin{1}) & strcmp(varargin{1},'set'),
    PARA = setPARA(PARA,varargin{2:end});
    return;
end;
% -----------------------------------------------------------------------------

% -----------------------------------------------------------------------------
% do the work
% -----------------------------------------------------------------------------

Rc     = varargin{1};
TD     = varargin{2};
Omega  = varargin{3};
m      = varargin{4};
Y      = varargin{5};

doDerivative = (nargout > 3);

h   = Omega./m;
hd  = prod(h);
n   = prod(m);
%X   = getGrid(Omega,m);
%Rc  = interpolation(RD,Omega,X);
[Tc,dTc] = interpolation(TD,Omega,Y,doDerivative);

switch PARA.MODE,

    case 'SSD',
        % D   = phi(res(y))
        % dD  = dPhi(res(y)) * dRes(y)
        % d2D = res(y)' * d2Phi(res(y)) * dRes(y) + stuff we don't consider
        %
        % example SSD:
        %   phi = hd/2 res'*res,  dPhi = hd * res, d2Phi = hd
        %   res = T(y) - R,       dRes = dT

        res   = (Tc-Rc);
        D     = hd/2 * res' * res;

        if ~doDerivative, return; end;

        dRes  = dTc;
        dD    = hd * res' * dRes;
        d2Phi = hd;

    case 'SSD_W',
        % D   = phi(res(y))
        % dD  = dPhi(res(y)) * dRes(y)
        % d2D = res(y)' * d2Phi(res(y)) * dRes(y) + stuff we don't consider
        %
        % example SSD:
        %   phi = hd/2 res'*res,  dPhi = hd * res, d2Phi = hd
        %   res = T(y) - R,       dRes = dT

        mask   = varargin{6};

        res    = (Tc-Rc).*mask;
        D      = hd/2 * res' * res;

        if ~doDerivative, return; end;

        dRes  = sdiag(mask) * dTc;
        dD    = hd * res' * dRes;
        d2Phi = hd;

    case 'SSD_WP',
        % D   = phi(res(y))
        % dD  = dPhi(res(y)) * dRes(y)
        % d2D = res(y)' * d2Phi(res(y)) * dRes(y) + stuff we don't consider
        %
        % example SSD:
        %   phi = hd/2 res'*res,  dPhi = hd * res, d2Phi = hd
        %   res = T(y) - R,       dRes = dT
        period = PARA.period;
        period2 = period/2;

        mask   = varargin{6};

        res1   = Tc - Rc;

        p      = zeros(size(res1));
        p(res1 > period2)  = -period;
        p(res1 < -period2) =  period;

        res1   = res1 + p;
        res    = res1.*mask;
        D      = hd/2 * res' * res;

        if ~doDerivative, return; end;

        dRes  = sdiag(mask) * dTc;
        dD    = hd * res' * dRes;
        d2Phi = hd;

    case 'NGF',
        % D   = phi(res(y))
        % dD  = dPhi(res(y)) * dRes(y)
        % d2D = res(y)' * d2Phi(res(y)) * dRes(y) + stuff we don't consider
        %
        % example NGF:
        %   phi = hd/2 res'*res,  dPhi = hd * res, d2Phi = hd
        %   dR_i  = \nabla R_i / |\nabla R_i|_edge
        %   res = (\nabla T(y_i)' * dR_i) / ( |\nabla T(y_i)|_edge )
        %   dRes = complicated, see below

        edge = PARA.edge;
        [G1,G2,G3] = getGrad('c',Omega,m);
        if isempty(G3), G3 = 0; end;

        d1R = G1 * Rc(:);    d2R = G2 * Rc(:);    d3R = G3 * Rc(:);
        d1T = G1 * Tc(:);    d2T = G2 * Tc(:);    d3T = G3 * Tc(:);

        ndR = sqrt(d1R.^2 + d2R.^2 + d3R.^2 + edge^2);
        ndT = sqrt(d1T.^2 + d2T.^2 + d3T.^2 + edge^2);

        nd1R = d1R./ndR;      %nd1T = d1T./ndT;
        nd2R = d2R./ndR;      %nd2T = d2T./ndT;
        nd3R = d3R./ndR;      %nd3T = d3T./ndT;

        res1 = (nd1R.*d1T + nd2R.*d2T + nd3R.*d3T);
        res2 = 1./ndT;
        res  = res1 .* res2;

        D    = -hd/2 * res' * res;

        if ~doDerivative, return; end;

        dRes1 = sdiag(nd1R)*G1 + sdiag(nd2R)*G2 + sdiag(nd3R)*G3;
        dRes2 = -sdiag(1./ndT.^3)*(sdiag(d1T)*G1 + sdiag(d2T)*G2 + sdiag(d3T)*G3);
        dRes  = (sdiag(res2)*dRes1 + sdiag(res1)*dRes2) * dTc;

        dD    = -hd * res' * dRes;
        d2Phi = hd; % note the missing minus sign is not a bug!

    case 'MI'
        % D   = phi(res(y))
        % dD  = dPhi(res(y)) * dRes(y)
        % d2D = res(y)' * d2Phi(res(y)) * dRes(y) + stuff we don't consider
        %
        % example MI:
        %   phi   = res' * log(res + tol) + ...
        %   dPhi  = log(res + tol) + res./(res + tol) + ...
        %   d2Phi = (res + 2*tol)./(res + tol)^2 + ...
        %   res   = rho(T(y),R)
        %   dRes  = drho, see pdfestimate
        tol         = PARA.entropyTol;
        [rho,drho]  = pdfestimate(Rc,Tc,PARA,doDerivative);
        [n1,n2]     = size(rho);

        rhoR = sum(rho,2);
        rhoT = sum(rho,1)';
        rho  = rho(:);

        res  = rho;
        D    = rhoR'*log(rhoR+tol)+rhoT'*log(rhoT+tol) - rho'*log(rho+tol);

        if ~doDerivative, return; end;

        SR    = sparse(kron(ones(1,n2),speye(n1,n1)));
        ST    = sparse(kron(speye(n2,n2),ones(1,n1)));

        dPhi  = ...
            (log(rhoR+tol)+rhoR./(rhoR+tol))'*SR ...
            +(log(rhoT+tol)+rhoT./(rhoT+tol))'*ST ...
            -(log(rho +tol)+rho ./(rho +tol))';

        dRes  = drho*dTc;
        dD    = dPhi * dRes;

        d2Phi = ...
            SR'*sdiag((rhoR + 2*tol)./(rhoR+tol).^2)*SR ...
            +ST'*sdiag((rhoT + 2*tol)./(rhoT+tol).^2)*ST ...
            -sdiag((rho + 2*tol)./(rho+tol).^2);

        a = 1/sqrt(PARA.ngvR*PARA.ngvT);
        a = 1e0;
        d2Phi = - a * d2Phi;

    otherwise, error(sprintf('MODE = %s not implemented',MODE));
end;

return;

% =============================================================================
function d = sdiag(d);
d = spdiags(d,0,length(d),length(d));
% =============================================================================
function testMe(varargin)

fprintf('test %s\n',mfilename)

if nargin == 0,
    TD = zeros(100,100);
    TD(31:60,31:60) = 100;
    RD = TD + 1e1*randn(size(TD));
    Omega = [1,1];
    m     = [32,32];
    X  = getGrid(Omega,m);
    interpolation('set','MODE','linear-smooth');
    Rc = interpolation(RD,Omega,X);

    X = X+0.1;
end;

fctn = @(X) distance(Rc,TD,Omega,m,X);
testDerivative(fctn,X);

return;

% =============================================================================
function testDerivative(fctn,xc)

fprintf('derivative(%s)\n',mfilename)

[f,Tc,res,df]  = feval(fctn,xc);
v       = randn(size(xc));
dfv     = df*v;
hh      = logspace(0,-10,11);
fprintf('%12s %12s %12s \n','h','|fc-ft|','|fc+vdfc-ft|');
for j=1:length(hh),
    xt = xc + hh(j)*v;
    ft = feval(fctn,xt);
    n1 = norm(f(:)-ft(:));
    n2 = norm(f(:)+hh(j)*dfv-ft(:));
    fprintf('%12.4e %12.4e %12.4e\n',hh(j),n1,n2);
end;
return;
% =============================================================================

% ------------------------------------------------------------------------------
% set persistent parameter
% -----------------------------------------------------------------------------
function PARA = setPARA(PARA,varargin)

%disp(mfilename)
%varargin{:}

if ~isfield(PARA,'MODE') | isempty(getfield(PARA,'MODE')),
    PARA = setfield(PARA,'MODE','SSD');
end;

for j=1:length(varargin),
    if strcmp(varargin{j},'MODE'),
        PARA.MODE = varargin{j+1};
        varargin([j,j+1]) = [];
        break;
    end;
end;

fprintf('set distance MODE to %s, ',PARA.MODE);
switch PARA.MODE,
    case 'SSD',
        fprintf('no additional parameter needed\n');
    case 'SSD_W',
        fprintf('no additional parameter needed\n');
    case 'SSD_WP',
        if ~isfield(PARA,'period'), PARA.period = [];  end;
        if isempty(PARA.period),    PARA.period = 10;  end;
        for k=1:length(varargin)/2,
            str = sprintf('PARA=setfield(PARA,''%s'',varargin{%d});',...
                varargin{2*k-1},2*k);
            %disp(str);
            eval(str);
        end;
        fprintf('period=%s\n',num2str(PARA.period));
    case 'NGF',
        if ~isfield(PARA,'edge'), PARA.edge = [];  end;
        if isempty(PARA.edge),    PARA.edge = 10;  end;
        for k=1:length(varargin)/2,
            str = sprintf('PARA=setfield(PARA,''%s'',varargin{%d});',...
                varargin{2*k-1},2*k);
            %disp(str);
            eval(str);
        end;
        fprintf('edge=%s\n',num2str(PARA.edge));
    case 'MI',
        PARA = setMIpara(PARA,varargin{:});
        fprintf('setMIpara used\n');
    otherwise,
        error(sprintf('MODE = %s',MODE))
end;

return;
% -----------------------------------------------------------------------------
% -----------------------------------------------------------------------------


function D = driving_function_mono_sdm_kx_ls(kx,xs,f,conf)
%DRIVING_FUNCTION_MONO_SDM_KX_LS driving signal for a line source in SDM in
%the kx-domain
%
%   Usage: D = driving_function_mono_sdm_kx_ps(kx,xs,f,conf)
%
%   Input parameters:
%       kx          - kx dimension [nx1]
%       xs          - position of line source / m [1x3]
%       f           - frequency of the monochromatic source / Hz
%       conf        - configuration struct (see SFS_config)
%
%   Output parameters:
%       D           - driving function signal [nx1]
%
%   See also: driving_function_mono_sdm_kx

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 4;
nargmax = 4;
narginchk(nargmin,nargmax);
isargmatrix(kx);
isargxs(xs);
isargpositivescalar(f);
isargstruct(conf);

%% ===== Configuration ==================================================
xref = conf.xref;
c = conf.c;
dimension = conf.dimension;
driving_functions = conf.driving_functions;
x0 = conf.secondary_sources.center;
withev = conf.sdm.withev;

%% ===== Computation ====================================================
% Calculate the driving function in time-frequency domain

% Frequency
omega = 2*pi*f;
% Indexes for evanescent contributions and propagating part of the wave field
idxpr = (( abs(kx) <= (omega/c) ));
idxev = (( abs(kx) > (omega/c) ));
D = zeros(1,length(kx));

if strcmp('2D',dimension)

    % === 2-Dimensional ==================================================

    % Ensure 2D
    xs = xs(1:2);

    switch driving_functions
    case 'default'
        % --- SFS Toolbox ------------------------------------------------
        to_be_implemented;
    otherwise
        error(['%s: %s, this type of driving function is not implemented ', ...
            'for a 2D line source.'],upper(mfilename),driving_functions);
    end

elseif strcmp('2.5D',dimension)

    % === 2.5-Dimensional ================================================

    switch driving_functions
    case 'default'
        % --- SFS Toolbox ------------------------------------------------
        to_be_implemented;
    otherwise
        error(['%s: %s, this type of driving function is not implemented ', ...
            'for a 2.5D line source.'],upper(mfilename),driving_functions);
    end

elseif strcmp('3D',dimension)

    % === 3-Dimensional ==================================================

    switch driving_functions
    case 'default'
        % --- SFS Toolbox ------------------------------------------------
        to_be_implemented;
    otherwise
        error(['%s: %s, this type of driving function is not implemented ', ...
            'for a 3D line source.'],upper(mfilename),driving_functions);
    end

else
    error('%s: the dimension %s is unknown.',upper(mfilename),dimension);
end


function bratu ( )

%*****************************************************************************80
%
%% BRATU uses BVP4C to solve the BRATU problem.
%
%  Discussion:
%
%    The Bratu equation includes a parameter lambda.  Depending on the
%    value of lambda, there may be 2, 1, or no solutions to the BVP.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2013
%
%  Author:
%
%    Original MATLAB version by Shampine, Kierzenka, Reichelt.
%    This version by John Burkardt.
%
%  Reference:
%
%    Lawrence Shampine, Jacek Kierzenka, Mark Reichelt,
%    Solving boundary value problems for ordinary differential equations
%    in MATLAB with bvp4c.
%
  global lambda

  lambda_test = [ 0.45, 1.00, 3.50 ];

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BRATU:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Use BVP4C to solve the following boundary value problem:\n' );
  fprintf ( 1, '  y" + lambda * exp ( y ) = 0\n' );
  fprintf ( 1, '  y(0) = 0, y(1) = 0\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  When lambda = 1, there are two solutions.\n' );
  fprintf ( 1, '  Try\n' );
  fprintf ( 1, '    y(x) = 0.1, y''(x) = 0.\n' );
  fprintf ( 1, '  and\n' );
  fprintf ( 1, '    y(x) = 3.0, y''(x) = 0.0\n' );

  figure_num = 0;

  for test = 1 : 3
    lambda = lambda_test(test);
    figure_num = bratu_solver ( lambda, figure_num );
  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BRATU:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function figure_num = bratu_solver ( lambda, figure_num )

%*****************************************************************************80
%
%% BRATU_SOLVER calls BVP4C to solve the Bratu problem for a particular lambda.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real LAMBDA, the value of the parameter lambda.
%
%    Input/output, integer FIGURE_NUM, the number of figures being displayed.
%

%
%  Compute SOL1 with the first initial guess.
%
  x_init = linspace ( 0.0, 1.0, 5 );
  y_init = [ 0.1, 0.0 ];
  solinit = bvpinit ( x_init, y_init );
  sol1 = bvp4c ( @bratu_ode, @bratu_bc, solinit );
%
%  Compute SOL2 with the second initial guess.
%
  x_init = linspace ( 0.0, 1.0, 5 );
  y_init = [ 3.0, 0.0 ];
  solinit = bvpinit ( x_init, y_init );
  sol2 = bvp4c ( @bratu_ode, @bratu_bc, solinit );

  x = linspace ( 0.0, 1.0, 101 );
  y1 = deval ( sol1, x );
  y2 = deval ( sol2, x );
%
%  Display a plot of the two solutions.
%
  figure_num = figure_num + 1;
  figure ( figure_num )
  plot ( x, y1(1,:), 'r-', ...
         x, y2(1,:), 'g-', 'Linewidth', 2 );
  xlabel ( '<--- X --->' )
  ylabel ( '<--- Y --->' )
  title ( sprintf ( 'Bratu''s equation for \\lambda = %g\n', lambda ) );
  grid on
  filename = sprintf ( 'bratu_%f.png', lambda );
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saving plot file as "%s"\n', filename );

  return
end
function dydx = bratu_ode ( x, y )

%*****************************************************************************80
%
%% BRATU_ODE evaluates the right hand side of the ODE.
%
%  Discussion:
%
%    We assume that the differential equation has been rewritten as a
%    system of first order equations of the form
%
%      dydx = f(x,y)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the point at which the ODE is to be evaluated.
%
%    Input, real Y(M), the value of the solution at X.
%
%    Output, real DYDX(M), the value of the right hand side given X and Y.
%
  global lambda

  dydx(1) = y(2);
  dydx(2) = - lambda * exp ( y(1) );

  return
end
function bc = bratu_bc ( ya, yb )

%*****************************************************************************80
%
%% SAMPLE1_BC evaluates the boundary conditions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 September 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real YA(M), YB(M), the solution value at the left and right endpoints.
%
%    Output, real BC(2), the value of the boundary conditions.
%
  bc(1) = ya(1);
  bc(2) = yb(1);

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


function test_qrsol
%TEST_QRSOL test cs_qrsol
%
% Example:
%   test_qrsol
% See also: testall

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

index = UFget ;
[ignore f] = sort (max (index.nrows, index.ncols)) ;

k = 0 ;
rs1 = zeros (1,0) ;
rs2 = zeros (1,0) ;

for i = f
    Prob = UFget (i,index) ;
    A = Prob.A ;
    if (~isreal (A))
        continue ;
    end

    [m n] = size (A) ;                                                  %#ok
    b = rand (m,1) ;

    x1 = A\b ;
    x2 = cs_qrsol (A,b) ;

    x1 (~isfinite (x1)) = 0 ;
    x2 (~isfinite (x2)) = 0 ;

    r1 = norm (A*x1-b) ;
    r2 = norm (A*x2-b) ;

    k = k + 1 ;
    rs1 (k) = r1 ;
    rs2 (k) = r2 ;

    fprintf ('%30s  MATLAB: %6.2e CS: %6.2e\n', Prob.name, r1, r2) ;

    loglog (rs1, rs2, 'o') ;
    drawnow

    clear A b x1 x2
    % pack

end


function [mbar] = psi2mbar(psi)
% Convert units of pressure from pounds per square inch to millibar.
% Chad Greene 2012
mbar = psi*68.9476;


function f = duplicator_fh(duplication_indices,xdim,w)
%
% This factory creates a function handle to an MV2DF, which represents the
% function:
%
%    y = x(duplication_indices)
%

if nargin==0
    test_this();
    return;
end

map = @(x) x(duplication_indices);

%xdim = max(duplication_indices);
ydim = length(duplication_indices);
c = zeros(1,ydim);
r = zeros(1,ydim);
at = 1;
for i=1:xdim
    ci = find(duplication_indices==i);
    n = length(ci);
    r(at:at+n-1) = i;
    c(at:at+n-1) = ci;
    at = at + n;
end
reverse = sparse(r,c,1,xdim,ydim);

transmap = @(y) reverse*y;

f = @(w) linTrans(w,map,transmap);

if exist('w','var') && ~isempty(w)
    f = f(w);
end

end

function test_this()

dup = [ 1 3 1 3];
x = [ 1 2 3 4];

f = duplicator_fh(dup,length(x));

y = f(x),

test_MV2DF(f,x);

end


function demoBallisticScenario()
%%DEMOBALLISTICSCENARIO Demonstrate how the Joint Probabilistic Data
%             Association Filter (JPDAF) and the Global Nearest Neighbor
%             (GNN)-JPDAF can be used when tracking  two ballistic targets
%             with the possibility of missed detections and false alarms.
%             Track initiation and termination are not considered. This
%             function demonstrates how nonlinear filtering algorithms can
%             be used with the JPDAF/ GNN-JPDAF. Clistering is not
%             performed.
%
%A simple scenario of a ballistic target going from the Sea of Japan to
%an area in the middle of the Pacific (ignoring atmospheric drag, using a
%simple J2 model for the Earth's gravitational field is considered
%(ignoring light pressure and the gravitational attraction of other
%celestial bodies). Measurements are obtained by a radar in the Aleutian
%islands using a spherical measurement model. Refraction and other
%atmospheric effects are not simulated; Doppler is not used. Tracking and
%measurements are given in an Earth-centered Earth-fixed (ECEF) coordinate,
%system, requiring corrections for the Coriolis effect. Measurement-warping
%effects due to the Earth's rotation are not considered.
%
%The true trajectories for simulation are created using the
%createBallisticTrajectories subroutine in this file. Measurements are
%assumed to be available when the target is above a certain elevation in
%the receiver's coordinate system. The elevation in the receiver's
%coordinate system is measured with respect to the local tangent plane to
%the WGS84 reference ellipsoid. The trajectories go from the Sea of Japan
%to the middle of the Pacific. The function orbVelDet2PtMinEng is used to
%get an approximate initial velocity for a minimum energy trajectory
%between the points. However, the function is only an approximation as it
%is meant for use in an Earth-centered-inertial coordinate system (thus
%trajectories are minimum energy ignoring the energy imparted by the
%rotation of the Earth. Additionally, the endpoints are specified in an
%ECEF coordinate system, so the initial conditions outputted by the
%function are only approximate (a ballistic projectile would miss its
%target due to the Earth's rotation). Additionally, atmospheric drag is
%not taken into account and the function assumes a Keplerian dynamic model.
%Thus, the results might be useful as initial estimates for a
%higher-fidelity trajectory estimation routine, but would be unsuitable for
%use on their own. For the purpose of this simulation, the trajectory is
%sufficiently realistic.
%
%To make two targets sufficiently close that they will contest
%measurements (making a JPDAF relevant), the second target's trajectory was
%set to be that of the first target, but with a 50m offset in range. The
%resulting trajectory is no longer truly ballistic, but is close enough for
%the simulation.
%
%The tracks are started using the spher2CartOrbitCubature function (which
%is for a Keplerian model, not a J2 model). The correct measurements for
%two scans are used to start the tracks. After that, data association
%uncertainty can exist. This method of starting the tracks demonstrates how
%cubature integration can be used with an arbitrary dynamic model and
%measurement function to start a track. Note that additional steps would
%need to be taken if one were tracking satellites and multiple orbits
%between signtings were allowed.
%
%The JPDAF and GNN-JPDAF algorithms require a likelihood matrix to
%make their (soft) associations of measurements to targets. The function
%makeStandardCartOnlyLRMatHyps creates the likelihood matrix based on the
%exponent of the dimensionless score function of [1]. This requires that
%the clutter density in measurement space and the detection probability be
%known (or estimated). A nominal detection probability of 80% is used.
%
%The makeStandardCartOnlyLRMatHyps function also updates the tracks for
%each of the measurements. The sqrtKalmanUpdate function is used with
%Cartesian-converted measurements. The measurement conversion is performed
%using the spher2CartCubature function via fifth-order cubature
%integration.
%
%The target state is predicted using the momentMatchPred function, which
%can handle the nonlinear dynamic model. The momentMatchPred function
%utilizes non-stochastic Runge-Kutta methods to propagate the mean and
%square root covariance matrix.
%
%The trajectories are plotted along with the measurements. Also plotted is
%the offset from the range estimates of the target and the location of the
%first target. One could plot the mean squared error of the trajectories as
%well, but this is less informative regarding how well targets are
%separated as the cross-range errors for a monostatic tracking scenario
%such as this tend to be very high.
%
%REFERENCES:
%[1] Y. Bar-Shalom, S. S. Blackman, and R. J. Fitzgerald, "Dimensionless
%    score function for multiple hypothesis tracking," IEEE Transactions on
%    Aerospace and Electronic Systems, vol. 43, no. 1, pp. 392-400, Jan.
%    2007.
%
%June 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

disp('An example of simple nonlinear filtering and data association for a ballistic model')

%%%SETUP THE MODEL
disp('Setting parameters and creating the simulated trajectories.')
PD=0.8;%Detection probability --same for all targets.
%The number of observations in the simulation.
numObs=100;

%Assumed clutter parameter in terms of false alarms per meter-radian^2
%(spherical coordinates). False alarms are generated in the measurement
%domain of the radar. The tracker can still work if the value for lambda
%does not perfectly match the value simulated/ the real value. However, one
%should note that lambda in the tracker cannot be arbitrarily large lest
%the tracker eventually ignore all measurements (false alarms become always
%more likely than true tracks).
lambda=1e-2;
numMethods=2;%2 filters, a JPDAF and an GNN-JPDAF.

%The AlgSel parameters are inputs to the singleScanUpdate function.
%Method 1 uses the JPDAF
algSel1(1)=1;
algSel2(1)=0;
%Method 2 uses the GNN-JPDAF
algSel1(2)=0;
algSel2(2)=0;

%The location of the simulated radar in the Aleutian islands.
posRx=[51.86310; -177.98669]*(pi/180);
rRx=ellips2Cart([posRx;0]);

%The continuous-time dynamic model.
rDot=@(rState,t)aJ2Gravity(rState);

%Get the local coordinate axes of the receiver.
uENU=getENUAxes(posRx);

%Rotation matrix from global Cartesian coordinates to ENU
RGlob2ENU=findTransParam(eye(3,3),uENU);

%The nonlinear measurement function
h=@(x)Cart2Sphere(x(1:3,:),0,true,rRx,rRx,RGlob2ENU);

%Next, create the ballistic trajectories
[tObs,xObs,zObsTrue]=createBallisticTrajectories(rDot,h,numObs);
numTargets=size(zObsTrue,2);
PD=repmat(PD,[numTargets,1]);
zDim=size(zObsTrue,1);

%The time between samples is constant in this simulation, though missed
%detections are possible.
deltaT=tObs(2)-tObs(1);

sigmaR=10;
sigmaAzel=0.1*(pi/180);
%The square root measurement covariance matrix, local spherical
%coordinates. Here, it is assumed diagonal.
SR=diag([sigmaR;sigmaAzel;sigmaAzel]);

%Region in range around the targets in which to generate false alarms.
clutR=50e3;%50km
%Angular region in which to generate random false alarms around the
%targets.
clutAngs=2*(pi/180);
%The volume used for generating false alarms based times the given false
%alarm density is the expected number of false alarms per scan.
lambdaV=lambda*(2*clutR)*(2*clutAngs)^2;

%For measurement conversion
[xi3,w3]=fifthOrderCubPoints(3);

%For state propagation
[xi6,w6]=fifthOrderCubPoints(6);

%Initializations
q0=processNoiseSuggest('PolyKal-ROT',1e-2,deltaT);

a=rDot;
D=@(x,t)DPoly(1,q0,1,3);

disp('Initializing the tracks.')
%Generate the first two observations explicitly to start the tracks using an
%information filter. All methods get the same initialization.
xStates=zeros(6,numTargets,numObs,numMethods);
SStates=zeros(6,6,numTargets,numObs,numMethods);
zScans=cell(numObs,1);

zScans{1}=[];
zScans{2}=[];
zScansSpher{1}=[];
zScansSpher{2}=[];
for curTrack=1:numTargets
    z1=zObsTrue(:,curTrack,1)+SR*randn(3,1);
    [zCart1,RCart1]=spher2CartCubature(z1,SR,0,true,rRx,rRx,RGlob2ENU,xi3,w3);
    z2=zObsTrue(:,curTrack,2)+SR*randn(3,1);
    [zCart2,RCart2]=spher2CartCubature(z2,SR,0,true,rRx,rRx,RGlob2ENU,xi3,w3);

    zScansSpher{1}=z1;
    zScansSpher{2}=z2;

    [xInit,PInit]=spher2CartOrbitCubature([z1,z2],SR,0,true,deltaT,rRx,rRx,RGlob2ENU,true,xi6,w6);

    SInit=chol(PInit,'lower');

    for curMethod=1:numMethods
        xStates(:,curTrack,2,curMethod)=xInit;
        SStates(:,:,curTrack,2,curMethod)=SInit;
    end
    zScans{1}=[zScans{1},zCart1];
    zScans{2}=[zScans{2},zCart2];
end

disp('Running the tracker simulation (this will take a little while).')
%Run the tracker starting with the third measurement.
for curObs=3:numObs
    %Determine the number of false alarms to generate.
    numFalse=PoissonD.rand(1,lambdaV);

    %Determine which, if any, targets should be detected.
    isDet=rand(numTargets,1)<PD;

    %Allocate space for the detections.
    numMeas=numFalse+sum(isDet);
    zCur=zeros(3,numMeas);
    curDet=1;

    %Generate the detection from the targets, if any.
    for curTar=1:numTargets
        if(isDet(curTar)||curObs<5)
            zCur(:,curDet)=zObsTrue(:,curTar,curObs)+SR*randn(3,1);
            curDet=curDet+1;
        end
    end

    %Generate the false alarm detections, if any. They are centered around
    %the first target.
    rClutBounds=zObsTrue(1,1,curObs)+[-clutR;clutR];
    azBounds=zObsTrue(2,1,curObs)+[-clutAngs;clutAngs];
    elBounds=zObsTrue(3,1,curObs)+[-clutAngs;clutAngs];
    for curFalse=1:numFalse
        r=UniformD.rand(1,rClutBounds);
        az=UniformD.rand(1,azBounds);
        el=UniformD.rand(1,elBounds);

        zCur(:,curDet)=[r;az;el]+SR*randn(3,1);
        curDet=curDet+1;
    end

    %Convert all of the measurements to Cartesian coordinates using
    %cubature integration.
    zCart=zeros(zDim,numMeas);
    SRCart=zeros(zDim,zDim,numMeas);
    measJacobDet=zeros(numMeas,1);
    for curMeas=1:numMeas
        [zCart(:,curMeas),RCart]=spher2CartCubature(zCur(:,curMeas),SR,0,true,rRx,rRx,RGlob2ENU,xi3,w3);
        SRCart(:,:,curMeas)=chol(RCart,'lower');

        %The measurement Jacobian determinants are used for transforming
        %the clutter density.
        measJacobDet(curMeas)=det(calcSpherConvJacob(zCur(:,curMeas),0,true,rRx,rRx,RGlob2ENU));
    end
    zScans{curObs}=zCart;
    zScansSpher{curObs}=zCur;

    tPrev=tObs(curObs-1);
    tCur=tObs(curObs);

    %Update all of the tracks for the methods.
    for curMethod=1:numMethods
        %Predict the tracker to the current time for each target.
        x=xStates(:,:,curObs-1,curMethod);
        S=SStates(:,:,:,curObs-1,curMethod);
        for curTar=1:numTargets
            [x(:,curTar),S(:,:,curTar)]=momentMatchPred(x(:,curTar),S(:,:,curTar),a,D,3,tPrev,tCur,xi6,w6);
        end

        %Brute-force hypothesis formation and likelihood matrix
        %computation. A is the likelihood matrix.
        [A,xHyp,PHyp]=makeStandardCartOnlyLRMatHyps(x,S,zCart,SRCart,[],PD,lambda,[],[],measJacobDet);

        %Perform the single scan assignment
        [xPost,PPost]=singleScanUpdate(xHyp,PHyp,A,algSel1(curMethod),algSel2(curMethod));

        xStates(:,:,curObs,curMethod)=xPost;
        for curTar=1:numTargets
            SStates(:,:,curTar,curObs,curMethod)=chol(PPost(:,:,curTar),'lower');
        end
    end
end

disp('Plotting the measurements and tracks over the Earth.')
%Indicates how the lines for the method are drawn.
methodStyle{1}='r';
methodStyle{2}='b';

%%%Plot the estimated trajectories on the true trajectory
figure(1)
clf
hold on
clf
plotMapOnEllipsoid();
hold on;

%Plot all of the measurements for all of the scans
for curObs=1:numObs
    zCur=zScans{curObs};
    scatter3(zCur(1,:),zCur(2,:),zCur(3,:),'.b')
end

%Plot the tracks
plot3(xObs(1,:),xObs(2,:),xObs(3,:),'-g','linewidth',4)
for curMethod=1:numMethods
    %Track 1 is drawn in red; track 2 is drawn in blue.
    for curTrack=1:numTargets
        if(curTrack==1)
            lineOpts=['-',methodStyle{curMethod}];
        else
            lineOpts=['--',methodStyle{curMethod}];
        end

        x=xStates(:,curTrack,:,curMethod);
        plot3(x(1,2:end),x(2,2:end),x(3,2:end),lineOpts,'LineWidth',2)
    end
end

disp('Plotting the offset in range of the track estimates from the first target.')
disp('The lines for track 1 are solid; those for track 2 are dashed.')
disp('JPDAF estimates are red; GNN estimates are blue.')
disp('The observations are shown in black. The estimates are relatively close to the observations.')
%%%Compute and then plot the offset in range from the primary target.
rErrTargets=zeros(numObs,numTargets,numMethods);
for curObs=2:numObs
    for curMethod=1:2
        for curTrack=1:numTargets
            xState=xStates(:,curTrack,curObs,curMethod);
            SState=SStates(:,:,curTrack,curObs,curMethod);
            r=unbiasedR(xState,SState,RGlob2ENU,rRx,xi6,w6);

            rErrTargets(curObs,curTrack,curMethod)=r-zObsTrue(1,1,curObs);
        end
    end
end

figure(3)
clf
hold on
for curMethod=1:numMethods
    %Track 1 is drawn in red; track 2 is drawn in blue.
    for curTrack=1:numTargets
        if(curTrack==1)
            lineOpts=['-',methodStyle{curMethod}];
        else
            lineOpts=['--',methodStyle{curMethod}];
        end

        r=rErrTargets(:,curTrack,curMethod);
        plot(tObs(2:end),r(2:end),lineOpts,'LineWidth',2)
    end
end

%Plot the range of all of the measurements for all of the scans too
for curObs=2:numObs
    zCur=zScansSpher{curObs};
    numObs=size(zCur,2);
    if(numObs>0)
        scatter(repmat(tObs(curObs),[numObs,1]),bsxfun(@minus,zCur(1,:),zObsTrue(1,1,curObs)),'.k');
    end
end

h1=xlabel('Time (seconds)');
h2=ylabel('Offset in Range (meters)');

set(gca,'FontSize',18,'FontWeight','bold','FontName','Times')
set(h1,'FontSize',20,'FontWeight','bold','FontName','Times')
set(h2,'FontSize',20,'FontWeight','bold','FontName','Times')
axis([tObs(2), tObs(end), -300, 300])

end

function [tObs,xObs,zObsTrue]=createBallisticTrajectories(rDot,h,numObs)
    posStart=([39.61693; 134.54975])*pi/180;%The Sea of Japan
    %The middle of the Pacific.
    posEnd=([40.56751; -159.53953])*pi/180;
    rStart=ellips2Cart([posStart;0]);
    rEnd=ellips2Cart([posEnd;0]);

    minEL=10*(pi/180);%Minimum elevation for tracking.

    %Get starting velocity
    [vStart,tMinEllip]=orbVelDet2PtMinEng(rStart,rEnd);
    tSpan=[0; tMinEllip*1.25];

    %The function RKAdaptiveOverRange could be used to integrate over the
    %trajectory to a higher polynomial accuracy, but it is simpler to use
    %ode45, because the interpolation routine deval for the solution is
    %easy to use to get observations at desired times.
    options=odeset('RelTol',1e-8,'AbsTol',1e-10,'Refine',20);
    [tFull,xFull]=ode45(@(t,rState)rDot(rState,t),tSpan,[rStart;vStart],options);
    xFull=xFull';

    %Noise-free observations in the local coordinate system of the receiver.
    zTrue=h(xFull(1:3,:));
    elIdx=find(zTrue(3,:)>minEL);

    %Allocate space depending on the chosen scenario.
    zObsTrue=zeros(3,2,numObs);%The main target and one other return.

    %Get the primary trajectory at desired times
    tObs=linspace(tFull(elIdx(1)),tFull(elIdx(end)),numObs);
    options=odeset('RelTol',1e-8,'AbsTol',1e-10);
    sol=ode45(@(t,rState) rDot(rState,t),tSpan,[rStart;vStart],options);
    xObs=deval(sol,tObs);
    zObsTrue(:,1,:)=h(xObs(1:3,:));

    %The second target is offset by a constant amount in range.
    zObsTrue(2:end,2,:)=zObsTrue(2:end,1,:);
    zObsTrue(1,2,:)=zObsTrue(1,1,:)+50;
end

function r=unbiasedR(xState,SState,RGlob2ENU,rRx,xi,w)
    %Get an unbiased conversion of a Cartesian state into a range to a
    %sensor using cubature conversion (ignoring all propagation effects).

    %Transform the cubature points.
    xi=bsxfun(@plus,SState*xi,xState);

    %Subtract the receiver location and rotate into ENU coordinates.
    xiLocal=RGlob2ENU*bsxfun(@minus,xi(1:3,:),rRx);

    %Compute ranges
    rCub=sqrt(sum(xiLocal(1:3,:).^2,1));
    r=sum(bsxfun(@times,rCub(:),w(:)));
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.


function [Angles, Offsets, w1cw, w1rms, w1rp, Tau] = SPGR_prepare( Prot )

Angles  = Prot.Angles;
Offsets = Prot.Offsets;
w1cw  = zeros(length(Angles),1);
w1rms = w1cw;
w1rp  = w1cw;
Tau   = w1cw;

for ii = 1:length(Angles)
    Pulse = GetPulse(Angles(ii), Offsets(ii), Prot.Tm, Prot.MTpulse.shape, Prot.MTpulse.opt);
    w1cw(ii) = compute_w1cw(Prot.TR, Pulse);
    w1rms(ii) = compute_w1rms(Pulse);
    [w1rp(ii), Tau(ii)] = compute_w1rp(Pulse);
end

end


function fixed = p05_fixed_points ( m, fixed_num )

%*****************************************************************************80
%
%% P05_FIXED_POINTS returns the fixed points in problem 05.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  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, integer M, the spatial dimension.
%
%    Input, integer FIXED_NUM, the number of fixed points.
%
%    Output, real FIXED(M,FIXED_NUM), the fixed points.
%
  center1 = [  0.0, 0.0 ];
  center2 = [ -0.4, 0.0 ];
  r1 = 1.0;
  r2 = 0.55;

  fixed = [ ...
    center1(1) - r1,  center1(2); ...
    center2(1) - r2,  center2(2); ...
    center2(1) + r2,  center2(2); ...
    center1(1) + r1,  center1(2) ]';

  return
end


%INITWIDEANGLEPROJMAP  Initializes maps for cv.remap for wide-angle
%
%     [map1, map2] = cv.initWideAngleProjMap(cameraMatrix, distCoeffs, imageSize, destImageWidth)
%     [map1, map2, scale] = cv.initWideAngleProjMap(cameraMatrix, distCoeffs, imageSize, destImageWidth)
%     [...] = cv.initWideAngleProjMap(..., 'OptionName',optionValue, ...)
%
% ## Input
% * __cameraMatrix__ Input camera matrix `A = [f_x 0 c_x; 0 f_y c_y; 0 0 1]`
% * __distCoeffs__ Input vector of distortion coefficients
%   `[k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4,taux,tauy]` of 4, 5, 8, 12 or 14
%   elements. If the vector is empty, the zero distortion coefficients are
%   assumed.
% * __imageSize__ image size `[w,h]`.
% * __destImageWidth__
%
% ## Output
% * __map1__ The first output map. See `M1Type`.
% * __map2__ The second output map. See `M1Type`.
% * __scale__
%
% ## Options
% * __M1Type__ Type of the first output map, default `single2`. See
%   cv.convertMaps. Accepted types are:
%   * __int16__ first output map is a MxNx2 `int16` array, second output map
%     is MxNx1 `uint16` (fixed-point representation).
%   * __single1__ first output map is a MxNx1 `single` matrix, second output
%     map is MxNx1 `single` (separate floating-point representation).
%   * __single2__ first output map is a MxNx2 `single` matrix, second output
%     map is empty (combined floating-point representation).
% * __ProjType__ projection type, default 'EqRect'. One of:
%   * __Ortho__
%   * __EqRect__
% * __Alpha__ default 0
%
% See also: cv.initUndistortRectifyMap, cv.remap, cv.convertMaps
%


function [ d_mid ] = searchForDistanceLoss( Loss_wanted, freq, land_sea, urban_suburban_rural, h_a, h_eff, h_2, tPct, lPct, mobile, indoor)
%searchForDistanceLoss finds the distance at which a certain loss value
%occurs given a certain set of parameters for the JTG5-6 model

d_upper = 1000;
d_lower = 0.001;
while 1
    d_mid = d_lower + (d_upper-d_lower)/2;
    Loss_mid = JTG5_6( freq , land_sea, urban_suburban_rural, h_a, h_eff, h_2, d_mid, tPct, lPct, mobile, indoor) ;%-tx_erp ;
    delta = abs(Loss_mid - Loss_wanted );
    if delta < 0.05
        break
    else
        if Loss_mid > Loss_wanted
            d_upper = d_mid;
        else
            d_lower = d_mid;
        end
    end
end
end


function [pp, pa, ap, aa] = tapas_sem_prosa_gen_lh(x, theta, ptheta)
%% Generate the likelihood function.
%
% Input
%   x       Time interval to plot the likelihood function.
%   theta   Parameters to use
%   ptheta  Priors
%
% Output
%   pp      Prosaccade trials, prosaccade action
%   pa      Prosaccade trial, antisaccade action
%   ap      Antisaccade trial, prosaccade action
%   aa      Antisaccade trial, antisaccade action
%

% aponteeduardo@gmail.com
% copyright (C) 2015
%

[PROSACCADE, ANTISACCADE] = tapas_sem_action_codes();

DIM_THETA = tapas_sem_prosa_ndims();

nt = numel(x);

u = struct('tt', zeros(nt, 1));
y = struct('t', x, 'a', zeros(nt, 1));

%
method = ptheta.method;

u.tt(:) = PROSACCADE;
y.a(:) = PROSACCADE;

pp = exp(method(y.t, y.a, u.tt, theta));

%

u.tt(:) = ANTISACCADE;
y.a(:) = PROSACCADE;

ap = exp(method(y.t, y.a, u.tt, theta));

%

u.tt(:) = PROSACCADE;
y.a(:) = ANTISACCADE;

pa = exp(method(y.t, y.a, u.tt, theta));

%

u.tt(:) = ANTISACCADE;
y.a(:) = ANTISACCADE;

aa = exp(method(y.t, y.a, u.tt, theta));

end


function r8vec_sort_heap_a_test ( )

%*****************************************************************************80
%
%% R8VEC_SORT_HEAP_A_TEST tests R8VEC_SORT_HEAP_A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 20;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8VEC_SORT_HEAP_A_TEST\n' );
  fprintf ( 1, '  R8VEC_SORT_HEAP_A ascending sorts an R8VEC.\n' );

  b = 0.0;
  c = 3.0 * n;
  seed = 123456789;

  [ a, seed ] = r8vec_uniform_ab ( n, b, c, seed );

  r8vec_print_some ( n, a, 1, 10, '  Original array:' );

  a = r8vec_sort_heap_a ( n, a );

  r8vec_print_some ( n, a, 1, 10, '  Ascending sorted array:' );

  return
end


%% Random phase scheme
close all
clear all

%% system parameters
load('user_channel.mat','K','N','M','ite','pd','ps','Hd_w','theta_init','AP_angle','IRS_angle',...
    'G_sig','User_angle','Hr_sig','eb1','eb2','path_d','path_i');
etai=1;
weight=1./((path_d));
weight=weight./sum(weight);
omega=weight;
snr_w=linspace(0,10,3);
rate_w=zeros(1,length(snr_w));
parfor s0=1:length(snr_w)
    %%
    fprintf('snr=%d\n',snr_w(s0));
    Pt=10.^(snr_w(s0)/10);
    rate=zeros(1,ite);
    for t0=1:ite
        %%
        beta=zeros(1,K);
        Hd=pd.*Hd_w(:,:,t0);
        theta=theta_init(:,:,t0);
        Theta=diag(theta');
        %%
        G=channel_G(AP_angle,IRS_angle,G_sig(:,:,t0),eb1,eb2,N,M);
        Hr=ps.*channel_Hr(User_angle,Hr_sig(:,:,t0),eb1,eb2,K,N);
        %%
        H=Hd+Hr*Theta*G;
        [ ~,~,f1 ] = init_W( H,M,K,Pt,omega );
         rate(t0)=f1;
    end
    rate_w(s0)=mean(rate);
end
%%
figure
plot(snr_w,rate_w,'ro-');
save('down_IRS_phaserand.mat','snr_w','rate_w','M','K','N','omega');
 %%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Test ALM in the paper ''Fast Alternating Linearization Methods for
%       Minimizing the Sum of Two Convex Functions'', Donald Goldfarb,
%       Shiqian Ma and Katya Scheinberg, Tech. Report, Columbia University,
%       2009 - 2010.
%
% Author: Shiqian Ma
% Date  : Apr. 20, 2010
% IEOR, Columbia University, Copyright (2010)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

randn('state',0); rand('state',0);
% addpath('C:\Mywork\Optimization\work\code\ADM\Sparse_LowRank_Mat\Matdata');

% get data
tic;
dataformat = 'surveillance-video-Hall';
opts = getdata(dataformat);
time_getdata = toc;
fprintf('%f seconds to get data ! \n', time_getdata);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Call ALM to solve the problem
tic; out_ALM = ALM_SADAL_smoothed(opts.D,opts); time_ALM = toc;
fprintf('*******************************************************************\n');
%%%%%%%%% print stats %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% fprintf('*******************************************************************\n');
% fprintf('ALM  : iter: %d, relX: %3.2e, relY: %3.2e, StopCrit: %3.2e, time: %f\n', ...
%     out_ALM.iter, out_ALM.relX, out_ALM.relY, out_ALM.StopCrit, time_ALM);

% plot the figures

j = 10;
subplot(3,3,1); imshow(reshape(opts.D(:,j),144,176),[]);
subplot(3,3,2); imshow(reshape(out_ALM.X(:,j),144,176),[]);
subplot(3,3,3); imshow(reshape(out_ALM.Y(:,j),144,176),[]);

j = 80;
subplot(3,3,4); imshow(reshape(opts.D(:,j),144,176),[]);
subplot(3,3,5); imshow(reshape(out_ALM.X(:,j),144,176),[]);
subplot(3,3,6); imshow(reshape(out_ALM.Y(:,j),144,176),[]);

j = 150;
subplot(3,3,7); imshow(reshape(opts.D(:,j),144,176),[]);
subplot(3,3,8); imshow(reshape(out_ALM.X(:,j),144,176),[]);
subplot(3,3,9); imshow(reshape(out_ALM.Y(:,j),144,176),[]);