classdef SwerlingIISqLawD
%%SWERLINGIISQLAWD This class implements various functions and detections
%                statistics related to the Swerling II target model when
%                used with a square law detector. All methods are static
%                (the class holds no information and does not need to be
%                instantiated).
%
%Implemented methods are: mean, var, PDF, CDF, PD4Threshold, PD4PFA,
%                         avgSNR4PDThresh, avgSNR4PDPFA, thresh4AvgSNRPD,
%                         rand
%
%Swerling models are given in [1] and in Chapter 11 of [2]. The Swerling II
%model assumes that the observed power SNR of the target varies in terms of
%an exponential distribution with rate parameter (1/avgSNR), that in a
%pulse train for detection the target SNR does fluctuate from pulse to
%pulse, and that a square law detector is used to incoherently integrate
%multiple pulses. The difference from a Swerling I model is the target
%amplitude fluctuates from pulse to pulse.
%
%The square law detector for N samples is
%y=sum_{i=1}^N r_i^2
%where r_i is the ith real amplitude. The model for the amplitude given a
%target amplitude or power is developed in Chapter 9 of [2]. The squared
%real amplitude is r^2_i=y_{I,i}^2+y_{Q,i}^2 where y_{I,i} and y_{Q,i} are
%the in-phase and quadrature components of the filter output. Define
%Rp=2*sampPowSNR, as in Equation 9.2-34. The sampled power SNR of the
%target in the Swerling II model is a different random value for each
%pulse. The sample power SNR is sampPowSNR=ExponentialD.rand(1,1/avgSNR)
%as mentioned in [1]. The value y=y_{I,i}+sqrt(-1)*y_{Q,i} conditioned on
%the sampled power SNR is modeled as being distributed circularly complex
%Gaussian with mean sqrt(Rp) and variance 2. (See, Equation 9.3-35a in
%[2]). The variance being 2 simply reflects having normalized the variance
%on the I and Q components each to 1.  As in Equation 10.4-13 of [2], the
%distribution of Y=y/2 conditioned on the target SNR values is noncentral
%chi-squared with a change of variables. The value y conditioned on the
%target SNR values is noncentral chi-squared with nu=2*N degrees of freedom
%and lambda=N*Rp as the noncentrality parameter. The expressions in [1] and
%[2] are in terms of Y and not y, so a transformation has to be performed
%to make things in terms of y.
%
%Under this model, for a given average power SNR value, one can generate a
%random sample from the distribution as
% sample=0;
% for curPulse=1:N
%     sampPowSNR=ExponentialD.rand(1,1/avgSNR);
%     Rp=2*sampPowSNR;%The peak power of the signal from the SNR, Equation
%                 %9.4-8 in [2].
%     A=sqrt(Rp);%Equation 9.4-8, the amplitude of the signal
%     sample=sample+sum(abs(ComplexGaussianD.rand(1,A,2)).^2);
% end
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%[2] J. V. Di Franco and W. L. Rubin, Radar Detection. SciTech Publishing
%    Inc., Rayliegh, NC: 2004.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

methods(Static)
function val=mean(avgSNR,N)
%%MEAN Obtain the mean of the distribution of the detection power
%      (normalized to a noise variance of 1) under a Swerling II model in a
%      square-law detector.
%
%INPUTS:avgSNR The average power signal to noise ratio of the target.
%        N The number of pulses that are to be incoherently added for
%          detection (in a square-law detector). In a Swerling II model, a
%          different realization of the target power is used across each
%          pulse and the noise corrupting the pulses varies. If this
%          parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: val The value of the mean for the given parameters.
%
%The expression for the mean implemented here is E{x}, where the expected
%value is taken over the PDF in Equation III.10 of [1]. However, the first
%moment from that expression has to be multiplied by 2, because as seen in
%Equation 10.4-4 of [2], the definition in [2], which is the same as in
%[1], is in terms of a scaled version of the square law detector.
%
%EXAMPLE:
%Here, we validate the mean by generating random samples and comparing the
%computed mean to the sample mean.
% avgSNR=2;
% N=4;
% numSamples=1e5;
% meanVal=SwerlingIISqLawD.mean(avgSNR,N)
% meanSampVal=mean(SwerlingIISqLawD.rand([numSamples,1],avgSNR,N))
%One will see that both mean values are about 24.
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%[2] J. V. Di Franco and W. L. Rubin, Radar Detection. SciTech Publishing
%    Inc., Rayliegh, NC: 2004.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<2||isempty(N))
        N=1;
    end

    val=2*N.*(1+avgSNR);
end

function val=var(avgSNR,N)
%%VAR Obtain the variance of the distribution of the detection power
%     (normalized to a noise variance of 1) under a Swerling II model in a
%     square-law detector.
%
%INPUTS:avgSNR The average power signal to noise ratio of the target.
%        N The number of pulses that are to be incoherently added for
%          detection (in a square-law detector). In a Swerling II model, a
%          different realization of the target power is used across each
%          pulse and the noise corrupting the pulses varies. If this
%          parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: val The value of the variance for the given parameters.
%
%To derive the formula implemented here, the first and second noncentral
%moments were obtained by finding E{x^2} and E{x} integrating over the PDF
%in Equation III.10 in [1].  One then uses the identity that
%variance=E{x^2}-E{x}^2
%However, the variance from that expression has to be multiplied by 4,
%because as seen in Equation 10.4-4 of [2], the definition in [2], which is
%the same as the definition in [1], is in terms of a scaled version of the
%square law detector.
%
%EXAMPLE:
%Here, we validate the variance by generating random samples and comparing
%the computed variance to the sample variance.
% avgSNR=2;
% N=4;
% numSamples=1e5;
% varVal=SwerlingIISqLawD.var(avgSNR,N)
% varSampVal=var(SwerlingIISqLawD.rand([numSamples,1],avgSNR,N))
%One will see that both variance values are about 144.
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%[2] J. V. Di Franco and W. L. Rubin, Radar Detection. SciTech Publishing
%    Inc., Rayliegh, NC: 2004.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<2||isempty(N))
        N=1;
    end

    val=4*N*(1+avgSNR)^2;
end

function val=PDF(v,avgSNR,N)
%%PDF Evaluate the scalar probability density function (PDF) of the
%     distribution of the detection power (normalized to a noise variance
%     of 1) under a Swerling II model in a square-law detector.
%
%INPUTS: v The point or points at which the PDF should be evaluated. v>0
%          for nonzero PDF values.
%   avgSNR The average power signal to noise ratio of the target.
%        N The number of pulses that are to be incoherently added for
%          detection (in a square-law detector). In a Swerling II model, a
%          different realization of the target power is used across each
%          pulse and the noise corrupting the pulses varies. If this
%          parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: val The values of the PDF evaluated at the given points.
%
%The PDF is taken from Equation III.10 of [1]. However the PDF is derived
%based on a scaled square-law detector, as in Equation 10.4-4 in [2], which
%uses the same definition as in [1]. Thus, in implementing the PDF, the
%scaling is removed.
%
%EXAMPLE:
%Here, we validate the PDF by generating random samples and comparing the
%PDF plot with a histogram of the random samples.
% avgSNR=2;
% N=4;
% numSamples=10e3;
%
% figure(1)
% clf
% histogram(SwerlingIISqLawD.rand([numSamples,1],avgSNR,N),'Normalization','pdf')
% hold on
% numPoints=1000;
% x=linspace(0,80,numPoints);
% vals=SwerlingIISqLawD.PDF(x,avgSNR,N);
% plot(x,vals,'linewidth',2)
% h1=xlabel('x');
% h2=ylabel('PDF(x)');
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
% set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
%One will see that the histogram matches well with the plot.
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%[2] J. V. Di Franco and W. L. Rubin, Radar Detection. SciTech Publishing
%    Inc., Rayliegh, NC: 2004.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    %Initial change of variable to undo the scaling from Equation 10.4-4 in
    %[2].
    v=v/2;

    val=exp((N-1)*log(v)-N*log(1+avgSNR)-gammaln(N)-v/(1+avgSNR));

    %Adjust for the change of variable to undo the scaling from Equation
    %10.4-4 in [2].
    val=val/2;

    val(v<0)=0;
end

function val=CDF(v,avgSNR,N)
%%CDF Evaluate the scalar cumulative distribution function (CDF) of the
%     distribution of the detection power (normalized to a noise variance
%     of 1) under a Swerling II model in a square-law detector.
%
%INPUTS: v The point or points at which the CDF should be evaluated. v>0
%          for nonzero CDF values.
%   avgSNR The average power signal to noise ratio of the target.
%        N The number of pulses that are to be incoherently added for
%          detection (in a square-law detector). In a Swerling II model, a
%          different realization of the target power is used across each
%          pulse and the noise corrupting the pulses varies. If this
%          parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: val The values of the CDF evaluated at the given points.
%
%The derivation of the detection probability in [1] is as 1-the value of
%the CDF. Thus, this function just evaluated
%1-SwerlingIISqLawD.PD4Threshold(avgSNR,v,N).
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(N))
        N=1;
    end

    val=1-SwerlingIISqLawD.PD4Threshold(avgSNR,v,N);
    val(v<=0)=0;
end

function PD=PD4Threshold(avgSNR,thresh,N)
%%PD4THRESHOLD Determine the detection probability of a Swerling II target
%           given its signal to noise ratio and the value of the
%           detection threshold, assuming that a square-law detector is
%           used. The noise in each sample is assumed to have variance 1
%           (normalized noise power).
%
%INPUTS: avgSNR A vector or matrix of average power signal to noise ratios
%               of the target at which one wishes to evaluate the detection
%               probability.
%        thresh The scalar normalized detection threshold to use. This is
%               the threshold to use if the noise variance is 1.
%             N The number of pulses that are to be incoherently added for
%               detection (in a square-law detector). In a Swerling II
%               model, a different realization of the target power is used
%               across each pulse and the noise corrupting the pulses
%               varies. If this parameter is omitted or an empty matrix is
%               passed, N=1 is used.
%
%OUTPUTS: PD The detection probability of the target.
%
%This function implements Equation II.10 in [1]. However, the expression
%used is derived based on a scaled square-law detector, as in Equation
%10.4-4 in [2], which uses the same definition as in [1]. Thus, the
%threshold in this function is scaled appropriately.
%
%EXAMPLE:
%Here, we validate the results by comparing the PD from this function to
%the PD computed using random samples.
% avgSNR=2;
% thresh=30;
% N=4;
% PD=SwerlingIISqLawD.PD4Threshold(avgSNR,thresh,N)
% numSamples=1e5;
% PDSamp=mean(SwerlingIISqLawD.rand([numSamples,1],avgSNR,N)>=thresh)
%One will see that both PD values are about 0.265.
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%[2] J. V. Di Franco and W. L. Rubin, Radar Detection. SciTech Publishing
%    Inc., Rayliegh, NC: 2004.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(N))
       N=1;
    end
    thresh=thresh/2;

    PD=1-PearsonsGammaInc(thresh./((1+avgSNR)*sqrt(N)),N-1);
end

function PD=PD4PFA(avgSNR,PFA,N)
%%PD4PFA Determine the detection probability of a Swerling II target given
%        its signal to noise ratio and the probability of false alarm
%        implied by a particular unspecified detection threshold, assuming
%        that a square law detector is used.
%
%INPUTS: avgSNR A vector or matrix of average power signal to noise ratios
%               of the target at which one wishes to evaluate the detection
%               probability.
%           PFA The probability of false alarm, 0<=PFA<1.
%             N The number of pulses that are to be incoherently added for
%               detection (in a square-law detector). In a Swerling II
%               model, a different realization of the target power is used
%               across each pulse and the noise corrupting the pulses
%               varies. If this parameter is omitted or an empty matrix is
%               passed, N=1 is used.
%
%OUTPUTS: PD The detection probability of the target, 0<=PD<=1.
%
%This function just calls PFA2SquareLawThreshold to convert the false alarm
%rate into a normalized detection threshold after which it calls
%SwerlingIISqLawD.PD4Threshold.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(N))
       N=1;
    end

    thresh=PFA2SquareLawThreshold(PFA,N);
    PD=SwerlingIISqLawD.PD4Threshold(avgSNR,thresh,N,method);
end

function avgSNR=avgSNR4PDThresh(PD,thresh,N)
%%AVGSNR4PDTHRESH Given a detection probabilty and a normalized detection
%       threshold (normalized in terms of the receiver noise having a unit
%       covariance), determine the average power signal to noise ratio
%       (SNR) of the target needed under a Swerling II model for one or
%       more pulses in a square-law detector.
%
%INPUTS: PD The detection probability of the target , 0<=PD<1.
%    thresh The scalar normalized detection threshold to use. This is the
%           threshold to use if the noise variance is 1.
%         N The number of pulses that are to be incoherently added for
%           detection (in a square-law detector). In a Swerling II model, a
%           different realization of the target power is used across each
%           pulse and the noise corrupting the pulses varies. If this
%           parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: avgSNR The average power signal to noise ratio needed to achieve
%                the desired probability of detection at the given
%                threshold. If this is negative, then for a given PD, the
%                threshold is so low that due to the high false alarm
%                rate, the desired PD is impossiblely low.
%
%Equation II.10 in [1] gives an expression for PD in terms of thresh and
%the average SNR. Here, we have simply inverted the expression.
%
%However, the expressions used are derived based on a scaled square-law
%detector, as in Equation 10.4-4 in [2], which is the same definition used
%in [1]. Thus, the threshold in this function is scaled appropriately.

%EXAMPLE:
%Here, we show that the results are consistent.
% N=4;
% PD=0.5;
% thresh=5;
% avgSNR=SwerlingIISqLawD.avgSNR4PDThresh(PD,thresh,N);
% PDBack=SwerlingIISqLawD.PD4Threshold(avgSNR,thresh,N)
% numSamples=1e5;
% PDSamp=mean(SwerlingIISqLawD.rand([numSamples,1],avgSNR,N)>=thresh)
%One will see that PDBack is the same as PD and about the same as PDSamp.
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%[2] J. V. Di Franco and W. L. Rubin, Radar Detection. SciTech Publishing
%    Inc., Rayliegh, NC: 2004.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(N))
       N=1;
    end

    %The division by 2 deals with the scaling in Equation 10.4-4.
    avgSNR=(thresh/2)/gammaincinv(1-PD,N)-1;
end

function avgSNR=avgSNR4PDPFA(PD,PFA,N)
%%AVGSNR4PDTHRESH Given a detection probabilty and the probability of false
%       alarm, determine the average power signal to noise ratio (SNR) of
%       the target needed under a  Swerling II model for one or more pulses
%       in a square-law detector.
%
%INPUTS: PD The detection probability of the target , 0<=PD<1.
%       PFA The probability of false alarm, 0<=PFA<1.
%         N The number of pulses that are to be incoherently added for
%           detection (in a square-law detector). In a Swerling II model, a
%           different realization of the target power is used across each
%           pulse and the noise corrupting the pulses varies. If this
%           parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: avgSNR The average power signal to noise ratio needed to achieve
%                the desired probability of detection at the given
%                threshold. If this is negative, then for a given PD, the
%                threshold is so low that due to the high false alarm
%                rate, the desired PD is impossiblely low.
%
%Equation II.10 in [1] gives an expression for PD in terms of a normalized
%threshold and the average SNR. Here, we call the function
%PFA2SquareLawThreshold and insert the result into
%SwerlingIISqLawD.avgSNR4PDThresh, which inverts the expression in [1].
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(N))
        N=1;
    end

    thresh=PFA2SquareLawThreshold(PFA,N);
    avgSNR=SwerlingIISqLawD.avgSNR4PDThresh(PD,thresh,N);
end

function thresh=thresh4AvgSNRPD(PD,avgSNR,N)
%%THRESH4AVGSNRPD Given a detection probability and the average power
%           signal to noise ratio (SNR), determine the threshold needed
%           under a Swerling II model for one or more pulses in a
%           square-law detector.
%
%INPUTS: PD The detection probability of the target , 0<=PD<1.
%    avgSNR A vector or matrix of average power signal to noise ratios
%           of the target at which one wishes to evaluate the threshold.
%         N The number of pulses that are to be incoherently added for
%           detection (in a square-law detector). In a Swerling II model, a
%           different realization of the target power is used across each
%           pulse and the noise corrupting the pulses varies. If this
%           parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: thresh The normnalized detection threshold(s) (assuming the noise
%                variance is 1).
%
%Equation II.10 in [1] gives an expression for PD in terms of a normalized
%threshold and the average SNR. Here, we simply invert the equation.
%
%However, the expressions used are derived based on a scaled square-law
%detector, as in Equation 10.4-4 in [2], which is the same defintiion used
%in [1]. Thus, the threshold in this function is scaled appropriately.
%
%EXAMPLE:
%Here, we show that the results are consistent.
% N=4;
% PD=0.5;
% avgSNR=2;
% thresh=SwerlingIISqLawD.thresh4AvgSNRPD(PD,avgSNR,N);
% PDBack=SwerlingIISqLawD.PD4Threshold(avgSNR,thresh,N)
% numSamples=1e5;
% PDSamp=mean(SwerlingIISqLawD.rand([numSamples,1],avgSNR,N)>=thresh)
%One will see that PDBack=0.5 and PDSamp is about the same.
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%[2] J. V. Di Franco and W. L. Rubin, Radar Detection. SciTech Publishing
%    Inc., Rayliegh, NC: 2004.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(N))
        N=1;
    end

    thresh=2*gammaincinv(1-PD,N).*(avgSNR+1);
end

function vals=rand(NDims,avgSNR,N)
%%RAND Generate random variables representing a target whose sampled signal
%      to average noise power ratio is generated according to a Swerling II
%      model detected by a square-law detector.
%
%INPUTS: NDims If NDims is a scalar, then rand returns an NDimsXNDims
%          matrix of random variables. If NDims=[M,N1] is a two-element row
%          vector, then rand returns an MXN1 matrix of random variables.
%   avgSNR The average power signal to noise ratio of the target.
%        N The number of pulses that are to be incoherently added for
%          detection (in a square-law detector). In a Swerling II model, a
%          different realization of the target power is used across each
%          pulse and the noise corrupting the pulses varies. If this
%          parameter is omitted or an empty matrix is passed, N=1 is used.
%
%OUTPUTS: vals A matrix whose dimensions are determined by N of the
%              generated random variables.
%
%Following the model of [1], a sample from the exponential distribution is
%performed to obtain the complex signal to noise ratio of every single one
%of the N pulses. Then, the square law detector output conditioned on the
%signal to noise ratio is generated directly using NonFlucSqLawD.rand for
%each pulse and the values are added.
%
%REFERENCES:
%[1] P. Swerling, "Probability of detection for fluctuating targets," The
%    RAND Corporation, Santa Monica, CA, Tech. Rep. RM-1217, 1954.
%
%March 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.

    if(nargin<3||isempty(N))
        N=1;
    end

    if(isscalar(NDims))
        dims=[NDims, NDims];
    else
        dims=NDims;
    end

    vals=zeros(dims);

    numEls=numel(vals);
    for curEl=1:numEls
        for curPulse=1:N
            %Equation I.1 for  the input signal-to-noise power ratio. It is
            %different for each sample in the pulse train in the Swerling
            %II model.
            curSNR=ExponentialD.rand(1,1/avgSNR);
            vals(curEl)=vals(curEl)+NonFlucSqLawD.rand(1,curSNR,1);
        end
    end
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.


%% check S1Grid/find

x = S1Grid([1,2,3,4,9],0,10)
x = S1Grid([1,2,3,4,9,9.8],0,10,'periodic')

find(x,4,5)

x = S1Grid(0:39990,0,40000,'periodic');
y = linspace(0,39000,3000);

tic
for i = 1:100
  ind = find(x,y,50);
end
toc

tic
for i = 1:1
  for j = 1:length(y)
    find(x,y(j),50);
  end
end
toc

%% check S2Grid/find

x = equispacedS2Grid('points',500);
plot(subGrid(x,find(x,xvector,10*degree)));
full(find(x,xvector,10*degree))

x = equispacedS2Grid('points',5000);
y = vector3d(equispacedS2Grid('points',100));

tic
for i = 1:length(y)
  find(x,y(i),0.5);
end
toc

tic
for i = 1:100
  find(x,y,0.5);
end
toc

%% check SO3Grid/find

cs = crystalSymmetry('trigonal');
ss = specimenSymmetry('1');

x = equispacedSO3Grid(cs,ss,'points',100000);
y = equispacedSO3Grid(cs,ss,'points',100000);

tic
angle_outer(x,y,5*degree);
toc

tic
find(x,quaternion(y),5*degree);
toc

find(x,quaternion.id,20*degree)

q = axis2quat(xvector+yvector,45*degree);
q = quaternion.id;

sx = quaternion(subGrid(x,find(x,q,10*degree)));

dist(cs,specimenSymmetry,q,sx) / degree

plot(inv(sx)*xvector)
plot(inv(sx)*yvector)
plot(inv(sx)*zvector)

A = cos(ay)*cos(cy)*cos(ax)*(cos(by)*cos(bx)-1) - ...
  sin(ay)*sin(cy)*cos(ax)*(cos(bx)-cos(by));

B = cos(ay)*cos(cy)*cos(ax)*(cos(by)*cos(bx)-1) - ...
  sin(ay)*sin(cy)*cos(ax)*(cos(bx)-cos(by));


function [E,J]=SynthMeasWatsonSHCylSingleRadIsoV_GPD_B0(x, protocol, fibredir, roots)
% Substrate: Impermeable cylinders with one radius in a homogeneous background.
% Orientation distribution: Watson's distribution with SH approximation
% Signal approximation: Gaussian phase distribution.
% Notes: This version includes an isotropic diffusion compartment with its own
% diffusivity.
% Includes a free parameter for the measurement at b=0.
%
% [E,J]=SynthMeasWatsonSHCylSingleRadIsoV_GPD_B0(x, protocol, fibredir, roots)
% returns the measurements E according to the model and the Jacobian J of the
% measurements with respect to the parameters.  The Jacobian does not
% include derivates with respect to the fibre direction.
%
% x is the list of model parameters in SI units:
% x(1) is the volume fraction of the intracellular space.
% x(2) is the free diffusivity of the material inside and outside the cylinders.
% x(3) is the hindered diffusivity outside the cylinders in perpendicular directions.
% x(4) is the radius of the cylinders.
% x(5) is the concentration parameter of the Watson's distribution.
% x(6) is the volume fraction of the isotropic compartment.
% x(7) is the diffusivity of the isotropic compartment.
% x(8) is the measurement at b=0.
%
% protocol is the object containing the acquisition protocol.
%
% fibredir is a unit vector along the symmetry axis of the Watson's
% distribution.  It must be in Cartesian coordinates [x y z]' with size [3 1].
%
% roots contains solutions to the Bessel function equation from function
% BesselJ_RootsCyl.
%
% author: Gary Hui Zhang (gary.zhang@ucl.ac.uk)
%

S0 = x(8);

% Call the other function to get normalized measurements.
if(nargout == 1)
    Enorm=SynthMeasWatsonSHCylSingleRadIsoV_GPD(x, protocol, fibredir, roots);
else
    [Enorm,Jnorm]=SynthMeasWatsonSHCylSingleRadIsoV_GPD(x, protocol, fibredir, roots);
end

E = Enorm*S0;

if(nargout>1)
    J = Jnorm*S0;
    [meas, params] = size(J);
    J(:,params+1) = Enorm;
end


% wavelet_factory_1d: Create wavelet cascade from filters
% Usage
%    [Wop, filters] = wavelet_factory_1d(N, filter_options, scat_options)
% Input
%    N: The size of the signals to be transformed.
%    filter_options: The filter options, same as for filter_bank.
%    scat_options: General options to be passed to wavelet_1d and
%        wavelet_layer_1d. Contains scat_options.M, which determines the max-
%        imal order of the scattering transform.
% Output
%    Wop: A cell array of wavelet transforms needed for the scattering trans-
%       form.
%    filters: A cell array of the filters used in defining the wavelets.

function [Wop, filters] = renorm_wavelet_layer_factory_1d(N, filter_options, scat_options)
	filters = filter_bank(N, filter_options);

	for m = 0:scat_options.M
		filt_ind = min(numel(filters), m+1);
		Wop{m+1} = @(X)(renorm_layers_wavelet_1d(X, filters{filt_ind}, ...
			scat_options));
	end
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% Laplace Transform properties

% Differentiation in the time-domain

x=cos(t);

L=diff(x,t) ;
laplace(L,s)

X=laplace(x,s);
x0=1;
R=s*X-x0;
simplify(R)


function runtop
% See Article 8.6
a=.01; h=4; b=2; n=6; m=3; k=20;
psi=45; theta=-45; phi=0;
for j=1:2:720

   psi=180+j; theta=-45+5*sin(.15*psi); phi=1.5*j;
   topdraw(a,h,b,8,m,18,psi,theta,phi);

   % topdraw(a,h,b,8,m,18,j,-45,1.5*j);
end
pause(1),%  close

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

function [x,y, z]=topdraw(a,h,b,n,m,k,psi,theta,phi)
% [x,y, z]=topdraw(a,h,b,n,m,k,psi,theta,phi)

% [X,Y,Z]=topsurf(n,m,k,a,h,b)
if nargin==0
  a=.01; h=4; b=2; n=6; m=3; k=20;
  psi=45; theta=-45; phi=0;
end
%s=sqrt(h^2+(a-b)^2); uax=cumsum([0,a,s,b]);
%u=cornrpts(uax/uax(4),nax+1);
%v=linspace(0,1,ncrc+1);

[dx,dy,dz]=topsurf(n,m,k,a,h,b);

x=dz; y=dx; z=dy;

m=eulerang(psi,theta,phi);
p=size(x); v=[x(:),y(:),z(:)]*m;
x=reshape(v(:,1),p); y=reshape(v(:,2),p);
z=reshape(v(:,3),p);
w=[-1 1 -1 1 -1 1]*sqrt(h^2+b^2);

%rotate3d on;
clf; surf(x,y,z),  view([-45,30])
axis equal; axis(w); axis on
xlabel('x axis'), ylabel('y axis')
zlabel('z axis')
title('NUTATING TOP PRECESSION')
colormap([127/255 1 212/255]); drawnow, shg

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

function m=eulerang(psi,theta,phi)
% m=eulerang(psi,theta,phi)
a=pi/180*[psi,theta,phi]; c=cos(a); s=sin(a);
m=[c(1)*c(2), s(1)*c(2), -s(2); -s(1)*c(3)+...
   c(1)*s(2)*s(3), c(1)*c(3)+s(1)*s(2)*s(3),...
   c(2)*s(3); s(1)*s(3)+c(1)*s(2)*c(3),...
  -c(1)*s(3)+s(1)*s(2)*c(3), c(2)*c(3)];

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

function [X,Y,Z]=topsurf(n,m,k,a,h,b)
% [X,Y,Z]=topsurf(n,m,k,a,h,b)
if nargin==0
n=8; m=4; k=20; a=.2; h=4; b=1;
end
tol=100*eps*(a+h+b);
a=a+(a==0)*tol; b=b+(b==0)*tol;

D=2*pi/n; u=cos(D/2); v=sin(D/2);
z=u+i*linspace(-v,v,m+1)'; z(m+1)=[];
z=z*exp(i*D*(0:n-1)); z=[z(:);u-i*v];
x=real(z); y=imag(z); N=length(x);
% plot(x,y,x,y,'.'), axis equal, shg, pause

qd=cumsum([0;a;sqrt(h^2+(a-b)^2);b]);
qd=qd/max(qd); q=cornrpts(qd,k);
K=length(q);
Z=interp1(qd,[0;0;h;h],q(:))*ones(1,N);
r=interp1(qd,[0;a;b;0],q); r=r(:);
X=r*x(:)'; Y=r*y(:)';

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

function v=cornrpts(u,N)
% v=cornrpts(u,N)
% This function generates a set of approximately
% N points between min(u) and max(u) including
% all points in u plus additional points evenly
% spaced in each successive interval.
% u   -  vector of points
% N   -  approximate number of output points
%        between min(u(:)) and max(u(:))
% v   -  vector of points in increasing order
u=sort(u(:))'; np=length(u); d=u(np)-u(1); v=u(1);
for j=1:np-1
  dj=u(j+1)-u(j); nj=max(1,fix(N*dj/d));
  v=[v,[u(j)+dj/nj*(1:nj)]];
end


function det = r8cb_det ( n, ml, mu, a_lu )

%*****************************************************************************80
%
%% R8CB_DET computes the determinant of a R8CB matrix factored by R8CB_NP_FA.
%
%  Discussion:
%
%    The R8CB storage format is appropriate for a compact banded matrix.
%    It is assumed that the matrix has lower and upper bandwidths ML and MU,
%    respectively.  The matrix is stored in a way similar to that used
%    by LINPACK and LAPACK for a general banded matrix, except that in
%    this mode, no extra rows are set aside for possible fillin during pivoting.
%    Thus, this storage mode is suitable if you do not intend to factor
%    the matrix, or if you can guarantee that the matrix can be factored
%    without pivoting.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer ML, MU, the lower and upper bandwidths.
%    ML and MU must be nonnegative, and no greater than N-1.
%
%    Input, real A_LU(ML+MU+1,N), the LU factors from R8CB_NP_FA.
%
%    Output, real DET, the determinant of the matrix.
%
  det = prod ( a_lu(mu+1,1:n) );

  return
end


function [yPred,PInvPred]=discEIFPred(yPrev,PInvPrev,f,FJacob,Q,FHessian)
%%DISCEIFPRED Perform the discrete-time prediction step that comes with
%             the first and second-order extended information filter (EIF).
%             As the extended information filter as presented in [1] has no
%             real propagation step, this just extracts the state, calls
%             discEKFPred, and converts the result back to an information
%             state. This function is useful if one is using an information
%             state because of how the measurement update in an information
%             filter is performed.
%
%INPUTS:    yPrev   The xDimX1 information state at the previous time-step.
%                   The information state is the inverse covariance matrix
%                   times the target state.
%        PInvPrev   The xDimXxDim inverse of the state covariance matrix at
%                   the previous time-step.
%               f   A function handle for the state transition function
%                   that takes the state as its parameter.
%           FJacob  A function handle for calculating the xDim X xDim
%                   Jacobian of f, or the xDim X xDim Jacobian matrix
%                   itself. If an empty matrix is passed, then
%                   FJacob will be found using numerical differentiation
%                   via the numDiff function with default parameters.
%               Q   The xDimX xDim process noise covariance matrix.
%         FHessian  This parameter is only provided if a second-order EKF
%                   is desired. This is either a function handle for the
%                   state transition Hessian hypermatrix, or it is the
%                   state transition Hessian hypermatrix itself. The matrix
%                   is xDim X xDim X xDim. The matrix FH=FHessian(x) is
%                   such that FH(i,j,k) is the second derivative of the
%                   kth element of the vector returned by f with
%                   respect to the ith and jth components of x. The Hessian
%                   matrix is symmetric. If this parameter is omitted, a
%                   first-order filter is used.
%
%OUTPUTS:   yPred    The xDim X 1 predicted information state vector.
%           PInvPred The predicted xDim X xDim inverse state covariance
%                    matrix.
%
%REFERENCES:
%[1] K. P. B. Chandra, D.-W. Gu, and I. Postlethwaite, "Square root
%    cubature information filter," IEEE Sensors Journal, vol. 13, no. 2,
%    pp. 750-758, Feb. 2013.
%
%October 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<6)
    FHessian=[];
end

PPrev=pinv(PInvPrev);
xPrev=PPrev*yPrev;

[xPred, PPred]=discEKFPred(xPrev,PPrev,f,FJacob,Q,FHessian);

PInvPred=pinv(PPred);
yPred=PInvPred*xPred;

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 varargout = optimN(varargin)
% Full multigrid matrix solver stuff (circulant boundaries)
%_______________________________________________________________________
%
% FORMAT v = optimN('fmg',A, b, param)
% v     - the solution n1*n2*n3*n4
% A     - parameterisation of 2nd derivatives
%         n1*n2*n3*(n4*(n4+1)/2)
%         The first n4 volumes are the diagonal elements, which are
%         followed by the off-diagonals (note that 2nd derivs are%
%         symmetric).  e.g. if n4=3, then the ordering would be
%         (1,1),(2,2),(3,3),(1,2),(1,3),(2,3)
% b     - parameterisation of first derivatives n1*n2*n3*n4
% param - 6 parameters (settings)
%         - [1] Regularisation type, can take values of
%           - 1 Membrane energy
%           - 2 Bending energy
%         - [2][3][4] Voxel sizes
%         - [5][6][7] Regularisation parameters
%           - For membrane and bending energy, the parameters
%             are lambda, unused and id.
%         - [8] Number of Full Multigrid cycles
%         - [9] Number of relaxation iterations per cycle
%
%           Note that more cycles and iterations may be needed
%           for bending energy than for membrane energy.
%
% Solve equations using a Full Multigrid method.  See Press et al
% for more information.
% v = inv(A+H)*b
% A, b and v are all single precision floating point.
% H is a large sparse matrix encoded by param(1:7).
% The tensor field encoded by A MUST be positive-definite. If it is not,
% then anything could happen (see references about Fisher scoring for
% help on ensuring that second derivatives are positive definite).
%
%_______________________________________________________________________
%
% FORMAT m = optimN('vel2mom', v, param)
% v     - velocity (flow) field n1*n2*n3*n4.
% param - 4 parameters (settings)
%         - [1] Regularisation type, can take values of
%           - 1 Membrane energy
%           - 2 Bending energy
%         - [2][3][4] Voxel sizes
%         - [5][6][7] Regularisation parameters
%           - For membrane and bending energy, the parameters
%             are lambda, unusaed and id.
% m       - `momentum' field n1*n2*n3*n4.
%
% Convert a flow field to a momentum field by m = H*v, where
% H is the large sparse matrix encoding some form of regularisation.
% v and m are single precision floating point. This function has uses
% beyond only image registration.
%
%_______________________________________________________________________
%
% Note that the boundary conditions are circulant throughout.
% For Neumann boundary conditions (zero gradients at the boundaries)
% use optimNn.
%
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: optimN.m 4758 2012-05-29 15:34:08Z john $

[varargout{1:nargout}] = optim_compat(0,varargin{:});

%error('Not compiled for %s in MATLAB %s  (see make.m)\n', computer, version);


function [A] = spm_matrix(P, order)
% returns an affine transformation matrix
% FORMAT [A] = spm_matrix(P, order)
% P(1)  - x translation
% P(2)  - y translation
% P(3)  - z translation
% P(4)  - x rotation about - {pitch} (radians)
% P(5)  - y rotation about - {roll}  (radians)
% P(6)  - z rotation about - {yaw}   (radians)
% P(7)  - x scaling
% P(8)  - y scaling
% P(9)  - z scaling
% P(10) - x affine
% P(11) - y affine
% P(12) - z affine
%
% order (optional) application order of transformations.
%
% A     - affine transformation matrix
%___________________________________________________________________________
%
% spm_matrix returns a matrix defining an orthogonal linear (translation,
% rotation, scaling or affine) transformation given a vector of
% parameters (P).  By default, the transformations are applied in the
% following order (i.e., the opposite to which they are specified):
%
% 1) shear
% 2) scale (zoom)
% 3) rotation - yaw, roll & pitch
% 4) translation
%
% This order can be changed by calling spm_matrix with a string as a
% second argument. This string may contain any valid MATLAB expression
% that returns a 4x4 matrix after evaluation. The special characters 'S',
% 'Z', 'R', 'T' can be used to reference the transformations 1)-4)
% above. The default order is 'T*R*Z*S', as described above.
%
% SPM uses a PRE-multiplication format i.e. Y = A*X where X and Y are 4 x n
% matrices of n coordinates.
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id$

% pad P with 'null' parameters
%---------------------------------------------------------------------------
q  = [0 0 0 0 0 0 1 1 1 0 0 0];
P  = [P q((length(P) + 1):12)];

% default multiplication order if not specified
%---------------------------------------------------------------------------
if nargin < 2
    order = 'T*R*Z*S';
end;

T  =   [1   0   0   P(1);
        0   1   0   P(2);
        0   0   1   P(3);
        0   0   0   1];

R1  =  [1    0      0          0;
        0    cos(P(4))  sin(P(4))  0;
        0   -sin(P(4))  cos(P(4))  0;
        0    0      0          1];

R2  =  [cos(P(5))  0    sin(P(5))  0;
        0          1    0      0;
       -sin(P(5))  0    cos(P(5))  0;
        0          0    0          1];

R3  =  [cos(P(6))   sin(P(6))   0  0;
       -sin(P(6))   cos(P(6))   0  0;
        0           0           1  0;
        0           0       0  1];

R   = R1*R2*R3;

Z   =  [P(7)    0       0       0;
        0       P(8)    0       0;
        0       0       P(9)    0;
        0       0       0       1];

S   =  [1       P(10)   P(11)   0;
        0       1   P(12)   0;
        0       0       1   0;
        0       0       0       1];

A = eval(sprintf('%s;', order));
if ~isnumeric(A) || ndims(A) ~= 2 || any(size(A) ~= 4)
    error('Order expression ''%s'' did not return a valid 4x4 matrix.', ...
          order);
end;


function [] = Resultant_Oscillation( Nb,n )

% Nb=10;
% n=10;
% figure(1)
% hold on

R=100;

psi=[0:1:360]*pi/180;

npsi=length(psi);

bih1=sin(psi);

for i=0:Nb-1
    bihn(i+1,:)=sin(n*(2*i*pi/Nb+psi));
end
resultant=sum(bihn);
if max(resultant)>1e-3
    resultant=resultant/max(resultant);
end

% title('Resultant Oscillation Transmitted to Hull','Position',[179.129 1.007 17.321],'FontWeight','Bold')
hold on
axis([0,360,-1,1])
Waves=plot(psi*180/pi,resultant,'r',  psi*180/pi,bih1,'b');
% legend('Transmitted','1 Per rev','Location','BestOutside')
set(gca,'ytick',0)
set(gca,'xtick',[0:90:360])
set(gca,'XAxisLocation','top')
set(gca,'XTickLabel',[])
set(gca,'YTickLabel',[])

grid on
save DataFile1 Waves


function [SDANN, SDNNI] = CalcSDANN(windows_all, tNN, NN, HRVparams)
% [SDANN, SDNNI] = CalcSDANN(windows_all, tNN, NN, HRVparams)
%
%   OVERVIEW:  Calculates SDANN and SDNNindex for all segments
%              of a specified constant length with no overlap.
%
%   INPUT:     windows_all :
%              NN          : a single column of NN (normal normal) interval
%                            data in seconds
%              tNN         : the time indices of the rr interval data (seconds)
%              HRVparams   : settings struct, including segment length - the
%                            length of the interval requested
%                            (it is defaulted at 5minutes == 300 seconds)
%              NOTE: seglength must be specified in the same units as <times>
%
%   OUTPUT:     SDANN : standard deviation of the averages of values
%               SDNNI : mean of the standard deviations of all values
%
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Main script written by Adriana N. Vest
%       Dependent scripts written by various authors
%       (see functions for details)
%	COPYRIGHT (C) 2016
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information
%%

if nargin<3
    error('not enough input arguments!');
end

segmentlength = HRVparams.windowlength;

for i = 1:length(windows_all)
    if ~isnan(windows_all(i))
        idx = find(tNN >= windows_all(i) & tNN < windows_all(i) + segmentlength);

        nn_win = NN(idx);

        sm(i) = mean(nn_win);     % mean of each segment
        sstd(i) = std(nn_win);    % stdev of each segment
    else
        sm(i) = NaN;
        sstd(i) = NaN;
    end
end

% Remove NaN values from calculation
seg_mean = sm;
seg_stdev = sstd;
idxrem = find(isnan(sm));
seg_mean(idxrem) = [];
seg_stdev(idxrem) = [];

SDANN = std(seg_mean);              % stdev of means of each segment
SDNNI = mean(seg_stdev);            % mean of stdevs from each segment

end


%{
load('dataset/trafficdb/traffic_patches.mat');
[M,m,n,p] = convert_video3d_to_2d(im2double(imgdb{100}));
out = run_algorithm('MC', 'SVP', M, [])
show_results(M.*out.Omega,out.L,out.S,out.O,p,m,n);
%}

warning('off','all');
MIdx = M(Idx);
rank = 2;
[U,S,V] = svp(Idx,MIdx,size(M,1),size(M,2),rank);
L = U*diag(S)*V'; % low-rank
S = (M - L); % sparse
warning('on','all');


function V = pcapred_decomp (X,T,options)
% it assumes that X is centered

is_cell_strings = iscell(X) && ischar(X{1});
is_cell_matrices = iscell(X) && ~ischar(X{1});
is_struct = ~is_cell_strings && ~is_cell_matrices && isstruct(X);

orders = formorders(options.order,options.orderoffset,options.timelag,options.exptimelag);

N = 0;
if is_cell_strings || is_cell_matrices
    [~,XX] = loadfile(X{1},T{1},options);
    if options.zeromean==0, XX = XX(:,2:end); end
    XX2 = (XX' * XX);
    N = N + size(XX,1);
    for n=2:length(T)
        [~,XX] = loadfile(X{n},T{n},options);
        if options.zeromean==0, XX = XX(:,2:end); end
        XX2 = XX2 + (XX' * XX);
        N = N + size(XX,1);
    end
elseif is_struct
    XX = formautoregr(X.X,T,orders,options.maxorder,1,1);
    N = size(XX,1);
    XX2 = (XX' * XX);
else
    XX = formautoregr(X,T,orders,options.maxorder,1,1);
    N = size(XX,1);
    XX2 = (XX' * XX);
end

[V,~,~] = svd( XX2 / (N-1) );
V = V(:,(1:options.pcapred) + (options.vcomp-1) );

end


function [simulated] = ft_connectivitysimulation(cfg)

% FT_CONNECTIVITYSIMULATION simulates channel-level time-series data with a
% specified connectivity structure. This function returns an output data
% structure that resembles the output of FT_PREPROCESSING.
%
% Use as
%   [data] = ft_connectivitysimulation(cfg)
% where the configuration structure should contain:
%   cfg.method      = string, can be 'linear_mix', 'mvnrnd', 'ar', 'ar_reverse' (see below)
%   cfg.nsignal     = scalar, number of signals
%   cfg.ntrials     = scalar, number of trials
%   cfg.triallength = in seconds
%   cfg.fsample     = in Hz
%
% Method 'linear_mix' implements a linear mixing with optional time shifts
% where the number of unobserved signals can be different from the number
% of observed signals
%
% Required configuration options:
%   cfg.mix    = matrix, [nsignal x number of unobserved signals]
%                specifying the mixing from the unobserved signals to
%                the observed signals, or
%              = matrix, [nsignal x number of unobserved signals x number of
%                samples] specifying the mixing from the
%                unobserved signals to the observed signals which
%                changes as a function of time within the trial
%              = cell-arry, [1 x ntrials] with each cell a matrix as
%                specified above, when a trial-specific mixing is
%                required
%   cfg.delay  = matrix, [nsignal x number of unobserved signals]
%                specifying the time shift (in samples) between the
%                unobserved signals and the observed signals
%
% Optional configuration options:
%   cfg.bpfilter  = 'yes' (or 'no')
%   cfg.bpfreq    = [bplow bphigh] (default: [15 25])
%   cfg.demean    = 'yes' (or 'no')
%   cfg.baselinewindow = [begin end] in seconds, the default is the complete trial
%   cfg.absnoise  = scalar (default: 1), specifying the standard deviation of
%                   white noise superimposed on top of the simulated signals
%   cfg.randomseed = 'yes' or a number or vector with the seed value (default = 'yes')
%
% Method 'mvnrnd' implements a linear mixing with optional timeshifts in
% where the number of unobserved signals is equal to the number of observed
% signals. This method used the MATLAB function mvnrnd. The implementation
% is a bit ad-hoc and experimental, so users are discouraged to apply it.
% The time shift occurs only after the linear mixing, so the effect of the
% parameters on the simulation is not really clear. This method will be
% disabled in the future.
%
% Required configuration options:
%   cfg.covmat    = covariance matrix between the signals
%   cfg.delay     = delay vector between the signals in samples
%
% Optional configuration options:
%   cfg.bpfilter  = 'yes' (or 'no')
%   cfg.bpfreq    = [bplow bphigh] (default: [15 25])
%   cfg.demean    = 'yes' (or 'no')
%   cfg.baselinewindow = [begin end] in seconds, the default is the complete trial
%   cfg.absnoise  = scalar (default: 1), specifying the standard
%                   deviation of white noise superimposed on top
%                   of the simulated signals
%
% Method 'ar' implements a multivariate autoregressive model to generate
% the data.
%
% Required cfg options:
%   cfg.params   = matrix, [nsignal x nsignal x number of lags] specifying the
%                  autoregressive coefficient parameters. A non-zero
%                  element at cfg.params(i,j,k) means a
%                  directional influence from signal j onto
%                  signal i (at lag k).
%   cfg.noisecov = matrix, [nsignal x nsignal] specifying the covariance
%                  matrix of the innovation process
%
% Method 'ar_reverse' implements a multivariate autoregressive
% autoregressive model to generate the data, where the model coefficients
% are reverse-computed, based on the interaction pattern specified.
%
% Required cfg options:
%   cfg.coupling = nxn matrix, specifying coupling strength, rows causing
%                   column
%   cfg.delay    = nxn matrix, specifying the delay, in seconds, from one
%                   signal's spectral component to the other signal, rows
%                   causing column
%   cfg.ampl     = nxn matrix, specifying the amplitude
%   cfg.bpfreq   = nxnx2 matrix, specifying the lower and upper frequencies
%                   of the bands that are transmitted, rows causing column
%
% The generated signals will have a spectrum that is 1/f + additional
% band-limited components, as specified in the cfg.
%
% See also FT_FREQSIMULATION, FT_DIPOLESIMULATION, FT_SPIKESIMULATION,
% FT_CONNECTIVITYANALYSIS

% Copyright (C) 2009-2015, Donders Institute for Brain, Cognition and Behaviour
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% these are used by the ft_preamble/ft_postamble function and scripts
ft_revision = '$Id$';
ft_nargin   = nargin;
ft_nargout  = nargout;

% do the general setup of the function
ft_defaults
ft_preamble init
ft_preamble debug
ft_preamble provenance
ft_preamble randomseed
ft_preamble trackconfig

% the ft_abort variable is set to true or false in ft_preamble_init
if ft_abort
  return
end

% check input configuration for the generally applicable options
cfg = ft_checkconfig(cfg, 'required', {'nsignal' 'ntrials' 'triallength' 'fsample' 'method'});
cfg = ft_checkconfig(cfg, 'rename',   {'blc', 'demean'});

% method specific defaults
switch cfg.method
  case {'ar'}
    cfg.absnoise = ft_getopt(cfg, 'absnoise', zeros(cfg.nsignal,1));
    cfg          = ft_checkconfig(cfg, 'required', {'params' 'noisecov'});
  case {'linear_mix'}
    cfg.bpfilter = ft_getopt(cfg, 'bpfilter', 'yes');
    cfg.bpfreq   = ft_getopt(cfg, 'bpfreq',   [15 25]);
    cfg.demean   = ft_getopt(cfg, 'demean',   'yes');
    cfg.absnoise = ft_getopt(cfg, 'absnoise', 1);
    cfg          = ft_checkconfig(cfg, 'required', {'mix' 'delay'});
  case {'mvnrnd'}
    cfg.bpfilter = ft_getopt(cfg, 'bpfilter', 'yes');
    cfg.bpfreq   = ft_getopt(cfg, 'bpfreq',   [15 25]);
    cfg.demean   = ft_getopt(cfg, 'demean',   'yes');
    cfg.absnoise = ft_getopt(cfg, 'absnoise', 1);
    cfg          = ft_checkconfig(cfg, 'required', {'covmat' 'delay'});
  case {'ar_reverse'}
    % reverse engineered high order ar-model
    cfg = ft_checkconfig(cfg, 'required', {'coupling' 'delay' 'ampl' 'bpfreq'});
  otherwise
end

trial = cell(1, cfg.ntrials);
time  = cell(1, cfg.ntrials);
nsmp  = round(cfg.triallength*cfg.fsample);
tim   = (0:nsmp-1)./cfg.fsample;

% create the labels
label = cell(cfg.nsignal,1);
for k = 1:cfg.nsignal
  label{k,1} = ['signal',num2str(k, '%03d')];
end

switch cfg.method
  case {'ar'}

    nlag    = size(cfg.params,3);
    nsignal = cfg.nsignal;
    params  = zeros(nlag*nsignal, nsignal);
    for k = 1:nlag
      %params(((k-1)*nsignal+1):k*nsignal,:) = cfg.params(:,:,k);
      params(((k-1)*nsignal+1):k*nsignal,:) = cfg.params(:,:,k)';
      % Use the transposition to make the implementation consistent with what
      % comes out of ft_mvaranalysis. The transposition is introduced on May
      % 13, 2011. This swaps the directional influence for existing scripts.
    end
    for k = 1:cfg.ntrials
      tmp   = zeros(nsignal, nsmp+ceil(nlag*1.05));
      noise  = mvnrnd(zeros(nsignal,1), cfg.noisecov, ceil(nsmp+nlag*1.05))';
      state0 = zeros(nsignal*nlag, 1);
      for m = 1:nlag
        indx = ((m-1)*nsignal+1):m*nsignal;
        state0(indx) = params(indx,:)'*noise(:,m);
      end
      tmp(:,1:nlag) = flip(reshape(state0, [nsignal nlag]),2);

      for m = (nlag+1):(nsmp+ceil(nlag*1.05))
        state0    = reshape(flip(tmp(:,(m-nlag):(m-1)),2), [nlag*nsignal 1]);
        tmp(:, m) = params'*state0 + noise(:,m);
      end

      trial{k} = tmp(:,(ceil(nlag*1.05)+1):end);
      if any(cfg.absnoise>0)
        trial{k} = trial{k} + diag(cfg.absnoise)*randn(size(trial{k}));
      end
      time{k}  = tim;
    end

    % create the output data
    simulated         = [];
    simulated.trial   = trial;
    simulated.time    = time;
    simulated.fsample = cfg.fsample;
    simulated.label   = label;

  case {'linear_mix'}

    fltpad = 50; %hard coded to avoid filtering artifacts
    delay  = cfg.delay;
    delay  = delay - min(delay(:)); %make explicitly >= 0
    maxdelay = max(delay(:));

    if iscell(cfg.mix)
      %each trial has different mix
      mix = cfg.mix;
    else
      %make cell-array out of mix
      tmpmix = cfg.mix;
      mix    = cell(1,cfg.ntrials);
      for tr = 1:cfg.ntrials
        mix{1,tr} = tmpmix;
      end
    end

    nmixsignal = size(mix{1}, 2); %number of "mixing signals"
    nsignal    = size(mix{1}, 1);

    if numel(size(mix{1}))==2
      %mix is static, no function of time
      for tr = 1:cfg.ntrials
        mix{tr} = mix{tr}(:,:,ones(1,nsmp+maxdelay));
      end
    elseif numel(size(mix{1}))==3 && size(mix{1},3)==nsmp
      %mix changes with time
      for tr = 1:cfg.ntrials
        mix{tr} = cat(3,mix{tr},mix{tr}(:,:,nsmp*ones(1,maxdelay)));
      end
      %FIXME think about this
      %due to the delay the mix cannot be defined instantaneously with respect to all signals
    end

    for tr = 1:cfg.ntrials
      mixsignal = randn(nmixsignal,  nsmp + 2*fltpad + maxdelay);
      mixsignal = preproc(mixsignal, label, offset2time(-fltpad, cfg.fsample, size(mixsignal,2)), cfg, fltpad, fltpad);
      tmp       = zeros(cfg.nsignal, nsmp);
      for i=1:cfg.nsignal
        for j=1:nmixsignal
          begsmp   = 1    + delay(i,j);
          endsmp   = nsmp + delay(i,j);
          tmpmix   = reshape(mix{tr}(i,j,:),[1 nsmp+maxdelay]) .* mixsignal(j,:);
          tmp(i,:) = tmp(i,:) + tmpmix(begsmp:endsmp);
        end
      end
      trial{tr} = tmp;

      % add some noise
      trial{tr} = ft_preproc_baselinecorrect(trial{tr} + cfg.absnoise*randn(size(trial{tr})));

      % define time axis for this trial
      time{tr}  = tim;
    end

  case {'mvnrnd'}
    fltpad = 100; %hard coded

    shift = max(cfg.delay(:,1)) - cfg.delay(:,1);
    for k = 1:cfg.ntrials
      % create the multivariate time series plus some padding
      tmp = mvnrnd(zeros(1,cfg.nsignal), cfg.covmat, nsmp+2*fltpad+max(shift))';

      % add the delays
      newtmp = zeros(cfg.nsignal, nsmp+2*fltpad);
      for kk = 1:cfg.nsignal
        begsmp =      + shift(kk) + 1;
        endsmp = nsmp + 2*fltpad + shift(kk);
        newtmp(kk,:) = ft_preproc_baselinecorrect(tmp(kk,begsmp:endsmp));
      end

      % apply preproc
      newtmp = preproc(newtmp, label, offset2time(-fltpad, cfg.fsample, size(newtmp,2)), cfg, fltpad, fltpad);

      trial{k} = newtmp;

      % add some noise
      trial{k} = ft_preproc_baselinecorrect(trial{k} + cfg.absnoise*randn(size(trial{k})));

      % define time axis for this trial
      time{k}  = tim;
    end

    % create the output data
    simulated         = [];
    simulated.trial   = trial;
    simulated.time    = time;
    simulated.fsample = cfg.fsample;
    simulated.label   = label;

  case 'ar_reverse'
    % generate a spectral transfer matrix, and a cross-spectral matrix
    % according to the specifications

    % predefine some variables
    fstep = 1/5;
    fs    = cfg.fsample;
    Nyq   = fs./2;
    foi   = (0:fstep:Nyq);
    omega = foi./fs;
    n     = numel(foi);

    % local renaming
    nsignal = cfg.nsignal;
    fband   = cfg.bpfreq;
    coupling = cfg.coupling;
    ampl     = cfg.ampl;
    delay    = cfg.delay;

    % create a 1/f spectrum
    slope    = 0.5;
    oneoverf = sqrt(max(omega(2)./10,omega).^-slope); % takes sqrt for amplitude
    oneoverf = oneoverf./oneoverf(1);
    %oneoverf(1) = 0;
    %z = firws_filter(5.*fs, fs, Nyq./1.01);
    %z = z(1:numel(foi)); %.*exp(-1i.*pi.*foi.*rand(1)./100);
    %oneoverf = z.*oneoverf;

    % convert into indices
    findx = fband;
    for k = 1:numel(fband)
      if isfinite(fband(k))
        findx(k) = nearest(foi, fband(k));
      end
    end

    % allocate some memory
    mask = false(nsignal, nsignal, n);
    krn = zeros(size(mask));
    phi = zeros(size(krn));
    dat = zeros(size(krn));
    coupling_ampl = zeros(size(krn));

    for k = 1:nsignal
      for m = 1:nsignal
        if all(isfinite(squeeze(findx(k,m,:))))
          mask(k,m,findx(k,m,1):findx(k,m,2)) = true;
        end
        krn(k,m,mask(k,m,:))  = hanning(sum(mask(k,m,:)))';

        phi(k,m,:) = 2.*pi.*delay(k,m).*foi;
        %phi(k,m,:) = phi(k,m,:).*mask(k,m,:);
        %phi(k,m,mask(k,m,:)) = phi(k,m,mask(k,m,:))-mean(phi(k,m,mask(k,m,:)));
        if all(isfinite(squeeze(findx(k,m,:))))
          phi(k,m,1:findx(k,m,1)) = phi(k,m,findx(k,m,1));
          phi(k,m,findx(k,m,2):end) = phi(k,m,findx(k,m,2));
          phi(k,m,:) = phi(k,m,:)-mean(phi(k,m,:));
        end

        coupling_ampl(k,m,:) = coupling(k,m).*krn(k,m,:);
      end
    end

    % this matrix contains the intrinsic amplitude spectra on the diagonal
    for k = 1:nsignal
      if all(isfinite(squeeze(fband(k,k,:))))
        z = firws_filter((1/fstep).*fs, fs, [fband(k,k,1) fband(k,k,2)]);
        z = z(1:numel(foi)); %.*exp(-1i.*pi.*foi.*rand(1)./100);
        z = z.*ampl(k,k);

        plateau = nearest(foi,fband(k,k,1)):nearest(foi,fband(k,k,2));
        oneoverf(plateau) = mean(abs(oneoverf(plateau)));
        dat(k,k,:) = -(abs(oneoverf)+abs(z)).*exp(1i.*(angle(z)+angle(oneoverf)));
      else
        dat(k,k,:) = oneoverf;
      end
    end

    % now we can create a spectral transfer matrix
    tf = zeros(nsignal,nsignal,n)+1i.*zeros(nsignal,nsignal,n);
    for k = 1:nsignal
      for m = 1:nsignal
        if k~=m && all(isfinite(squeeze(fband(k,m,:))))
          z = firws_filter((1/fstep).*fs, fs, [fband(k,m,1) fband(k,m,2)]);
          z = z(1:numel(foi));
          tf(m,k,:) = coupling(k,m).*exp(-1i.*phi(k,m,:)).*shiftdim(z,-1); % deliberate index swap!

        elseif k==m
          tf(k,m,:) = dat(k,m,:);
        end
      end
    end

    % create the cross spectral matrix
    c = zeros(size(tf));
    for k = 1:n
      c(:,:,k) = tf(:,:,k)*tf(:,:,k)'; % assume noise to be I, i.e. the tf to swallow the amplitudes
    end

    % scale the Nyquist and DC bins
    c(:,:,1)   = real(c(:,:,1)./2);
    c(:,:,end) = real(c(:,:,end)./2);

    % create a freq-structure
    freq           = [];
    freq.crsspctrm = c;
    freq.label     = label;
    freq.freq      = foi;
    freq.dimord    = 'chan_chan_freq';

    % estimate the transfer-matrix non-parametrically
    tmpcfg        = [];
    tmpcfg.method = 'transfer';
    tmpcfg.granger.stabilityfix = true;
    t             = ft_connectivityanalysis(tmpcfg, freq);

    % estimate the ar-model coefficients
     a = transfer2coeffs(t.transfer,t.freq);

    % recursively call this function to generate the data, this is
    % somewhate tricky with respect to keeping the provenance info. Here,
    % it is solved by removing from the cfg the original user-specified
    % fields
    cfgorig      = cfg;
    cfg          = removefields(cfgorig, {'coupling' 'ampl' 'delay' 'bpfreq'});
    cfg.method   = 'ar';
    cfg.params   = a;
    cfg.noisecov = diag(diag(t.noisecov.*cfg.fsample./2));
    simulated    = ft_connectivitysimulation(cfg);
    cfg.previous = keepfields(cfgorig, {'coupling' 'ampl' 'delay' 'bpfreq'});

  otherwise
    ft_error('unknown method');
end

% do the general cleanup and bookkeeping at the end of the function
ft_postamble debug
ft_postamble trackconfig
ft_postamble randomseed
ft_postamble provenance
ft_postamble history simulated
ft_postamble savevar simulated

%%%%%%
% helper function
function A = transfer2coeffs(H, freq, labelcmb, maxlag)

% TRANSFER2COEFFS converts a spectral transfer matrix into the time domain
% equivalent multivariate autoregressive coefficients up to a specified
% lag, starting from lag 1.

if nargin<3
  labelcmb = [];
end
if nargin<4
  maxlag = [];
end

% do a check on the input data
siz = size(H);
if numel(siz)==3 && siz(1)==siz(2)
  % assume chan_chan_freq
  isfull = true;
elseif numel(siz)==2
  % assume chancmb_freq
  isfull = false;
  %assert(~isempty(labelcmb), 'input data appears to be chancmb_freq, but labelcmb is missing');
else
  ft_error('dimensionality of input data is not supported');
end

dfreq = round(diff(freq)*1e5)./1e5; % allow for some numeric issues
if ~all(dfreq==dfreq(1))
  ft_error('the frequency axis is not evenly spaced');
end

if freq(1)~=0
  ft_warning('when converting the transfer function to coefficients, the frequency axis should ideally start at 0, zero padding the spectral density');
  dfreq = mean(dfreq);
  npad  = freq(1)./dfreq;

  % update the freq axis and keep track of the frequency bins that are
  % expected in the output
  selfreq  = (1:numel(freq)) + npad;
  freq     = [(0:(npad-1))./dfreq freq];
  if isfull
    H = cat(3, zeros(siz(1),siz(2),npad), H);
  else
    H = cat(2, zeros(siz(1),npad), H);
  end
else
  selfreq  = 1:numel(freq);
end

% ensure H to be double precision
H = double(H);

% deal with the two different types of input
if isfull
  % check whether the last frequency bin is strictly real-valued.
  % if that's the case, then it is assumed to be the Nyquist frequency
  % and the two-sided spectral density will have an even number of
  % frequency bins. if not, in order to preserve hermitian symmetry,
  % the number of frequency bins needs to be odd.
  Hend = H(:,:,end);
  N    = numel(freq);
  m    = size(H,1);
  if all(imag(Hend(:))<abs(trace(Hend)./size(Hend,1)*1e-9))
    N2 = 2*(N-1);
  else
    N2 = 2*(N-1)+1;
  end

  % preallocate memory for efficiency
  Harr   = zeros(m,m,N2) + 1i.*zeros(m,m,N2);

  % the input cross-spectral density is assumed to be weighted with a
  % factor of 2 in all non-DC and Nyquist bins, therefore weight the
  % DC-bin with a factor of sqrt(2) to get a correct two-sided representation
  Harr(:,:,1) = H(:,:,1).*2;
  for k = 2:N
    Harr(:,:,       k) = H(:,:,k);
    Harr(:,:,(N2+2)-k) = conj(H(:,:,k));
  end

  % the input cross-spectral density is assumed to be weighted with a
  % factor of 2 in all non-DC and Nyquist bins, therefore weight the
  % Nyquist bin with a factor of sqrt(2) to get a correct two-sided representation
  if mod(size(Harr,3),2)==0
    Harr(:,:,N) = Harr(:,:,N).*sqrt(2);
  end

  % invert the transfer matrix to get the fourier representation of the
  % coefficients, and add an identity matrix
  I = eye(siz(1));
  for k = 1:size(Harr,3)
    Harr(:,:,k) = I-inv(Harr(:,:,k));
  end

  % take the inverse fft to get the coefficients
  A = ifft(reshape(permute(Harr, [3 1 2]), N2, []), 'symmetric');
  A = A(2:end,:);
  A = ipermute(reshape(A, [N2-1 siz(1) siz(1)]), [3 1 2]);

  if ~isempty(maxlag)
    A = A(:,:,1:maxlag);
  end
else
  % check whether the last frequency bin is strictly real-valued.
  % if that's the case, then it is assumed to be the Nyquist frequency
  % and the two-sided spectral density will have an even number of
  % frequency bins. if not, in order to preserve hermitian symmetry,
  % the number of frequency bins needs to be odd.
  Hend = H(:,end);
  N    = numel(freq);
  m    = size(H,1);
  if all(imag(Hend(:))<max(abs(Hend))*1e-9)
    % the above heuristic may be a bit silly, FIXME
    N2 = 2*(N-1);
  else
    N2 = 2*(N-1)+1;
  end

  % preallocate memory for efficiency
  Harr   = zeros(m,N2) + 1i.*zeros(m,N2);

  % the input cross-spectral density is assumed to be weighted with a
  % factor of 2 in all non-DC and Nyquist bins, therefore weight the
  % DC-bin with a factor of sqrt(2) to get a correct two-sided representation
  Harr(:,1) = H(:,1).*sqrt(2);
  for k = 2:N
    Harr(:,       k) = H(:,k);
    Harr(:,(N2+2)-k) = conj(H(:,k));
  end

  % the input cross-spectral density is assumed to be weighted with a
  % factor of 2 in all non-DC and Nyquist bins, therefore weight the
  % Nyquist bin with a factor of sqrt(2) to get a correct two-sided representation
  if mod(size(Harr,3),2)==0
    Harr(:,N) = Harr(:,N).*sqrt(2);
  end

  % invert the transfer matrix to get the fourier representation of the
  % coefficients, and add an identity matrix
  %
  % this assumes Harr to be in the rows quadruplets of pairwise
  % decompositions, i.e. reshapable, without checking the labelcmb
  ncmb = size(Harr,1)./4;
  I = eye(2);
  for k = 1:N2
    Htmp = reshape(Harr(:,k), [2 2 ncmb]);
    Htmp = repmat(I, [1 1 ncmb]) - inv2x2(Htmp);
    Harr(:,k) = Htmp(:);
  end

  % take the inverse fft to get the coefficients
  A = ifft(permute(Harr, [2 1]), 'symmetric');
  A = A(2:end,:);
  A = ipermute(A, [2 1]);

  if ~isempty(maxlag)
    A = A(:,1:maxlag);
  end

end

function z = firws_filter(N, Fs, Fbp)

switch numel(Fbp)
  case 1
    [dum, B] = ft_preproc_lowpassfilter(randn(1,N), Fs, Fbp, [], 'firws', 'onepass-minphase');
    z  = fft(B, N);

  case 2
    [dum, B] = ft_preproc_bandpassfilter(randn(1,N), Fs, Fbp, [], 'firws', 'onepass-minphase');
    z  = fft(B, N);

end


%RECOVERPOSE  Recover relative camera rotation and translation from an estimated essential matrix and the corresponding points in two images, using cheirality check
%
%     [R, t, good] = cv.recoverPose(E, points1, points2)
%     [R, t, good, mask, triangulatedPoints] = cv.recoverPose(...)
%     [...] = cv.recoverPose(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __E__ The input essential matrix, 3x3.
% * __points1__ Cell array of N 2D points from the first image, or numeric
%   array Nx2/Nx1x2/1xNx2. The point coordinates should be floating-point
%   (single or double precision).
% * __points2__ Cell array or numeric array of the second image points of the
%   same size and format as `points1`.
%
% ## Output
% * __R__ Recovered relative rotation, 3x3 matrix.
% * __t__ Recovered relative translation, 3x1 vector.
% * __good__ the number of inliers which pass the cheirality check.
% * __mask__ Output mask for inliers in `points1` and `points2`. In the output
%   mask only inliers which pass the cheirality check. Vector of length N, see
%   the `Mask` input option.
% * __triangulatedPoints__ 3D points which were reconstructed by triangulation,
%   see cv.triangulatePoints
%
% ## Options
% * __CameraMatrix__ Camera matrix `K = [fx 0 cx; 0 fy cy; 0 0 1]`. Note that
%   this function assumes that `points1` and `points2` are feature points from
%   cameras with the same camera matrix. default `eye(3)`.
% * __DistanceThreshold__ threshold distance which is used to filter out far
%   away points (i.e. infinite points). default 50.0
% * __Mask__ Input mask of length N for inliers in `points1` and `points2`
%   (0 for outliers and to 1 for the other points (inliers). If it is not
%   empty, then it marks inliers in `points1` and `points2` for then given
%   essential matrix `E`. Only these inliers will be used to recover pose.
%   Not set by default.
%
% This function decomposes an essential matrix using cv.decomposeEssentialMat
% and then verifies possible pose hypotheses by doing cheirality check. The
% cheirality check basically means that the triangulated 3D points should have
% positive depth. Some details can be found in [Nister03].
%
% This function can be used to process output `E` and `mask` from
% cv.findEssentialMat. In this scenario, `points1` and `points2` are the same
% input for cv.findEssentialMat.
%
% ## Example
%
%     % Estimation of fundamental matrix using the RANSAC algorithm
%     point_count = 100;
%     points1 = cell(1, point_count);
%     points2 = cell(1, point_count);
%     % initialize the points here ...
%     for i=1:point_count
%         points1{i} = ...;  % [x,y]
%         points2{i} = ...;  % [x,y]
%     end
%
%     % cametra matrix with both focal lengths = 1, and principal point = [0 0]
%     cameraMatrix = eye(3,3);
%
%     [E, mask] = cv.findEssentialMat(points1, points2, ...
%         'CameraMatrix',cameraMatrix, 'Method','Ransac');
%     [R, t, ~, mask] = cv.recoverPose(E, points1, points2, ...
%         'CameraMatrix',cameraMatrix, 'Mask',mask);
%
% ## References
% [Nister03]:
% > David Nister. "An efficient solution to the five-point relative pose
% > problem". Pattern Analysis and Machine Intelligence, IEEE Transactions on,
% > 26(6):756-770, 2004.
%
% See also: cv.findEssentialMat, cv.decomposeEssentialMat,
%  cv.triangulatePoints, relativeCameraPose
%


function plotColors(img)
%
%
%        plotColors(img)
%
%        This function visualizes colors of the image in its 3D color
%        space.
%
%        Input:
%           -img: an image
%
%     Copyright (C) 2015  Francesco Banterle
%
%     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/>.
%

check3Color(img);

[r, c, col] = size(img);

x = img(:,:,1);
y = img(:,:,2);
z = img(:,:,3);

c = reshape(img, r * c, col);

scatter3(x(:), y(:), z(:), [], c, 'filled');

end


function laguerre_test_int_test05 ( )

%*****************************************************************************80
%
%% LAGUERRE_TEST_INT_TEST05 tests P00_RAT_TRANSFORM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 December 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LAGUERRE_TEST_INT_TEST05\n' );
  fprintf ( 1, '  P00_RAT_TRANSFORM. applies a rational tranform\n' );
  fprintf ( 1, '  to estimate an integral on [ALPHA,+oo)\n' );
  fprintf ( 1, '  as a transformed integral on (0,1/(1+ALPHA)]\n' );
  fprintf ( 1, '  and applying a Gauss-Legendre rule.\n' );

  problem_num = p00_problem_num ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '                           Exact\n' );
  fprintf ( 1, '   Problem     Order       Estimate    Error\n' );

  for problem = 1 : problem_num

    exact = p00_exact ( problem );

    order = 1;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %8d            %14.6f\n', problem, exact );

    for order_log = 0 : 6

      estimate = p00_rat_transform ( problem, order );

      err = abs ( exact - estimate );

      fprintf ( 1, '            %8d  %14.6f  %14.6e\n', order, estimate, err );

      order = order * 2;

    end

  end

  return
end


% Add multiple edges in an edge list
%
% INPUTS: original (non-compact) edge list
% OUTPUTS: final compact edge list (no row repetitions)
%
% Example: [1 2 2; 2 2 1; 4 5 1] -> [1 2 3; 4 5 1]
% GB: last updated, Sep 25 2012

function elc=addEdgeWeights(el)

el2=[el(:,1), el(:,2)]; % make the edge list searchable w/o the weights
visited=[];             % mark visited edges

elc=[];
for e=1:size(el,1)
    if sum(ismember(visited,el2(e,:),'rows'))==0  % if not visited yet
        ind=ismember(el2,el2(e,:),'rows');
        ind=find(ind==1);     % these are all the ocurrences of el(e,:)
        elc=[elc; el(e,1), el(e,2), sum(el(ind,3))];
        visited=[visited; el2(e,:)];
    end
end


function qr=v_rotmr2qr(mr)
%V_ROTMR2QR converts a matrix of real quaternion matrices to quaternion vectors
% Inputs:
%
%     MR(4m,4n,...)   mxn matrix of real quaternion matrices (each 4x4)
%
% Outputs:
%
%     QR(4m,n,...)   mxn matrix of real quaternion vectors (each 4x1)
%
% In matrix form, quaternions can be multiplied and added using normal matrix
% arithmetic. Each element of an mxn matrix of quaternions is itself a 4x4 block
% so the total dimension of MR is 4m x 4n.

%
%      Copyright (C) Mike Brookes 2000-2018
%      Version: $Id: v_rotmr2qr.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
s=size(mr);
s(2)=s(2)/4;
mr=reshape(mr,s(1),[]);
qr=reshape(mr(:,1:4:end),s);
if ~nargout
    qr=qr(1:4); % select the first element
    v_rotqr2ro(qr(:)); % plot a rotated cube
end


function [err,time,solver,eqn,node,elem] = afemPoisson(node,elem,pde,bdFlag,option,varargin)

%% Check input arguments
if nargin >=1 && ischar(node)
    option.elemType = node;
    clear node
end
if ~exist('node','var') || ~exist('elem','var')
    % default mesh: Lshape
    [node,elem] = squaremesh([-1,1,-1,1],1);
    [node,elem] = delmesh(node,elem,'x>0 & y<0');
end
if ~exist('option','var'), option = []; end
if ~exist('pde','var')
    pde = Lshapedata;                          % default data
end
if ~exist('bdFlag','var')
    bdFlag = setboundary(node,elem,'Dirichlet');
end

%% Parameters
option = afemoption(option,2);
maxIt = option.maxIt;
refType = option.refType;
elemType = option.elemType;
theta = option.theta;

%% Initialize err
errL2 = zeros(maxIt,1);   errH1 = zeros(maxIt,1); erreta = zeros(maxIt,1);
erruIuh = zeros(maxIt,1); errMax = zeros(maxIt,1);
errTime = zeros(maxIt,1); solverTime = zeros(maxIt,1);
assembleTime = zeros(maxIt,1); meshTime = zeros(maxIt,1);
itStep = zeros(maxIt,1);  stopErr = zeros(maxIt,1); flag = zeros(maxIt,1);
N = zeros(maxIt,1);

%% Generate an initial mesh
for k = 1:option.L0
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect(node,elem,bdFlag);
    end
end

%%  Adaptive Finite Element Method
% *SOLVE* -> *ESTIMATE* -> *MARK* -> *REFINE*
for k = 1:maxIt
    % Step 1: SOLVE
    switch elemType
        case 'P1'     % piecewise linear function P1 element
            [u,Du,eqn,info] = Poisson(node,elem,pde,bdFlag,option);
        case 'CR'     % piecewise linear function CR element
            [u,Du,eqn,info] = PoissonCR(node,elem,pde,bdFlag,option);
        case 'P2'     % piecewise quadratic function
            [u,Du,eqn,info] = PoissonP2(node,elem,pde,bdFlag,option);
        case 'WG'     % weak Galerkin element
            [u,Du,eqn,info] = PoissonWG(node,elem,pde,bdFlag,option);
    end
    % compute error
    tic;
    if isfield(pde,'Du')
        if ~isfield(pde,'d')
            pde.d = [];
        end
        if ~isempty(Du)
            errH1(k) = getH1error(node,elem,pde.Du,Du,pde.d);
        else
            errH1(k) = getH1error(node,elem,pde.Du,u,pde.d);
        end
    end
    if isfield(pde,'exactu')
        errL2(k) = getL2error(node,elem,pde.exactu,u);
        % interpolation
        if strcmp(elemType,'P1')
            uI = Lagrangeinterpolate(pde.exactu,node,elem);
        else
            uI = Lagrangeinterpolate(pde.exactu,node,elem,elemType,eqn.edge);
        end
        erruIuh(k) = sqrt((u-uI)'*eqn.A*(u-uI));
        errMax(k) = max(abs(u-uI));
    end
    errTime(k) = toc;
    % record time
    solverTime(k) = info.solverTime;
    assembleTime(k) = info.assembleTime;
    if option.printlevel>1
        fprintf('Time to compute the error %4.2g s \n H1 err %4.2g    L2err %4.2g \n',...
            errTime(k),errH1(k), errL2(k));
    end
    % record solver information
    itStep(k) = info.itStep;
    stopErr(k) = info.stopErr;
    flag(k) = info.flag;
    % plot
    N(k) = size(node,1);
    if option.plotflag && N(k) < 2e3 % show mesh and solution for small size
       figure(1);  showresult(node,elem,u);
    end
    % Step 2: ESTIMATE
    switch option.estType
        case 'recovery' % recovery type
            eta = estimaterecovery(node,elem,u);
        case 'residual' % residual type
            switch elemType
                case 'P1'
                    eta = estimateresidual(node,elem,u,pde,bdFlag);
                case 'WG'
                    eta = estimateresidualWG(node,elem,u,Du,pde);
            end
    end
    erreta(k) = sqrt(sum(eta.^2));
    % Step 3: MARK
    switch option.markType
        case 'L2'
            markedElem = mark(elem,eta,theta);
        case 'MAX'
            markedElem = mark(elem,eta,theta,'MAX');
    end
    % Step 4: REFINE
    if N(k) > option.maxN
        break;
    end
    [node,elem,bdFlag,HB,tree] = bisect(node,elem,markedElem,bdFlag); %#ok<ASGLU>
    % Step 4.2: COARSEN
    if option.coarsenflag
        eta = eleminterpolate(eta,tree);
        markedElem = mark(elem,eta,0.25*theta,'COARSEN');
        [node,elem,bdFlag] = coarsen(node,elem,markedElem,bdFlag);
    end
end

%% Plot convergence rates
if option.rateflag
    figure;
    set(gcf,'Units','normal');
    set(gcf,'Position',[0.25,0.25,0.55,0.4]);
    subplot(1,2,1)
    showrate2(N(1:k),errH1(1:k),10,'-*','||Du-Du_h||',...
              N(1:k),errL2(1:k),10,'k-+','||u-u_h||');
    subplot(1,2,2)
    showrate2(N(1:k),erruIuh(1:k),10,'m-+','||Du_I-Du_h||',...
              N(1:k),errMax(1:k),10,'r-*','||u_I-u_h||_{\infty}');
end

%% Output
err = struct('N',N(1:k),'H1',errH1(1:k),'L2',errL2(1:k),...
             'uIuhH1',erruIuh(1:k),'uIuhMax',errMax(1:k),'eta',erreta(1:k));
time = struct('N',N(1:k),'err',errTime(1:k),'solver',solverTime(1:k), ...
              'assmble',assembleTime(1:k),'mesh',meshTime(1:k));
solver = struct('N',N(1:k),'itStep',itStep(1:k),'time',solverTime(1:k),...
                'stopErr',stopErr(1:k),'flag',flag(1:k));

%% Display error
ts = zeros(k,3); ts = char(ts);
display('#Dof   ||u-u_h||     ||Du-Du_h||   ||DuI-Du_h||  ||uI-u_h||_{max}    eta');
display([num2str(err.N) ts num2str(err.L2,'%0.5e') ts num2str(err.H1,'%0.5e')...
         ts num2str(err.uIuhH1,'%0.5e') ts num2str(err.uIuhMax,'%0.5e') ...
         ts num2str(err.eta,'%0.5e')]);


function [Ymat Xmat N n m p T k q h]=panel6prelim(data_endo,data_exo,const,lags)

% first compute N, the number of units, as the dimension of the data_endo matrix
N=size(data_endo,3);

% compute p, the number of lags in the model
p=lags;

% then compute n, the number of endogenous variables in the model; it is simply the number of columns in the matrix 'data_endo'
n=size(data_endo,2);

% if the constant has been selected, augment the matrix of exogenous with a column of ones (number of rows equal to number of rows in data_endo)
if const==1
data_exo=[ones(size(data_endo,1),1) data_exo];
% if no constant was included, do nothing
else
end

% compute m, the number of exogenous variables in the model
% if data_exo is empty, set m=0
if isempty(data_exo)==1
m=0;
% if data_exo is not empty, count the number of exogenous variables that will be included in the model
else
m=size(data_exo,2);
% Also, trim a number initial rows equal to the number of lags, as they will be suppressed from the endogenous as well to create initial conditions
data_exo=data_exo(p+1:end,:);
end

% determine k, the number of parameters to estimate in each equation; it is equal to np+m
k=N*n*p+m;

% determine q, the total number of VAR parameters for each unit
q=n*k;

% determine h, the total number of VAR parameters for the whole model
h=N*q;

% obtain Ymat and Xmat
temp=[];
% stack the matrices of endogenous variables to obtain a temporary matrix
for ii=1:N
temp=[temp data_endo(:,:,ii)];
end
% use the lagx function on this matrix
temp=bear.lagx(temp,lags);

% set Ymat as the first Nn columns of the result
Ymat=temp(:,1:N*n);

% to build Xmat, take off the Nn initial columns of temp, and concatenate the exogenous
Xmat=[temp(:,N*n+1:end) data_exo];

% Define T, the number of periods of the model, as the number of rows of X
T=size(Xmat,1);


%%  mnist data
clear all; close all; clc;
load mnist_uint8;
x = cell(100, 1);
N = 600;
for i = 1 : 100
    x{i}{1} = reshape(train_x(((i - 1) * N + 1) : (i) * N, :), N, 28, 28) * 255;
end
%% ex 1
scae = {
    struct('outputmaps', 10, 'inputkernel', [1 5 5], 'outputkernel', [1 5 5], 'scale', [1 2 2], 'sigma', 0.1, 'momentum', 0.9, 'noise', 0)
};

opts.rounds     = 1000;
opts.batchsize  =    1;
opts.alpha      = 0.01;
opts.ddinterval =   10;
opts.ddhist     =  0.5;
scae = scaesetup(scae, x, opts);
scae = scaetrain(scae, x, opts);
cae = scae{1};

%Visualize the average reconstruction error
plot(cae.rL);

%Visualize the output kernels
ff=[];
for i=1:numel(cae.ok{1});
    mm = cae.ok{1}{i}(1,:,:);
    ff(i,:) = mm(:);
end;
figure;visualize(ff')


%% parameters for simulation
gam = [1.58, -0.6];
noise = .5;
T = 455;
framerate = 30;
firerate = 2;
seed = 3;

%% simulate calcium fluorescience
[Y, trueC, trueSpikes] = gen_data(gam, noise, T, framerate, ...
    firerate, [], [], seed);

%% plot results
figure('papersize', [15, 2.5]);
init_fig;
hold on;
col = {[0, 114, 176]; ...
    [0, 158, 115]; ...
    [213, 94, 0]};

% fluorescence trace
plot(1:T, Y(1,:)/3, 'o', 'color',  uint8(col{2}));

% calcium trace
plot(1:T, trueC(1,:)/3, 'color', 'k'); % uint8(col{1}));
% spike train
tsp = find(trueSpikes(1, :));
for m=1:length(tsp)
    plot([1,1]*tsp(m), [0, 1], 'color', uint8(col{3}));
end
axis tight;
xlabel('Time');
ylabel('Fluorescence');
legend('y', 'c', 's');
set(gco, 'fontweigth', 'bold');
saveas(gcf, 'fig/model.pdf');


% Create an occupancy grid map from points
function gridmap = OccuGrid(pts, pixelSize)

% Grid size
minXY = min(pts) - 3 * pixelSize;
maxXY = max(pts) + 3 * pixelSize;
Sgrid = round((maxXY - minXY) / pixelSize) + 1;

%
N = size(pts, 1);
hits = round( (pts-repmat(minXY, N, 1)) / pixelSize ) + 1;
idx = (hits(:,1)-1)*Sgrid(2) + hits(:,2);

grid  = false(Sgrid(2), Sgrid(1));
grid(idx) = true;

gridmap.occGrid = grid;
gridmap.metricMap = min(bwdist(grid),10);
gridmap.pixelSize = pixelSize;
gridmap.topLeftCorner = minXY;


% The function gamer creates the objective function and the constraints for
% the optimization problem to supply it to fmincon.

function [x,fval,exitflag,output] = gamer(n,Us,p,I,s,ub,lb,x0,Aeq,beq,pay,U)

    function F = myfun(x)
        Funct = 0;
        prod = 1;
        for i = 1 : n
            Funct = Funct + x(s+i);
        end
        for i = 1 : p
            for j = 1 : n
                prod = prod * x(I(i,j));
            end
            Funct = Funct - Us(i) * prod;
            prod = 1;
        end
        F = Funct;
    end

    function [c ceq] = confun(x)
        C = zeros(s,1);
        for i = 1 : s
            C(i) = -x(pay(i));
            for t = 1 : n
                add = 0;
                for j = 1 : p
                    prd = 1;
                    for k = 1 : n
                        if i == I(j,k)
                            prd = prd * U(j,k);
                        else
                            prd = prd * x(I(j,k));
                        end
                    end
                    if I(j,t) ~= i
                        prd = 0;
                    end
                    add = add + prd;
                end
                C(i) = add + C(i);
            end
        end
        c = C;
        ceq = [];
    end

options = optimset('Display','off');
warning 'off' 'all';
[x,fval,exitflag,output] = fmincon(@myfun,x0,[],[],Aeq,beq,lb,ub,@confun,options);

end


function u = acosh(a)
%ACOSH        slope inverse hyperbolic cosine acosh(a)
%

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

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

  u = a;

  u.r = acosh(a.r);
  u.s = slopeconvexconcave('acosh','1./sqrt(sqr(%)-1)',a,0);

  if rndold
    setround(rndold)
  end


function bp=backprojection(c);
% function backp=backprojection(c);
% calculates backprojections from eigenvalues (e) and Group Space
%(c.GroupSpace) which are the outputs of cmdscale.

eigenvals=diag(c.eigenvalues(1:c.ndims),0);
A=c.GroupSpace(:,1:c.ndims)*sqrt(eigenvals);
for n=1:size(c.data,3);
    mdata=c.data(:,:,n);
    backproj(:,:,n)=A'*(mdata*inv(mdata'*mdata))';
end
bp=mean(backproj,3);


  function img = easyhelix(cg, ig, proj, varargin)
%|
%| FBP reconstruction of cone-beam tomography data collected with
%| a circular source trajectory.
%| See feldkamp_example.m for example.
%|
%| in
%|	cg			ct_geom()
%|	ig			image_geom()
%|	proj	[ns nt na]	cone-beam projection views (line integrals)
%|
%|
%| out
%|	img	[nx ny nz]	reconstructed image
%|

% defaults
img = helix_do(proj, cg, ig, cg.na, cg.dt, cg.dsd, cg.dso, cg.orbit, cg.orbit_start);
end % feldkamp()

%
% feldkamp_do()
%
function img = helix_do(proj, cg, ig, na, dt, dsd, dso, orbit, orbit_start)

% step 1: fix z-sampling; for each CB source point, determine multifan
% betas = deg2rad(orbit_start + orbit * [0:na-1] / na); % [na] source angles

% assumes that na is even
num_turns = orbit/360;
phis = mod(deg2rad(orbit_start + orbit * [0:(na/num_turns-1)]/na), 2*pi);
numPhis = size(phis',1);

if ig.dz < .9*(dso/dsd * dt)
    min_spacing = ig.dz;
else
    min_spacing = .9*(dso/dsd * dt);
end

num_zsamp = ig.nz;
% num_zsamp = ceil((ig.z(size(ig.z,1))-ig.z(1))/min_spacing);
% zSamples = ig.z(1) + min_spacing * [0:num_zsamp-1];
zSamples = ig.z;
% -cg.source_zs(1)+cg.source_zs(2)
% myPitch = (-cg.source_zs(1)+cg.source_zs(2))*(na/2-1)
myPitch = cg.pitch * cg.nt * dso / dsd * cg.dt;

delta = asin(cg.rmax/dso);
dist = .5 * myPitch * (pi + 2 * delta)/(2*pi);

%step 2: rebin the cone beam data to fit a fan beam projection
[ns, nt, na] = size(proj);
fanBeam = zeros(ns, numPhis, num_zsamp);
for i=0:na-1
    currentZ = cg.source_zs(i+1);
    upper = currentZ + dist;
    lower = currentZ - dist;

    lcount = 1;

    while lcount < num_zsamp+1 && zSamples(lcount) < lower
        lcount = lcount + 1;
    end

    while lcount < num_zsamp+1 && zSamples(lcount) < upper
        deltaZ = zSamples(lcount)-currentZ;
        shortscan = (pi/2+delta)*(1-deltaZ/dist);

        is = ndgrid(1:cg.ns,1);
        spoints = cg.s;

        % tpoints = ((spoints).^2+dsd*dsd)./ (dso*dsd).* deltaZ;
        tpoints = ones(cg.ns,1).* deltaZ;

        scale = sqrt(spoints.^2+dsd^2)./sqrt(spoints.^2+tpoints.^2+dsd^2);

        t_in = (tpoints./cg.dt + cg.wt);

        x0 = floor(t_in);
        x1 = 1 + x0;

        alpha = t_in - x0;

        for j = 1:size(x0,1)
            weight = myParker(spoints(j),shortscan,delta,dsd);
            fanBeam(is(j), mod(i,numPhis)+1, lcount) = weight*proj(is(j), nt/2, i+1);
        end

        lcount = lcount + 1;
    end

end

% Everything below here goes really fast
disp('we are here')
    orbitStart = zeros(num_zsamp,1);
    % decide on beginning orbit
    orbit = deg2rad(cg.orbit_start);;
    zloc = cg.source_z0;
    change_phis = phis(2)-phis(1);
    for z = 1: num_zsamp
        orbitFound = 0;
        while orbitFound == 0
            if zSamples(z) < zloc + dist
                orbitStart(z) = orbit;
                orbitFound = 1;
            else
                orbit = orbit + change_phis;
                zloc = zloc + (cg.source_zs(2) - cg.source_zs(1));
            end
        end
    end

    newNA = floor(2*dist/myPitch*numPhis);
    newFanBeam = zeros(ns, newNA, num_zsamp);

    for z = 1 : num_zsamp
        angle = round((orbitStart(z)-deg2rad(cg.orbit_start)) / change_phis);
        for o = 0: newNA-1
            nextAngle = angle + o;
            newFanBeam(:, o+1, z) = fanBeam(:, mod(nextAngle, numPhis)+1, z);
        end
    end

    down = cg.down;
    wantedZ=ceil(ig.nz/2);
     im(newFanBeam(:,:,wantedZ), 'test'), cbar

    for z = 1:num_zsamp
        sinost = sino_geom('fan', 'ns', cg.ns*down, 'na', newNA*down, ...
            'ds', cg.ds/down, 'down', down, 'orbit', 'short', ...
            'orbit_start', rad2deg(orbitStart(z)), ...
            'offset_s', cg.offset_s, ...
            'dsd', cg.dsd, 'dod', cg.dod, 'dfs', cg.dfs);
        wt = fbp_fan_short_wt(sinost);

        for i = 1:ns
            for j = 1:newNA
                newFanBeam(i, j, z) = wt(i,j)*newFanBeam(i, j, z);
            end
        end
    end

    im(newFanBeam(:,:,:), 'test'), cbar

   % same image geometry as before, except for the z direction
    ig = image_geom('nx', ig.nx*ig.down, 'ny', ig.ny*ig.down, 'dx', ...
        ig.dx/ig.down, 'down', ig.down);
    mask2 = true([ig.nx ig.ny]);
    mask2(end) = 0; % trick: test it
    ig.mask = repmat(mask2, [1 1]);
    clear mask2

    img = zeros(ig.nx,ig.ny,num_zsamp);

    for i = 1:num_zsamp
        sinost = sino_geom('fan', 'ns', cg.ns*down, 'na', newNA*down, ...
            'ds', cg.ds/(down), 'down', down, 'orbit', 'short', ...
            'orbit_start', rad2deg(orbitStart(i)), ...
            'offset_s', cg.offset_s, ...
            'dsd', cg.dsd, 'dod', cg.dod, 'dfs', cg.dfs);
        geomsino = fbp2(sinost, ig);
        img(:,:,i) = fbp2(newFanBeam(:,:,i), geomsino);
    end

    im(img(:,:,round(num_zsamp/2)+1)), cbar;

end


function x = grid1 ( ndim, nstep, x1, x2 )

%*****************************************************************************80
%
%% GRID1 finds grid points between X1 and X2 in N dimensions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NDIM, the dimension of the points X1 and X2.
%
%    Input, integer NSTEP, the number of points to be generated.
%    NSTEP must be at least 2.
%
%    Input, real X1(NDIM), X2(NDIM), the first and last
%    points, between which the equally spaced points are
%    to be computed.
%
%    Output, real X(NDIM,NSTEP), the set of equally spaced
%    points.  Each column of X represents one point, with X(*,1) = X1
%    and X(*,NSTEP) = X2.
%
  if ( nstep <= 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'GRID1 - Fatal error!\n' );
    fprintf ( 1, '  NSTEP <= 1.\n' );
    fprintf ( 1, '  NSTEP = %d\n', nstep );
    error ( 'GRID1 - Fatal error!' );
  end

  for i = 1 : nstep
    x(1:ndim,i) = ...
      ( ( nstep - i     ) * x1(1:ndim)'   ...
      + (         i - 1 ) * x2(1:ndim)' ) ...
      / ( nstep     - 1 );
  end

  return
end


function [P]=tesSmooth_LAP(TES,V,IND_V,cPar)

%% CONTROL PARAMETERS

if isfield(cPar,'LambdaSmooth')
    LambdaSmooth=cPar.LambdaSmooth;
else
    LambdaSmooth=0.5; %DEFAULT
end

if isfield(cPar,'n')
    nMax=cPar.n;
else
    nMax=1; %DEFAULT
end

if isfield(cPar,'RigidConstraints')
    indRigid=cPar.RigidConstraints;
else
    indRigid=[]; %DEFAULT
end

if isfield(cPar,'Tolerance')
    SSQD_Tol=cPar.Tolerance;
else
    SSQD_Tol=[]; %DEFAULT
end

%%

if ~isempty(SSQD_Tol)
    SSQD_old=[];
    SSQD_ratio=0;
end

if isempty(IND_V)
    [~,IND_V]=patchIND(TES,V,2);
end
logicValid=IND_V>0;
indNoneValid=find(sum(logicValid,2)==0);

%%

nDims=size(V,2); %Number of dimensions

P=V;
PP=V;
Q=V;
for qIter=1:nMax

    %% SIMPLE LAPLACIAN SMOOTHENING

    %Loop for all dimensions
    for qDim=1:1:nDims
        Xp=NaN(size(IND_V,1),size(IND_V,2));
        Xp(logicValid)=P(IND_V(logicValid),qDim);
        Xp=gnanmean(Xp,2);
        Xp(indNoneValid)=V(indNoneValid,qDim);
        PP(:,qDim)=Xp;
    end
    P=P+LambdaSmooth.*(PP-P);

    %%

    %Put back constrained points
    if ~isempty(indRigid)
       P(indRigid,:)=V(indRigid,:);
    end

    if ~isempty(SSQD_Tol)
        %Compute sum of squared differences with respect to previous iteration
        SSQD_new=gnansum((P(:)-Q(:)).^2);
        if ~isempty(SSQD_old)
            SSQD_ratio=SSQD_new./SSQD_old;
        end

        %Store current metrics
        Q=P;
        SSQD_old=SSQD_new;
        if abs(1-SSQD_ratio)<=SSQD_Tol
           break %STOP SMOOTHING LOOP IF TOLERANCE IS REACHED
        end
    end

end

%%
% _*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 Cpos = ShiftBaselinePosSignal(C,dim)
% for 2D arrays only.
% 'dim' (dimension along which the signal is processed) can be 1 or 2

if ~exist('dim','var')
    [s1,s2] = size(C);
    if s1>s2
        dim = 1;
    else
        dim = 2;
    end
end

Cpos = zeros(size(C));
if dim==2
    C = C';
end
for i = 1:size(C,2)
    x = C(:,i);
    shift = prctile(x,5);
    Cpos(:,i) = C(:,i)-shift;
end
if dim==2
    Cpos = Cpos';
end

end


%diag_mat(A,B,2)=[B 0 0;0 A 0;0 0 A];
function [res]=diagmat(A,B,rep)

if (isempty(A))
    res=B;
    return;
end

if (isempty(rep))
    rep=1;
end

nrow=size(A,1);
ncol=size(A,2);

if (isempty(B))
    res=zeros(nrow*rep,ncol*rep);
    for in=1:rep
        res((in-1)*nrow+1:in*nrow,(in-1)*ncol+1:in*ncol)=A;
    end
else
    nrow1=size(B,1);
    ncol1=size(B,2);
    res=zeros(nrow*rep+nrow1,ncol*rep+ncol1);
    res(1:nrow1,1:ncol1)=B;
    for in=1:rep
        res((in-1)*nrow+1+nrow1:in*nrow+nrow1,(in-1)*ncol+1+ncol1:in*ncol+ncol1)=A;
    end
end


function C = cross_product(A,B)

% Calculates the cross product C of the 3-vectors A and B

C = [A(2)*B(3)-A(3)*B(2);  A(3)*B(1)-A(1)*B(3);  A(1)*B(2)-A(2)*B(1)];


function K = gaussian(kern,d1,d2,ind1,ind2,kerParam),

% K = gaussian(d1,d2,ind1,ind2,param), compute the kernel
%     matrix between d1 and d2
% for a gaussian kernel exp(-||x-z||^2/(2*param^2))
%     where x is from d1 and z from d2

   K=get_x(d2,ind2)*get_x(d1,ind1)';
   kertmp=kernel;
   Kdn = get_norm(kertmp,d1,ind1).^2;
   Kn = get_norm(kertmp,d2,ind2).^2;
   K = ones(length(Kn),1)*Kdn' + Kn*ones(1,length(Kdn)) - 2*K;

   [numEx vDim oDim]=get_dim(d1);
   sigma=kerParam;

%   K = 1/(2*(2*pi)^(vDim/2)*sigma) * exp(-0.5*sigma^2*K);

K= 1/(sigma*(2*pi)^(vDim/2))*exp(-K/(2*sigma^2));


function K=interpc(coef,lat)

j=fix(lat/15);
if j<1
    K=coef(1); return;
end

if j>4
    K=coef(5); return;
end

K=coef(j)*(1-lat/15+j)+coef(j+1)*(lat/15-j);

return