classdef StudentTD
%%STUDENTD Functions to handle the multivariate Student-t distribution.
%    Note that the multivariate Student-t distribution with one degree of
%    freedom is the same as the multivariate Cauchy distribution.
%    Also the scalar student-t distribution is the distribution of
%    x/sqrt(y/n) where x is a normal random variable, y is a central
%    chi squared random variable and n is the number of degrees of freedom
%    of y. In terms of tracking, the distribution can be of interest,
%    because, as noted in [1], angular measurements in the presence of
%    glint have been fit to Cauchy distributions in some instances.
%Implemented methods are: mean, cov, PDF,
%   Methods only for scalar distributions: CDF, invCDF
%   Methods only for scalar, central, non-scaled distributions: rand,
%                                                               entropy
%
%REFERENCES:
%[1] U. Nickel, "Angular superresolution with phased array radar: A review
%    of algorithms and operational constraints," IEEE Proceedings, vol.
%    134, Part F, no. 1, pp. 53-59, Feb. 1987.
%
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

methods(Static)

    function val=mean(mu,nu)
    %%MEAN  Obtain the mean of the multivariate Student-t distribution for
    %       given location and scale parameters and degrees of freedom.
    %
    %INPUTS: mu The DX1 location vector of the student's-t distribution.
    %        nu The scalar number of degrees of freedom of the Student-t
    %           distribution. nu>=0.
    %
    %OUTPUTS: val The mean of the multivariate Student-t distribution.
    %
    %The mean is undefined if nu<=1.
    %
    %October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

        if(nu>1)
            val=mu;
        else
            val=NaN;
        end
    end

    function val=cov(Sigma,nu)
    %%MEAN  Obtain the covariance matrix of the multivariate Student-t
    %       distribution for given location and scale parameters and degrees
    %       of freedom.
    %
    %INPUTS: Sigma The DXD symmetric, positive definite scale matrix.
    %           nu The scalar number of degrees of freedom of the Student-t
    %              distribution. nu>=0.
    %
    %OUTPUTS: val The covariance matrix of the multivariate Student-t
    %             distribution.
    %
    %The covariance matrix is undefined if nu<=2.
    %
    %October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

        if(nu>2)
            val=nu/(nu-2)*Sigma;
        else
            val=NaN;
        end
    end

    function val=PDF(x,mu,Sigma,nu)
    %%PDF Evaluate the monovariate or multivariate Student-t distribution at
    %     the desired point.
    %
    %INPUTS: x The DX1 vector at which the (possibly multivariate) Student's-t
    %          distribution is to be evaluated.
    %       mu The DX1 location vector of the student's-t distribution.
    %    Sigma The DXD symmetric, positive definite scale matrix.
    %       nu The scalar number of degrees of freedom of the Student-t
    %          distribution. nu>=0.
    %
    %OUTPUTS: val The PDF of the Student's-t distribution at x with the given
    %             parameters.
    %
    %The vector version of the Student-t Distribution is given in Appendix B of
    %[1]. As nu->Inf, the distribution reduces to a multivariate Gaussian
    %distribution with mean mu and covariance matrix Sigma.
    %
    %Logarithms are used in the implementation to reduce the effect of
    %precision problems that can arise if nu is large. The problems arise due
    %to the ratio of gamma functions.
    %
    %REFERENCES:
    %[1] C. M. Bishop, Pattern Recognition and Machine Learning. Cambridge,
    %    United Kingdom: Springer, 2007.
    %
    %October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.

        D=size(x,1);

        diff=x-mu;
        Delta2=diff'*pinv(Sigma)*diff;

        num=gammaln((nu+D)/2);
        denom=gammaln(nu/2)+log(sqrt(det(nu*pi*Sigma)))+((D+nu)/2)*log(1+Delta2/nu);

        val=num-denom;
        val=exp(val);
    end

    function val=CDF(x,nu,mu,sigma)
    %%CDF Evaluate cumulative distribution function (CDF) of a a scalar
    %     Student-t distribution at a specified points given the mean
    %     and the variance, or for a StudentT(nu,0,1) distribution if the
    %     mean and variance are omitted.
    %
    %INPUTS: x: a vector of values at which the CDF will be evaluated
    %       nu: scalar degrees of freedom
    %       mu: scalar mean parameter
    %    sigma: scalar scale parameter such that t = (x-mu)/sigma is a standard
    %           t-distributed variable. Note this is not the standard
    %           deviation.
    %
    %OUTPUTS: val: The scalar value(s) of the CDF with mean mu and
    %              evaluated at the point(s) x.
    %
    %Note: This function relies on the built-in betainc functions which
    %      computes the incomplete beta function.
    %
    %October 2020 Codie T. Lewis, Naval Research Laboratory, Washington D.C.

        if(nargin<3||isempty(mu))
           mu = 0;
        end

        if(nargin<4||isempty(sigma))
           sigma = 1;
        end

        %Standardize inputs
        t = (x-mu)/sigma;

        down = t<0;

        s = nu./(t.^2+nu);
        val = 1-0.5*betainc(s,nu/2,1/2);
        val(down) = 1-val(down);

    end

    function val=invCDF(prob,nu,mu,sigma)
    %%invCDF Evaluate the inverse cumulative distribution function (CDF) of
    %        a scalar Student-t distribution at a specified points given the
    %        mean and the variance, or for a StudentT(nu,0,1) distribution if
    %        the mean and variance are omitted.
    %
    %INPUTS: prob: a vector of values at which the invCDF will be evaluated
    %          nu: scalar degrees of freedom
    %          mu: scalar mean parameter
    %       sigma: scalar scale parameter such that t = (x-mu)/sigma is a
    %              standard t-distributed variable. Note this is not the
    %              standard deviation.
    %
    %OUTPUTS: val: The scalar value(s) of the invCDF with mean mu and
    %              evaluated at the point(s) x.
    %
    %Note: This function relies on the built-in betaincinv function which
    %      computes the incomplete beta function.
    %
    %October 2020 Codie T. Lewis, Naval Research Laboratory, Washington D.C.

        if(nargin<3||isempty(mu))
           mu = 0;
        end

        if(nargin<4||isempty(sigma))
           sigma = 1;
        end

        up = prob>=1/2;
        down = prob<1/2;
        t = zeros(size(prob));

        t(up) = sign(prob(up)-0.5).*sqrt((nu./betaincinv(2*(1-prob(up)),nu/2,1/2))-nu);
        t(down) = sign(prob(down)-0.5).*sqrt((nu./betaincinv(2*(prob(down)),nu/2,1/2))-nu);

        val = mu+t*sigma;
    end

    function vals=rand(N,nu)
    %%RAND Generate scalar Student-t random variables with unit scale factor
    %      and a given number of degrees of freedom.
    %
    %INPUTS: N If N is a scalar, then rand returns an NXN matrix of random
    %          variables. If N=[M,N1] is a two-element row vector, then rand
    %          returns an MXN1 matrix of random variables.
    %       nu The scalar number of degrees of freedom of the Student-t
    %          distribution. nu>=0.
    %
    %OUTPUTS: vals A matrix whose dimensions are determined by N of the
    %              generated scalar Student-t random variables.
    %
    %The algorithm implemented is the TIR algorithm in [1].
    %
    %REFERENCES:
    %[1] A. J. Kinderman, J. F. Monahan, and J. G. Ramage, "Computer methods
    %    for sampling from student's t distribution," Mathematics of
    %    Computation, vol. 31, no. 140, pp. 1009-1018, Oct. 1977.
    %
    %August 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

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

    vals=zeros(dims);
    numVals=numel(vals);

    b=sqrt(2*exp(-1/2)-1);
    alpha=nu;
    for curVal=1:numVals
        while(1)
            %Step 1
            u=rand(1);

            if(u<b/2)
                x=4*u-b;
                %Step 2
                v=rand(1);
                if(v<=1-abs(x)/2)
                   vals(curVal)=x;
                   break;
                end

                uAlpha=(1+x^2/alpha)^(-(alpha+1)/2);

                if(v<=uAlpha)
                    vals(curVal)=x;
                    break;
                end
                continue;
            end

            if(u<0.5)
                %Step 3
                temp=4*u-1-b;
                x=(abs(temp)+b)*sign(temp);
                v=rand(1);

                %Step 4
                if(v<=1-abs(x)/2)
                    vals(curVal)=x;
                    break;
                end

                if(v>=(1+b^2)/(1+x^2))
                    continue;
                end

                uAlpha=(1+x^2/alpha)^(-(alpha+1)/2);
                if(v<=uAlpha)
                    vals(curVal)=x;
                    break;
                end
                continue;
            end

            if(u<0.75)
                %Step 5
                temp=8*u-5;
                x=2/((abs(temp)+1)*sign(temp));
                u1=rand(1);
                v=x^(-2)*u1;

                %Step 4 again
                if(v<=1-abs(x)/2)
                    vals(curVal)=x;
                    break;
                end

                if(v>=(1+b^2)/(1+x^2))
                    continue;
                end

                uAlpha=(1+x^2/alpha)^(-(alpha+1)/2);
                if(v<=uAlpha)
                    vals(curVal)=x;
                    break;
                end
                continue;
            end

            %Step 6
            x=2/(8*u-7);
            v=rand(1);

            uAlpha=(1+x^2/alpha)^(-(alpha+1)/2);
            if(v<x^2*uAlpha)
                vals(curVal)=x;
                break;
            end
        end
    end
    end

    function entropyVal=entropy(nu)
    %%ENTROPY Obtain the differential entropy in nats of the scalar Student-t
    %         distribution with unit scale factor and a given number of degrees
    %         of freedom.  The differential entropy of a continuous
    %         distribution is entropy=-int_x p(x)*log(p(x)) dx where the
    %         integral is over all values of x. Units of nats mean that the
    %         natural logarithm is used in the definition. Unlike the Shannon
    %         entropy for discrete variables, the differential entropy of
    %         continuous variables can be both positive and negative.
    %
    %INPUTS: nu The scalar number of degrees of freedom of the Student-t
    %           distribution. nu>=0.
    %
    %OUTPUTS: entropyVal The value of the differential entropy in nats.
    %
    %Differential entropy is defined in Chapter 8 of [1].
    %
    %REFERENCES:
    %[1] T. M. Cover and J. A. Thomas, Elements of Information Theory, 2nd ed.
    %    Hoboken, NJ: Wiley-Interscience, 2006.
    %
    %April 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

        entropyVal=((nu+1)/2)*(psi((nu+1)/2)-psi(nu/2))+(1/2)*log(nu)+betaln(nu/2,1/2);
    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.


%%This Matlab script generates Figure 5 in the paper:
%
%Emil Björnson, Özgecan Özdogan, Erik G. Larsson, “Intelligent Reflecting
%Surface vs. Decode-and-Forward: How Large Surfaces Are Needed to Beat
%Relaying?,” IEEE Wireless Communications Letters, To appear
%
%Download article: https://arxiv.org/pdf/1906.03949
%
%This is version 1.0 (Last edited: 2019-10-28)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%paper as described above.

close all;
clear;

%% Set simulation parameters

%Carrier frequency (in GHz)
fc = 3;

%Bandwidth
B = 10e6;

%Noise figure (in dB)
noiseFiguredB = 10;

%Compute the noise power in dBm
sigma2dBm = -174 + 10*log10(B) + noiseFiguredB;
sigma2 = db2pow(sigma2dBm);

%Define the channel gain functions based on the 3GPP Urban Micro in
%"Further advancements for E-UTRA physical layer aspects (Release 9)."
%3GPP TS 36.814, Mar. 2010. Note that x is measured in m and that the
%antenna gains are included later in the code
pathloss_3GPP_LOS = @(x) db2pow(-28-20*log10(fc)-22*log10(x));
pathloss_3GPP_NLOS = @(x) db2pow(-22.7-26*log10(fc)-36.7*log10(x));

%Define the antenna gains at the source, relay/IRS, and destination. The
%numbers are in linear scale
antennaGainS = db2pow(5);
antennaGainR = db2pow(5);
antennaGainD = db2pow(0);

%Set the amplitude reflection coefficient
alpha = 1;

%Set the range of rate values
Rbar = [0.01 0.1:0.1:10];

%Set parameters related to circuit power consumption
Ps = 100; %Power dissipation in the transceiver hardware of the source
Pd = 100; %Power dissipation in the transceiver hardware of the destination
Pe = 5;   %Power dissipation per element in the IRS (mW)
Pr = 100; %Power dissipation in the transceiver hardware of the relay
nu = 0.5; %Efficiency of the power amplifier at the source

%Define distances in simulation setup

d_SR = 80; %Distance between the source and IRS/relay
dv = 10; %Minimum distance between destination and the IRS/relay

%Define the range of d1 values in the simulation setup
d1 = 70;

%Copmute distance between the source and destination
d_SD = sqrt(d1^2+dv^2);

%Compute distance between the IRS/relay and destination
d_RD = sqrt((d1-d_SR)^2+dv^2);

%Compute the channel gains using the 3GPP models and antenna gains
betaSR = pathloss_3GPP_LOS(d_SR)*antennaGainS*antennaGainR;
betaRD = pathloss_3GPP_LOS(d_RD)*antennaGainR*antennaGainD;
betaSD = pathloss_3GPP_NLOS(d_SD)*antennaGainS*antennaGainD;

%Prepare to save simulation results
EE_SISO = zeros(length(Rbar),1);
EE_IRS = zeros(length(Rbar),1);
EE_DF = zeros(length(Rbar),1);
Nopt = zeros(length(Rbar),1);

%% Go through all rate values
for ind = 1:length(Rbar)

    %Compute required SINR values
    SINR = 2^(Rbar(ind))-1; %SISO and IRS
    SINR_DF = 2^(2*Rbar(ind))-1; %DF relaying

    %Compute the transmit power in the SISO case, using Eq. (17)
    P_SISO = SINR*sigma2/betaSD;

    %Compute the energy efficiency in the SISO case
    %(the factor 1000 is used to convert mW to W)
    EE_SISO(ind) = 1000*B*Rbar(ind)/(P_SISO/nu + Ps + Pd);

    %Compute the transmit power in the DF relaying case, using Eq. (19)
    P_DF = SINR_DF*sigma2*(betaSR+betaRD-betaSD)/(2*betaRD*betaSR);

    %Compute the energy efficiency in the DF relaying case
    %(the factor 1000 is used to convert mW to W)
    EE_DF(ind) = 1000*B*Rbar(ind)/(P_DF/nu + Ps/2 + Pd + Pr);

    %Compute the power-minimizing number of reflecting elements
    Nopt(ind) = (2*SINR*sigma2/(alpha^2*betaSR*betaRD*Pe))^(1/3) - sqrt(betaSD/(betaSR*betaRD))/alpha;

    if Nopt(ind)<0
        Nopt(ind) = 0;
    end

    %Compute the transmit power in the IRS case, using Eq. (18)
    P_IRS = SINR*sigma2./(sqrt(betaSD) + Nopt(ind)*alpha*sqrt(betaSR*betaRD)).^2;

    %Compute the energy efficiency in the IRS case
    %(the factor 1000 is used to convert mW to W)
    EE_IRS(ind) = 1000*B*Rbar(ind)/(P_IRS/nu + Ps + Pd + Nopt(ind)*Pe);

end

%Plot simulation results
figure;
hold on; box on;
plot(Rbar,EE_DF/1e6,'b-.','LineWidth',2);
plot(Rbar,EE_IRS/1e6,'r-','LineWidth',2);
plot(Rbar,EE_SISO/1e6,'k--','LineWidth',2);
xlabel('Achievable rate [bit/s/Hz]','Interpreter','Latex');
ylabel('Energy efficiency [Mbit/Joule]','Interpreter','Latex');
legend('DF relay','IRS','SISO','Location','NorthWest');
set(gca,'fontsize',18);


% Distortionless data hiding based on integer wavelet transform (Watermark Embedding)
% Guorong Xuan   Jiang Zhu   Jidong Chen   Shi, Y.Q.   Zhicheng Ni   Wei Su
% Department of Computer Science, Tongji University, Shanghai
%
% This paper appears in: Electronics Letters
% Publication Date: 5 Dec 2002
% Volume: 38,  Issue: 25
% On page(s): 1646- 1648
% ISSN: 0013-5194

% August 12, 2007, 4:37pm
% updated August 19, 2007, 5:45pm
% updated 13 December, 2008
% By: Asad (asad_82@yahoo.com)

% NOTE: In case of error change the bit plane number as embedding may not be possible using that bitplane.
% Also in order to achieve more bpp, bit plane number can be changed.

clear all;
close all;

% define constants
BITPLANE_NUMBER = 4; %4 % specifies the bitplane number to embedd watermark
ERROR_NUM = 16; %16 % prevents this gray value from being used in embedding
WM_SIZE = 300;
% select the bitplane number and then the corresponding error num from the
% table below
% BITPLANE_NUMBER = [1     2   3   4  5  6  7  8]
% ERROR_NUM       = [128  64  32  16  8  4  2  1]
disp('------------------------ Embedding -------------------------------');

% read image and convert to gray scale if necessary
originalImage = imresize(imread('lena.tif'),[512 512],'bicubic');
if (isrgb(originalImage))
    originalImage = rgb2gray(originalImage);
end

% STEP 1: perform necessary preprocessing
%[valCount,X] = imhist(originalImage);

% STEP 2: perfom wavelet decomposition
LS = liftwave('cdf2.2','Int2Int');
[CA,CH,CV,CD] = lwt2(double(originalImage),LS);

% STEP 3: construct binary images from 5th bit of CH, CV and CD
for i=1:size(CH,1)
    for j=1:size(CH,2)
        % for constructing binary image using CH
        binSeq = dec2bin(abs(CH(i,j)),8);
        if binSeq(BITPLANE_NUMBER) == '1'
            bICH5(i,j) = 2;
        else
            bICH5(i,j) = 1;
        end
        % for constructing binary image using CV
        binSeq = dec2bin(abs(CV(i,j)),8);
        if binSeq(BITPLANE_NUMBER) == '1'
            bICV5(i,j) = 2;
        else
            bICV5(i,j) = 1;
        end
        % for constructing binary image using CD
        binSeq = dec2bin(abs(CD(i,j)),8);
        if binSeq(BITPLANE_NUMBER) == '1'
            bICD5(i,j) = 2;
        else
            bICD5(i,j) = 1;
        end
    end
end

% STEP 4a: Compress data in 5th Bit plane of CH
% find how many times 1 occurs in the level 5 binary bICH5 (horizontal)
[xx,yy,val] = find(bICH5 == 1);
totalVals = size(CH,1)*size(CH,2);

% code the sequence using arithmetic coding
count1 = [round((size(xx,1)/totalVals)*100) round(((totalVals - size(xx,1))/totalVals) * 100)];
seq1 = reshape(bICH5,1,size(CH,1)*size(CH,2));
arcCH5 = arithenco(seq1,count1);
str = sprintf('Original CH Bits Length = %d ------ Compressed CH Bits Length = %d',size(seq1,2),size(arcCH5,2));
disp(str);

% STEP 4b: Compress data in 5th Bit plane of CV
% find how many times 1 occurs in the level 5 binary bICV5 (vertical)
[xx,yy,val] = find(bICV5 == 1);
totalVals = size(CV,1)*size(CV,2);

% code the sequence using arithmetic coding
count2 = [round((size(xx,1)/totalVals)*100) round(((totalVals - size(xx,1))/totalVals) * 100)];
seq2 = reshape(bICV5,1,size(CV,1)*size(CV,2));
arcCV5 = arithenco(seq2,count2);
str = sprintf('Original CV Bits Length = %d ------ Compressed CV Bits Length = %d',size(seq2,2),size(arcCV5,2));
disp(str);

% STEP 4c: Compress data in 5th Bit plane of CD
% find how many times 1 occurs in the level 5 binary bICD5 (diagonal)
[xx,yy,val] = find(bICD5 == 1);
totalVals = size(CD,1)*size(CD,2);

% code the sequence using arithmetic coding
count3 = [round((size(xx,1)/totalVals)*100) round(((totalVals - size(xx,1))/totalVals) * 100)];
seq3 = reshape(bICD5,1,size(CD,1)*size(CD,2));
arcCD5 = arithenco(seq3,count3);
str = sprintf('Original CD Bits Length = %d ------ Compressed CD Bits Length = %d',size(seq3,2),size(arcCD5,2));
disp(str);

% STEP 5: read the watermark and reshape it for insertion
watermark = imresize(rgb2gray(imread('logo_im.jpg')),[WM_SIZE WM_SIZE],'bicubic');
%watermark = imresize(DMlcd5,[WM_SIZE WM_SIZE],'bicubic');
watermark = im2bw(watermark);
%figure,imshow(watermark,[]),title('Watermark');
watermark = reshape(watermark,1,WM_SIZE*WM_SIZE);

% STEP 6: Insert the watermark and compressed data into the image
% compute length of data to insert
dataLength = size(watermark,2) + size(arcCH5,2) + size(arcCV5,2) + size(arcCD5,2) + 6*8 + 4*16;%(2*8)*3;
available = size(CH,1)*size(CH,2) + size(CV,1)*size(CV,2) + size(CD,1)*size(CD,2);

if available < dataLength
    disp('Data to Embedd must be less than available limit.');
end

% allocate memory and initialize
embedData = zeros(1,dataLength);
% insert the header information
str1 = dec2bin(count1(1,1),8); str2 = dec2bin(count1(1,2),8);
header = strcat(str1,str2);
str1 = dec2bin(count2(1,1),8); str2 = dec2bin(count2(1,2),8);
header = strcat(header,str1,str2);
str1 = dec2bin(count3(1,1),8); str2 = dec2bin(count3(1,2),8);
header = strcat(header,str1,str2);
% insert length of each sequence for decoding
str1 = dec2bin(size(arcCH5,2),16); str2 = dec2bin(size(arcCV5,2),16);
str3 = dec2bin(size(arcCD5,2),16); str4 = dec2bin(size(watermark,2),32);
header = strcat(header,str1,str2,str3,str4);

for i=1:size(header,2)
    if header(1,i) == '1'
        embedData(1,i) = 1;
    else
        embedData(1,i) = 0;
    end
end
% get the header length
HL = size(header,2);
% concatenate data to get a single compressed data vector
embedData(1,HL+1:HL+size(arcCH5,2)) = arcCH5(1,:);
embedData(1,HL+size(arcCH5,2)+1:(HL+size(arcCH5,2) + size(arcCV5,2))) = arcCV5(1,:);
embedData(1,HL+size(arcCH5,2)+ size(arcCV5,2)+1:(HL + size(arcCH5,2) + size(arcCV5,2) + size(arcCD5,2))) = arcCD5(1,:);
embedData(1,HL+size(arcCH5,2)+ size(arcCV5,2)+size(arcCD5,2)+1:(HL+size(arcCH5,2) + size(arcCV5,2) + size(arcCD5,2)+ size(watermark,2))) = watermark(1,:);
originalBitsLength = HL + size(arcCH5,2)+ size(arcCV5,2)+size(arcCD5,2);

% embedd compressed data, watermark and header information
index = 1; % counter for obtaining 8 bit chunks
brk = 0;
for x=1:size(CH,1)
    for y=1:size(CH,2)
        if CH(x,y) ~= -ERROR_NUM
            neg = 0;
            if CH(x,y) < 0
                neg = 1;
            end
            binSeq = dec2bin(abs(CH(x,y)),8);
            if embedData(1,index) == 1
                binSeq(BITPLANE_NUMBER) = '1';
            else
                binSeq(BITPLANE_NUMBER) = '0';
            end
            num = bin2dec(binSeq);
            if neg == 1
                CH(x,y) = num * -1;
            else
                CH(x,y) = num;
            end

            % break the loop if all watermark bits are embedded
             if index < size(embedData,2)
                index = index + 1;
             else
                 brk = 1; break;
             end
        end
    end
    if brk == 1
        break;
    end
end

if index < size(embedData,2)
    for x=1:size(CV,1)
        for y=1:size(CV,2)
            if CV(x,y) ~= -ERROR_NUM
                neg = 0;
                if CV(x,y) < 0
                    neg = 1;
                end
                binSeq = dec2bin(abs(CV(x,y)),8);
                if embedData(1,index) == 1
                    binSeq(BITPLANE_NUMBER) = '1';
                else
                    binSeq(BITPLANE_NUMBER) = '0';
                end
                num = bin2dec(binSeq);
                if neg == 1
                    CV(x,y) = num * -1;
                else
                    CV(x,y) = num;
                end

                % break the loop if all watermark bits are embedded
                 if index < size(embedData,2)
                   index = index + 1;
                 else
                     brk = 1; break;
                 end
            end
        end
        if brk == 1
            break;
        end
    end
 end

if index < size(embedData,2)
    for x=1:size(CD,1)
        for y=1:size(CD,2)
            if CD(x,y) ~= -ERROR_NUM
                neg = 0;
                if CD(x,y) < 0
                    neg = 1;
                end
                binSeq = dec2bin(abs(CD(x,y)),8);
                if embedData(1,index) == 1
                    binSeq(BITPLANE_NUMBER) = '1';
                else
                    binSeq(BITPLANE_NUMBER) = '0';
                end
                num = bin2dec(binSeq);
                if neg == 1
                    CD(x,y) = num * -1;
                else
                    CD(x,y) = num;
                end

                % break the loop if all watermark bits are embedded
                if index < size(embedData,2)
                    index = index + 1;
                 else
                     brk = 1; break;
                end
            end
        end
        if brk == 1
            break;
        end
    end
 end

% compute inverse integer wavelet transform
watermarkedImage = ilwt2(CA,CH,CV,CD,LS);
% [tx,ty,tval] = find(watermarkedImage > 256);
% if ~isempty(tx)
%     disp('Watermarked Image values greater than 256');
% end
% [tx,ty,tval] = find(watermarkedImage <= 0);
% if ~isempty(tx)
%     disp('Watermarked Image values less than equal to 0');
% end

imwrite(uint8(watermarkedImage),'Watermarked Image.bmp','bmp');

str = sprintf('Payload(bpp) = %f -- Embedded Data(Header+Original Bits+Watermark) = %d bits -- Watermark Length = %d bits',(dataLength-originalBitsLength)/(4*(size(CH,1)*size(CH,1))),dataLength+HL,size(watermark,2));
disp(str);

[PSNR_OUT,Z] = psnr(originalImage,watermarkedImage);
str = sprintf('PSNR = %f',PSNR_OUT);
disp(str);

figure,imshow(originalImage,[]),title('Original Image');
figure,imshow(watermarkedImage,[]),title('Watermarked Image');

save WatermarkInfo watermarkedImage watermark BITPLANE_NUMBER WM_SIZE ERROR_NUM originalImage;

% WMI = imread('Watermarked Image.bmp');
% difference = double(watermarkedImage) - double(WMI);
% figure,imshow(difference,[]),title('Difference Image');


%% ParticleFilterX Demo
% ----------------------
% * This script demonstrates the process of configuring an running a
%   ParticleFilterX object to perform single-target state estimation.
%
% * A toy single-target scenario is considered, for a target that generates
%   regular reports of it's position in the absence of clutter and/or
%   missed detection.
%
%% Extract the GroundTruth data from the example workspace
load('single-target-tracking.mat');
NumIter = size(TrueTrack.Trajectory,2);

%% Models
% Instantiate a Transitionamic model
transition_model = ConstantHeadingX('VelocityErrVariance',0.01^2, 'HeadingErrVariance', (pi/20)^2);

% Instantiate an Observation model
measurement_model = LinearGaussianX('NumMeasDims',2,'NumStateDims',4,'MeasurementErrVariance',0.02,'Mapping',[1 3]);
%measurement_model = RangeBearing2CartesianX('NumStateDims',4,'MeasurementErrVariance',[0.001,0.02],'Mapping',[1 3]);

% Compile the State-Space model
model = StateSpaceModelX(transition_model,measurement_model);

%% Simulation
% Data Simulator
dataSim = SingleTargetMeasurementSimulatorX(model);

% Simulate some measurements from ground-truth data
MeasurementScans = dataSim.simulate(TrueTrack);
measurements = [MeasurementScans.Vectors];
measurement_model = LinearGaussianX('NumMeasDims',2,'NumStateDims',4,'MeasurementErrVariance',0.02,'Mapping',[1 2]);
model.Measurement = measurement_model;

%% Initiation
% Use the first measurement scan to perform single-point initiation
measurement = MeasurementScans(1).Measurements;
timestamp = measurement.Timestamp;

% Setup prior
xPrior = measurement_model.finv(measurement.Vector);
PPrior = 10*transition_model.covar();
dist = GaussianDistributionX(xPrior,PPrior);
StatePrior = ParticleStateX(dist,5000,timestamp);

% Initiate a track using the generated prior
track = TrackX(StatePrior, TagX(1));

%% Estimation
% Instantiate a filter objects
filter = ParticleFilterX('Model',model);
figure;
for t = 2:NumIter

    % Provide filter with the new measurement
    MeasurementList = MeasurementScans(t);

    % Perform filtering
    prior = track.State;
    prediction = filter.predict(prior, MeasurementList{1}.Timestamp);
    posterior = filter.update(prediction, MeasurementList);

    % Log the data
    track.Trajectory(end+1) = posterior;

    clf;
    hold on;
    meas = measurement_model.finv(measurements(:,1:t));
    true_means = [TrueTrack.Trajectory(1:t).Vector];
    track_means = [track.Trajectory(1:t).Mean];
    plot(true_means(1,1:t), true_means(2,1:t),'.-k', track_means(1,1:t), track_means(2,1:t), 'b-', meas(1,:), meas(2,:), 'rx');
    plot_gaussian_ellipsoid(track.Trajectory(t).Mean([1,2],1), track.Trajectory(t).Covar([1,2],[1,2]));
    legend('GroundTrouth','Estimated Mean','Measurements', 'Estimated Covariance');
    xlabel("x coordinate (m)");
    ylabel("y coordinate (m)");
    axis([2 9 1 9]);
    drawnow();
end


function [Coh] = SLC_cov(slcstack,SHP)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   This file is part of TomoSAR.
%
%   TomoSAR is distributed in the hope that it will be useful,
%   but without warranty of any kind; without even the implied
%   warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
%   See the Apache License for more details.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Author : Dinh Ho Tong Minh (INRAE) and Yen Nhi Ngo, Jan. 2022
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nlines,nwidths,n_slc]=size(slcstack);

% Normalize
slcstack(slcstack~=0) = slcstack(slcstack~=0)./abs(slcstack(slcstack~=0));

CalWin =SHP.CalWin;
RadiusRow=(CalWin(1)-1)/2;
RadiusCol=(CalWin(2)-1)/2;

mlistack = abs(slcstack);

% Coherence matrix estimate
Coh=zeros(n_slc,n_slc,nlines,nwidths,'single');
for ii=1:n_slc
    m1_intial = mlistack(:,:,ii);
    for ss = ii+1:n_slc
        m2_intial = mlistack(:,:,ss);
        Dphi = exp(1i*(angle(slcstack(:,:,ii).*conj(slcstack(:,:,ss)))));
        Intf= sqrt(m1_intial.*m2_intial).*Dphi;

        % Padding at edge
        m1 = padarray(m1_intial,[RadiusRow RadiusCol],'symmetric');
        m2 = padarray(m2_intial,[RadiusRow RadiusCol],'symmetric');
        Intf= padarray(Intf,[RadiusRow RadiusCol],'symmetric');
        nu = zeros(nlines,nwidths,'single');
        de1=nu;
        de2=nu;
        num=1;
        for jj = 1:nwidths
            for kk= 1:nlines
                x_global  = jj+RadiusCol;
                y_global  = kk+RadiusRow;
                MasterValue= m1(y_global-RadiusRow:y_global+RadiusRow,x_global-RadiusCol:x_global+RadiusCol);
                SlaveValue = m2(y_global-RadiusRow:y_global+RadiusRow,x_global-RadiusCol:x_global+RadiusCol);
                InterfValue= Intf(y_global-RadiusRow:y_global+RadiusRow,x_global-RadiusCol:x_global+RadiusCol);
                MasterValue= MasterValue(SHP.PixelInd(:,num));
                SlaveValue = SlaveValue(SHP.PixelInd(:,num));
                InterfValue= InterfValue(SHP.PixelInd(:,num));
                nu(kk,jj)  = sum(InterfValue);
                de1(kk,jj) = sum(MasterValue);
                de2(kk,jj) = sum(SlaveValue);
                num=num+1;
            end
        end
        Coh(ii,ss,:,:) = nu./sqrt(de1.*de2);
    end
end

% Make mirror operator
temp = ones(1,n_slc) ;
for jj = 1:nwidths
    for kk= 1:nlines
        W = Coh(:, :,kk,jj) ;
        Coh(:, :,kk,jj) = W + (W - diag(temp))';
    end
end

return


function [ function_value] = SimpleQPObjective( values, Problem)
%SimpleQPObjective Calculates obj.*sum(x.^2)
%   Where obj is provided as objArguments{1} of the Problem struct.

obj = Problem.objArguments{1};

function_value = sum(obj.*values.^2);
end


% GP_COV_DAMPEDCOS - Damped cosine covariance function.
%
% c(r) = exp(-r/lengthscale) * cos( 2*pi*r/wavelength )
%
% COVFUNC = GP_COV_DAMPEDCOS(D2)
%
% [K, DK] = COVFUNC(THETA)
%
% [N_THETA, N1, N2] = COVFUNC()

function covfunc = gp_cov_dampedcos(D, varargin)

options = struct('wavelength', [],...
                 'lengthscale', [] );
[options, errmsg] = argparse(options, varargin{:});
error(errmsg);

n_theta = isempty(options.wavelength) + isempty(options.lengthscale);

covfunc = @get_covariance;

  function varargout = get_covariance(theta)

  varargout = cell(nargout,1);

  if nargin == 0
    % Return the number of parameters and the size of the covariance matrix
    if nargout >= 1
      varargout{1} = n_theta; % number of parameters
      if nargout >= 2
        varargout{2} = size(D,1); % number of rows
        if nargout == 3
          varargout{3} = size(D,2); % number of columns
        else
          error('Too many outputs');
        end
      end
    end
    return
  end

  if numel(theta) ~= n_theta
    error('Wrong number of parameters (%d), should be %d', numel(theta), n_theta);
  end

  % Parse parameters
  if isempty(options.wavelength)
      wl = theta(1);
      if isempty(options.lengthscale)
          ls = theta(2);
      else
          ls = options.lengthscale;
      end
  else
      wl = options.wavelength;
      if isempty(options.lengthscale)
          ls = theta(1);
      else
          ls = options.lengthscale;
      end
  end

  % Covariance matrix
  K = exp(-D/ls) .* cos( 2*pi*D/wl );
  varargout{1} = K;

  % Gradient for hyperparameters
  if nargout >= 2
      dK = cell(n_theta,1);
      n = 1;
      if isempty(options.wavelength)
          dK{n} = exp(-D/ls) .* sin( 2*pi*D/wl ) .* ( 2*pi*D*wl^(-2) );
          n = n + 1;
      end
      if isempty(options.lengthscale)
          dK{n} = exp(-D/ls) .* ( D*ls^(-2) ) .* cos( 2*pi*D/wl );
      end
      varargout{2} = dK;
  end

  end % function get_covariance

end


function f = schwefelsfcn(x)
% Schwefel's function.

if strcmp(x,'init')
    f.Aineq = [] ;
    f.bineq = [] ;
    f.Aeq = [] ;
    f.beq = [] ;
    f.LB = -500*ones(1,2) ; f.UB = 500*ones(1,2) ;
    f.nonlcon = [] ;
    f.options.PopulationSize = 500 ;
    f.options.PopInitRange = [-500; 500] ;
    f.options.ConstrBoundary = 'absorb' ;
    f.options.KnownMin = 420.9687*ones(1,2) ;
else
    x = reshape(x,1,[]) ;
    f = sum(-x.*sin(sqrt(abs(x))),2) ;
end


function plotGraph(vertices, edges, figureRef)

if nargin < 3
    figureRef = figure;
end

cla(figureRef);

figure(figureRef);
plot(vertices(:,1),vertices(:,2),'.');
hold on;

nv = size(vertices,1);
ne = size(edges,1);

v1 = vertices(edges(:,1),:);
v2 = vertices(edges(:,2),:);

vList = ones(ne*3,2);

vList(1:3:ne*3,:) = v1;
vList(2:3:ne*3,:) = v2;
vList(3:3:ne*3,:) = NaN;

plot(vList(:,1),vList(:,2));

% for i=1:size(edges,1)
% %     plot([vertices(edges(i,1),1) vertices(edges(i,2),1)], ...
% %          [vertices(edges(i,1),2) vertices(edges(i,2),2)]);
%
%     line([vertices(edges(i,1),1) vertices(edges(i,2),1)], ...
%          [vertices(edges(i,1),2) vertices(edges(i,2),2)]);
% end

axis equal;


function [vol_tmp,H] = iconebeamdemo2007(dir,flprefix,theta,interp,filter,d,N,step,Ro,range_slices,cut)

%   author: Gianni Schena Sept. 2004 (schena@univ.trieste.it - I welcome suggestions !)
%   Reconstructs image/s for cone beam (CB) geometry according to
%   the classical Feldkamp et al. algorithm. Individual slices are stored in a volume
%
%   FDK10_CB reconstructs the 3D volume image vol from planar projection
%   data. The routine assumes that the center of rotation
%   is the center point of the projections.
%
%   THETA describes the angles (in degrees) at which the projections
%   were taken.  It it is a vector containing angle between
%   projections. THETA is a vector and must contain angles with
%   equal spacing between them.
%
%   The routine uses the filtered backprojection.The filter is designed directly
%   in the frequency domain and then multiplied by the FFT of the
%   projections.  The projections are zero-padded to a power of 2
%   before filtering to prevent spatial domain aliasing and to
%   speed up the FFT.
%
%   INTERP specifies the type of interpolation to use in the
%   backprojection.  The available options are listed in order
%   of increasing accuracy and computational complexity:
%
%      'nearest' - nearest neighbor interpolation
%      'linear'  - linear interpolation (suggested)
%
%   FILTER specifies the filter to use for frequency domain filtering.
%   FILTER is a string that specifies any of the following standard
%   filters:
%
%   'Ram-Lak'     The cropped Ram-Lak or ramp filter (default).  The
%                 frequency response of this filter is |f|.  Because
%                 this filter is sensitive to noise in the projections,
%                 one of the filters listed below may be preferable.
%   'Shepp-Logan' The Shepp-Logan filter multiplies the Ram-Lak
%                 filter by a sinc function.
%   'Cosine'      The cosine filter multiplies the Ram-Lak filter
%                 by a cosine function.
%   'Hamming'     The Hamming filter multiplies the Ram-Lak filter
%                 by a Hamming window.
%   'Hann'        The Hann filter multiplies the Ram-Lak filter by
%                 a Hann window.
%
%   D is a scalar in the range (0,1] that modifies the filter by
%   rescaling its frequency axis.  The default is 1.  If D is less
%   than 1, the filter is compressed to fit into the frequency range
%   [0,D], in normalized frequencies; all frequencies above D are set
%   to 0.
%
%   N is a scalar that specifies the number of rows and columns in the
%   reconstructed 2D slice images.  If N is not specified, the size is determined
%
%   [vol,H] = FDKX... (...) returns also the frequency response of the filter
%   in the vector H.
%
%   Ro is the orbital radius i.e. the distance source to object
%   Ds2dct is the distance between source and planar detector
%   Ds2dct = Ro + Do2dct = Ds2obj + Do2dct
%   pxlsz is the size of the (square) pixel of the detector dct
%   Class Support : All input arguments must be of class double.
%   The output arguments are of class double.

%   References:
%   A. C. Kak, Malcolm Slaney, "Principles of Computerized Tomographic
%   Imaging", IEEE Press 1988. Chapter 3 (available by downloading)

%[p,theta,filter,d,interp,N] = parse_inputs(varargin{:});
thetag=theta;
theta = pi*theta/180; % from degrees to radiants

A=read_1rad(1.,dir,flprefix); % read in image frame
figure , imshow(A,[]); title(' first projection') % disp first image file

%global flat , global dark

% get size of the data files
[nr_p , nc_p] = size(A) ; % get the number of rows and colums of the data files
%
range_slices=range_slices(range_slices>-nr_p/2 & range_slices<nr_p/2);
% remove slices out of range
%  N is a scalar that specifies the number of rows and columns in the
%  reconstructed 2D slice images.  If N is not specified, the size is determined
%  If the user didn't specify the size of the reconstruction, so
%  deduce it from the length of projections

% nr_p; % sinogram size i.e. one size of the projection

if N==0  ||  isempty(N) % if N is not given then ...
    N = 2*floor( nc_p /(2*sqrt(2)) ); % take the default value
end

imgDiag = 2*ceil(N/sqrt(2))+1;  % largest distance through image.

%np = length(theta);
W=pix_weight(nr_p,nc_p,Ro,0,0); % weights to account for a planar detector
%size(W)

% Design the filter H
len=nc_p;
H = designFilter(filter, len, d);
LH=length(H); % length filter in the line vector H

% Define the x & y axes for the reconstructed image so that the origin
xax = (1:N)-ceil(N/2); x = repmat(xax, N, 1);  y= repmat(xax', 1, N);
%y = rot90(x);   % x coordinates, the y coordinates are rot90(x)
mask= ((x-0).^2+(y-0).^2) <= (N/2).^2 ; % mask the inner circle of diam N
mask=true(N); % no mask : all  the pixels are to be reconstructed
maskV=mask(:); % mask in a vector

costheta = cos(theta); sintheta = sin(theta);
ctrIdx = ceil(len/2);     % index of the center of the projections (on the hor axis)

% range slices  to be recons
if exist('range_slices','var') & ~isempty(range_slices)
    slices=range_slices; % slices to be recons
    if isempty( find(slices==0) ) % se non c'e' la slice centrale aggiungila
        slices=[slices ,0];  slices=sort(slices) ;
    end
else
    % if no slices then recns the slice z=0 only
    slices=[0];% [-floor(nc_p/2):step:+floor(nc_p/2)];
end
cnt_slc=find(slices==0);
slices

%vol = repmat(0,[N,N,length(slices)]) ;% allocate 3D volume matrix
vol_tmp=repmat(single(0),[(N.^2),length(slices)]); % allocation
szproj=[nr_p,nc_p;]; % size(proj);
%

% Filtered Back-projection - i.e. from proj pixel (u,v) to voxel (x,y,z)
if strcmp(interp, 'nearest neighbor')
    x=x(maskV); y=y(maskV); % mask !

    nw_slices = real(sort(complex(slices))) ; % sort and reordering for faster reconstruction
   % eg. +5 -5 + 6 -6 ...it allows to change the sign of v rather than recalculating v

    ZFlag= abs(nw_slices(2:end)) == nw_slices(1:(end-1)) ; % logical variable
    ZFlag=real([0, ZFlag] ); % true if +z is followed by -z and one can exploit symmetry

    for ipos=1:length(slices) % right positioning the re-orded slices in the volume
        iposz_v(ipos)=find( nw_slices(ipos)== slices) ;  % slice correspondence
    end
    % data type and array allocation statements
    proj=repmat(0,nr_p,nc_p);
    tmp=zeros(nnz(mask),1); tmp2=zeros(N.^2,1); % allocate memory to temporary arrays
   % tmp1
    v0=zeros([size(x)]); v1=v0; u1=v1; % u=u1; v=u;
    good_u=logical(v0); idx=zeros(size(v0)); % allocation

    for i=1:length(theta) % loop over the projections
       % i
        dgri=thetag(i); % theta in degrees

        proj=read_1rad(i,dir,flprefix); % read in the i-th radiograph
        proj = radio2proj(proj,W,H,len); % tranforms radiog. into projection and multiply by W
        %%%
        proj(:,length(H))=0;  % Zero pad projections for
        for ir = 1:nr_p; % filtering row  by row  with row vector H
            proj(ir,:)= real (ifft ( fft(proj(ir,:)).*H ) );  % fast frequency domain filtering
        end
        proj(:,len+1:end) = [] ;   % Truncate the filtered projections
        %%%
        s =  x.*costheta(i) + y.*sintheta(i); t = -x.*sintheta(i) + y.*costheta(i); % Goddard & Trepanier
        % s = x.*costheta(i) + y.*sintheta(i); % also in matlab iradon
        R_s = Ro-s; u=round( Ro*t./(R_s) + ctrIdx) ; % u is the 1st projection pixel coordinate
        good_u = (u>0 & u<(nc_p+1)) ; % only valid pixels -i.e. within the projection frame
        lengoodu=length(good_u);
        u1=u(good_u);%
        inv_R_s_quad=1./(R_s(good_u).^2); Ro_dev_R_s=Ro./(R_s(good_u)); % for generally .* is faster than ./
        iz=0;
        id_1_term=(u1-1)*szproj(1); % does not depend up on v
        for z=nw_slices % for all the (re-ordered) slices
            iz=iz+1;  % z slice under reconstruction
           if ZFlag(iz)==1; % z symmetry (floting point is faster than logical operators!)
          %  if ( iz>=2 & ( abs(nw_slices(iz)) == nw_slices(iz-1) ) ) % z symmetry
                v0=-v0;   % allows calc time saving
            else %  v is the 2nd proj. pixel coord. (u,v)
                v0=Ro_dev_R_s.*z ;  %
            end
            v1=round( v0+nr_p/2 ) ;  % coord. correction
            %v1=v(good_u);  % logical operations i.e. only valid pixels
            idx = id_1_term + v1 ; % idx = sub2ind(szproj,u1, v1);
            tmp = proj(idx); % just in case
            tmp = tmp(:).*inv_R_s_quad; % back-projection ... from pixels to voxels ...!
            tmp1(good_u)=tmp; tmp1(lengoodu)=0;%
            tmp2(maskV)=tmp1; % prepare for image reconst. within the mask
            iposz=iposz_v(iz); % position of the current slice iz in the volume at iposz
            vol_tmp(:,iposz)=vol_tmp(:,iposz)+tmp2; % sum up back-projections without reshaping
        end % loop over z
    end % end loop over theta for the nearest neighbout method
    vol_tmp=reshape(vol_tmp,[N,N,length(slices)]) ; % reshape 1D vector into 2D image for each z
    % shows central slice only , in figure 10
    figure(10), imshow(fliplr(vol_tmp(:,:,cnt_slc)'),[]); % visualize central slice after vol reconstruction
    stringa = [' NN back-prj. cntrl slice , radiog. degrees: ' , num2str(dgri)];
    title(stringa);   colormap(gray(256));
    display(' done cone beam back projection  ! - nearest neighbour method')
    %***********************************************************
end

if  strcmp(interp, 'bilinear')
    x=x(mask); y=y(mask); % mask !
    temp_1=zeros(nnz(mask(:)),1); temp_2=zeros(nnz(mask(:)),1);
    tmp2=zeros(N.^2,1); % viene retroproiettato
    for i=1:length(theta) % loop over projections
    %   i
        dgri=thetag(i); % angolo corrente in gradi
        proj=read_1rad(i,dir,flprefix); % read in the i-th projection
        proj = radio2proj(proj,W,H,len);

        proj(:,length(H))=0;  % Zero pad projections
        for ir = 1:nr_p; % filtering row  by row
            proj(ir,:)=real (ifft ( fft(proj(ir,:)).*H ) );  % fast frequency domain filtering
        end % done for each row
        proj(:,len+1:end) = [];   % Truncate the filtered projections
        proj=proj(:);
        proj1=[proj(2:end,:); proj(end,:)]; proj1=proj1(:);
        % imshow(proj,[ ]) ;  title(' log proj');
        % proj. is transposed with respect to the original radiograph!
        s =  x.*costheta(i) + y.*sintheta(i); t = -x.*sintheta(i) + y.*costheta(i); % Goddard&Trepanier
        R_s=Ro-s; u=Ro*t./R_s; u=u+ctrIdx; fu=floor(u(:)); %
        iz=0;
        good=( (fu>0 & fu<nc_p+1) );  lengoodu=length(good);
        for z= slices
            iz=iz+1  ;%z % slice under reconstruction
            v=Ro*z./R_s + nr_p/2;  % coord. correction
            fv=floor(v(:));
           % good=( (fu>0 & fu<=nc_p) & (fv>0 & fv<=nr_p) );
            fu1=fu(good); fv1=fv(good);
            idx = (fu1-1)*szproj(1)+fv1 ; %idx = sub2ind(size(proj),fu, fv);
            temp_1(good) = (u(good)-fu1).*proj1(idx) + (fu1+1-u(good)).*proj(idx);
            proj=[proj(:,(2:end)),proj(:,end)];   proj1=[proj(2:end,:);proj(end,:)];
            temp_2(good) = (u(good)-fu1).*proj1(idx) + (fu1+1-u(good)).*proj(idx);
            R_s1=R_s(:);
            tmp =( (v(good)-fv1) .* temp_2(good)  + (fv1+1-v(good)) .* temp_1(good) ) ./ (R_s1(good).^2);
            tmp1(good)=tmp;  tmp1( lengoodu)=0; tmp2(mask(:))=tmp1;
            vol_tmp(:,iz)=vol_tmp(:,iz)+tmp2(:); % $$$$$$$$$$$$$
        end % loop over z
    end % end loop over theta for bilinear method
    vol_tmp=reshape(vol_tmp',[N,N,length(slices)]) ; % reshape $$$$$$$$$$
    % shows central slice in figure 10
    figure(10), imshow(fliplr(vol_tmp(:,:,cnt_slc)'),[]); % visualize central slice after vol reconstruction
    stringa = [' Bi-LNR bk prj. rad. ' , num2str(i-1)];  title(stringa)
    colormap(gray(256));    display(' done back projection  ! - Bilinear method')
end % closee else linear

% Filtered Back-projection - i.e. from proj pixel (u,v) to voxel (x,y,z)
if strcmp(interp, 'n_n_demo')
    x=x(maskV); y=y(maskV); % mask !

    nw_slices = real(sort(complex(slices))) ; % sort and reordering for faster reconstruction
   % eg. +5 -5 + 6 -6 ...it allows to change the sign of v rather than recalculating v

    ZFlag= abs(nw_slices(2:end)) == nw_slices(1:(end-1)) ; % logical variable
    ZFlag=real([0, ZFlag] ); % true if +z is followed by -z and one can exploit symmetry

    for ipos=1:length(slices) % right positioning the re-orded slices in the volume
        iposz_v(ipos)=find( nw_slices(ipos)== slices) ;  % slice correspondence
    end
    % data type and array allocation statements
    proj=repmat(0,nr_p,nc_p);
    tmp=zeros(nnz(mask),1); tmp2=zeros(N.^2,1); % allocate memory to temporary arrays
   % tmp1
    v0=zeros([size(x)]); v1=v0; u1=v1; % u=u1; v=u;
    good_u=logical(v0); idx=zeros(size(v0)); % allocation

    for i=1:length(theta) % loop over the projections
       % i
        dgri=thetag(i); % theta in degrees

        proj=read_1rad(i,dir,flprefix); % read in the i-th radiograph
        proj = radio2proj(proj,W,H,len); % tranforms radiog. into projection and multiply by W
        %%%
        proj(:,length(H))=0;  % Zero pad projections for
        for ir = 1:nr_p; % filtering row  by row  with row vector H
            proj(ir,:)= real (ifft ( fft(proj(ir,:)).*H ) );  % fast frequency domain filtering
        end
        proj(:,len+1:end) = [] ;   % Truncate the filtered projections
        %%%
        s =  x.*costheta(i) + y.*sintheta(i); t = -x.*sintheta(i) + y.*costheta(i); % Goddard & Trepanier
        % s = x.*costheta(i) + y.*sintheta(i); % also in matlab iradon
        R_s = Ro-s; u=round( Ro*t./(R_s) + ctrIdx) ; % u is the 1st projection pixel coordinate
        good_u = (u>0 & u<(nc_p+1)) ; % only valid pixels -i.e. within the projection frame
        lengoodu=length(good_u);
        u1=u(good_u);%
        inv_R_s_quad=1./(R_s(good_u).^2); Ro_dev_R_s=Ro./(R_s(good_u)); % for generally .* is faster than ./
        iz=0;
        id_1_term=(u1-1)*szproj(1); % does not depend up on v
        for z=nw_slices % for all the (re-ordered) slices
            iz=iz+1;  % z slice under reconstruction
           if ZFlag(iz)==1; % z symmetry (floting point is faster than logical operators!)
          %  if ( iz>=2 & ( abs(nw_slices(iz)) == nw_slices(iz-1) ) ) % z symmetry
                v0=-v0;   % allows calc time saving
            else %  v is the 2nd proj. pixel coord. (u,v)
                v0=Ro_dev_R_s.*z ;  %
            end
            v1=round( v0+nr_p/2 ) ;  % coord. correction
            %v1=v(good_u);  % logical operations i.e. only valid pixels
            idx = id_1_term + v1 ; % idx = sub2ind(szproj,u1, v1);
            tmp = proj(idx); % just in case
            tmp = tmp(:).*inv_R_s_quad; % back-projection ... from pixels to voxels ...!
            tmp1(good_u)=tmp; tmp1(lengoodu)=0;%
            tmp2(maskV)=tmp1; % prepare for image reconst. within the mask
            iposz=iposz_v(iz); % position of the current slice iz in the volume at iposz
            vol_tmp(:,iposz)=vol_tmp(:,iposz)+tmp2; % sum up back-projections without reshaping
            figure(8), imshow(reshape(vol_tmp,[N,N,length(slices)]),[ ]) ;
            title('back-projection in progress');
        end % loop over z
    end % end loop over theta for the nearest neighbout method
    vol_tmp=reshape(vol_tmp,[N,N,length(slices)]) ; % reshape 1D vector into 2D image for each z
    % shows central slice only , in figure 10
    figure(10), imshow(fliplr(vol_tmp(:,:,cnt_slc)'),[ ]); % visualize central slice after vol reconstruction
    stringa = [' NN back-prj. cntrl slice , radiog. degrees: ' , num2str(dgri)];
    title(stringa);   colormap(gray(256));
    display(' done cone beam back projection  ! - nearest neighbour method')
    %***********************************************************
end

vol_tmp= vol_tmp*Ro.^2*pi/(2*length(theta));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%
%%%  Sub-Function:  designFilter
%%%
function filt = designFilter(filter, len, d)
% Returns the Fourier Transform of the filter which will be
% used to filter the projections
%
% INPUT ARGS:   filter - either the string specifying the filter
%               len    - the length of the projections
%               d      - the fraction of frequencies below the nyquist
%                        which we want to pass
%
% OUTPUT ARGS:  filt   - the filter to use on the projections

order = max(64,2^nextpow2(2*len));

% First create a ramp filter - go up to the next highest
% power of 2.

filt = 2*( 0:(order/2) )./order;
w = 2*pi*(0:size(filt,2)-1)/order;   % frequency axis up to Nyquist

switch lower(filter)
    case 'ram-lak'
        % Do nothing
    case 'shepp-logan'
        % be careful not to divide by 0:
        filt(2:end) = filt(2:end) .* (sin(w(2:end)/(2*d))./(w(2:end)/(2*d)));
    case 'cosine'
        filt(2:end) = filt(2:end) .* cos(w(2:end)/(2*d));
    case 'hamming'
        filt(2:end) = filt(2:end) .* (.54 + .46 * cos(w(2:end)/d));
    case 'hann'
        filt(2:end) = filt(2:end) .*(1+cos(w(2:end)./d)) / 2;
    otherwise
        filter
        error('Invalid filter selected.');
end

filt(w>pi*d) = 0;                      % Crop the frequency response
filt = [filt , filt(end-1:-1:2)];    % Symmetry of the filter
return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function W = pix_weight(nr,nc,R,Nofs,Eofs)
% weight for the pixels of the projetions/radiographs
% R is the orbital radius i.e. source to object distance
% off set ... Nofs=Nord off set ; Eofs= East ..
% nr, nc number of rows and columns of W

if nargin <4 % missing off set values
    Nofs=0;Eofs=0; % no off set then
end

if abs(Nofs) > floor(nc/2) | abs(Eofs) > floor(nr/2)
    display(' off set overflow in pix_weight')
end
% grid !
[X,Y]=meshgrid([[floor(nc/2):-1:0],[1:(nc-floor(nc/2)-1)]],...
    [[floor(nr/2):-1:0],[1:(nr-floor(nr/2)-1)]]);
%W=R.* ( (X.^2 + Y.^2 + R^2).^-0.5 );
W=R* ( ((X-Nofs) .^2 + (Y-Eofs).^2 + R^2).^-0.5 );
%figure, imshow(W,[]); title(' pxls weights')
%figure, surf(W); title(' pixel weighting matrix ')
%colormap(jet)
return
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function y = embed(x, mask)
% %function y = embed(x, mask)
% %	embed x in nonzero elements of a logical mask
% %   x is a 1D array and mask is 2D
%
% if max(size(mask)) <= 1
%     y = x; % peculiar
% else
%     good = find(mask(:) ~= 0);
%     np = length(good);
%     if size(x, 1) ~= np
%         error 'bad input size' %
%     end
%     xdim = size(x);
%     y = zeros(prod(size(mask)), prod(xdim(2:end)));
%     y(good,:) = x;
%     y = reshape(y, size(mask), xdim(2:end));
% end
return
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function proj = radio2proj(radio,W,H,len)
% the routine corrects the radiog. for flat and dark images
% weights the radiographs
% log process the radio to generate a projection
global flat , global dark
%
% num=(radio-dark); den=(flat-dark);
% num(num<=0)=1.E-6; den(den<=0)=1.E-4;
% proj=num./den; % correzione per dark & flat
% proj(proj>1)=1; proj(proj<=0)=1E-6;
% proj=-log(proj); % da radio 2 proj
%size(radio)
proj=radio;
proj=proj.*W; % weighting projection
%return
% filtring projection
% proj(:,length(H))=0;  % Zero pad projections
% %proj = fft(proj);    % p holds fft of projections
% for ic = 1:size(proj,1)
%     % filter column by column i.e. row sino by row sino
%     %proj(:,ic) = proj(:,ic).*H(:); % fast frequency domain filtering
%     proj(ic,:)=real (ifft ( fft(proj(ic,:)).*H ) );
% end
% %proj = real(ifft(proj));     % p is the filtered projections
% proj(:,len+1:end) = [];   % Truncate the filtered projections
%figure(6),  imshow(proj,[ ]); title(' proiezione filtrata ')
return


% VL_HOMKERMAP Homogeneous kernel map
%   V = VL_HOMKERMAP(X, N) computes a 2*N+1 dimensional approximated
%   kernel map for the Chi2 kernel. X is an array of data points. Each
%   point is expanded into a vector of dimension 2*N+1 and saved to
%   the output V. The expanded feature vectors are stacked along the
%   first dimension, so that the output array V has the same
%   dimensions of the input array X except for the first one, which is
%   2*N+1 times larger.
%
%   The function accepts the following options:
%
%   Kernel:: KCHI2
%     One of KCHI2 (Chi2 kernel), KINTERS (intersection kernel), KJS
%     (Jensen-Shannon kernel). The 'Kernel' option name can be omitted,
%     i.e. VL_HOMKERMAP(..., 'kernel', 'kchi2') has the same effect of
%     VL_HOMKERMAP(..., 'kchi2').
%
%   Period:: [automatically tuned]
%     Set the period of the kernel specturm. The approximation is
%     based on periodicizing the kernel specturm. If not specified,
%     the period is automatically set based on the heuristic described
%     in [2].
%
%   Window:: [RECTANGULAR]
%     Set the window used to truncate the spectrum before The window
%     can be either RECTANGULAR or UNIFORM window. See [2] and the API
%     documentation for details.
%
%   Gamma:: [1]
%     Set the homogeneity degree of the kernel. The standard kernels
%     are 1-homogeneous, but sometimes smaller values perform better
%     in applications. See [2] for details.
%
%   Example::
%     The following code results in approximatively the same
%     similarities matrices between points X and Y:
%
%       x = rand(10,1) ;
%       y = rand(10,100) ;
%       psix = vl_homkermap(x, 3) ;
%       psiy = vl_homkermap(y, 3) ;
%       figure(1) ; clf ;
%       ker = vl_alldist(x, y, 'kchi2') ;
%       ker_ = psix' * psiy ;
%       plot([ker ; ker_]') ;
%
%   Note::
%     The homogeneous kernels K(X,Y) are normally defined for
%     non-negative data only. VL_HOMKERMAP defines them for both
%     positive and negative data by using the definition
%     SIGN(X)SIGN(Y)K(ABS(X),ABS(Y)) -- note that other extensions are
%     possible as well (see [2]).
%
%   REFERENCES::
%     [1] A. Vedaldi and A. Zisserman
%     `Efficient Additive Kernels via Explicit Feature Maps',
%     Proc. CVPR, 2010.
%
%     [2] A. Vedaldi and A. Zisserman
%     `Efficient Additive Kernels via Explicit Feature Maps',
%     PAMI, 2011 (submitted).
%
%   See also: VL_HELP().

% Authors: Andrea Vedaldi

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).


function fem2d_pack_test09 ( )

%*****************************************************************************80
%
%% TEST09 demonstrates ELEMENTS_EPS with T4 elements.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  element_order = 4;
  nelemx = 7;
  nelemy = 5;

  element_num = grid_t4_element_num ( nelemx, nelemy );
  node_num = grid_t4_node_num ( nelemx, nelemy );

  code = 'T4';
  file_name = 'fem2d_pack_test_t4.eps';
  title = 'Grid of T4 Elements';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST09\n' );
  fprintf ( 1, '  ELEMENTS_EPS creates an Encapsulated PostScript\n' );
  fprintf ( 1, '  file containing an image of a T4 mesh.\n' );

  element_node = grid_t4_element ( nelemx, nelemy );

  node = 0;

  for row = 0 : nelemy

    y = ( ( 3*nelemy - row ) *  0.0   ...
        + (          + row ) *  6.0 ) ...
        / ( 3*nelemy       );

    for col = 0 : nelemx

      node = node + 1;

      x = ( ( 2*nelemx - col ) * 0.0   ...
          + (          + col ) * 6.0 ) ...
          / ( 2*nelemx       );

      node_x(node) = x;
      node_y(node) = y;

    end
%
%  Skip over the two rows of interior nodes.
%
    node = node + nelemx;
    node = node + nelemx;

  end
%
%  The coordinates of interior nodes are the average of the vertices.
%
  for element = 1 : element_num
    node = element_node(4,element);
    node_x(node) = sum ( node_x(element_node(1:3,element)) ) / 3.0;
    node_y(node) = sum ( node_y(element_node(1:3,element)) ) / 3.0;
  end

  for element = 1 : element_num
    element_mask(element) = 1;
  end

  elements_eps ( file_name, node_num, node_x, node_y, code, ...
    element_order, element_num, element_mask, element_node, title );

  return
end


function average = pointSetsAverage(pointSets, varargin)
%POINTSETSAVERAGE Compute the average of several point sets
%
%   AVERAGESET = pointSetsAverage(POINTSETS)
%   POINTSETS is a cell array containing several liste of points with the
%   same number of points. The function compute the average coordinate of
%   each vertex, and return the resulting average point set.
%
%   Example
%   pointSetsAverage
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-04-01,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

% check input
if ~iscell(pointSets)
    error('First argument must be a cell array');
end

% number of sets
nSets   = length(pointSets);

% get reference size of coordinates array
set1    = pointSets{1};
refSize = size(set1);

% allocate memory for result
average = zeros(refSize);

% iterate on point sets
for i = 1:nSets
    % get current point set, and check its size
    set = pointSets{i};
    if sum(size(set) ~= refSize) > 0
        error('All point sets must have the same size');
    end

    % cumulative sum of coordinates
    average = average + set;
end

% normalize by the number of sets
average = average / nSets;


function a = wathen ( nx, ny, n )

%*****************************************************************************80
%
%% WATHEN returns a finite element matrix.
%
%  Discussion:
%
%    The Wathen matrix is a finite element matrix which is sparse.
%
%    The entries of the matrix depend in part on a physical quantity
%    related to density.  That density is here assigned random values between
%    0 and 100.
%
%    The matrix order N is determined by the input quantities NX and NY,
%    which would usually be the number of elements in the X and Y directions.
%    The value of N is
%
%      N = 3*NX*NY + 2*NX + 2*NY + 1,
%
%    and sufficient storage in A must have been set aside to hold
%    the matrix.
%
%    A is the consistent mass matrix for a regular NX by NY grid
%    of 8 node serendipity elements.
%
%    Here is an illustration for NX = 3, NY = 2:
%
%     23-24-25-26-27-28-29
%      |     |     |     |
%     19    20    21    22
%      |     |     |     |
%     12-13-14-15-16-17-18
%      |     |     |     |
%      8     9    10    11
%      |     |     |     |
%      1--2--3--4--5--6--7
%
%    For this example, the total number of nodes is, as expected,
%
%      N = 3 * 3 * 2 + 2 * 2 + 2 * 3 + 1 = 29
%
%  Properties:
%
%    A is symmetric positive definite for any positive values of the
%    density RHO(NX,NY), which is here given the value 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Nicholas Higham,
%    Algorithm 694: A Collection of Test Matrices in MATLAB,
%    ACM Transactions on Mathematical Software,
%    Volume 17, Number 3, September 1991, pages 289-305.
%
%    Andrew Wathen,
%    Realistic eigenvalue bounds for the Galerkin mass matrix,
%    IMA Journal of Numerical Analysis,
%    Volume 7, 1987, pages 449-457.
%
%  Parameters:
%
%    Input, integer NX, NY, values which determine the size of A.
%
%    Input, integer N, the order of N, as determined by NX and NY.
%
%    Output, real A(N,N), the matrix.
%
  em =  [ ...
     6.0, -6.0,  2.0, -8.0,  3.0, -8.0,  2.0, -6.0; ...
    -6.0, 32.0, -6.0, 20.0, -8.0, 16.0, -8.0, 20.0; ...
     2.0, -6.0,  6.0, -6.0,  2.0, -8.0,  3.0, -8.0; ...
    -8.0, 20.0, -6.0, 32.0, -6.0, 20.0, -8.0, 16.0; ...
     3.0, -8.0,  2.0, -6.0,  6.0, -6.0,  2.0, -8.0; ...
    -8.0, 16.0, -8.0, 20.0, -6.0, 32.0, -6.0, 20.0; ...
     2.0, -8.0,  3.0, -8.0,  2.0, -6.0,  6.0, -6.0; ...
    -6.0, 20.0, -8.0, 16.0, -8.0, 20.0, -6.0, 32.0 ];

  a(1:n,1:n) = 0.0;

  for j = 1 : ny

    for i = 1 : nx
%
%  For the element (I,J), determine the indices of the 8 nodes.
%
      node(1) = 3 * j * nx + 2 * j + 2 * i + 1;
      node(2) = node(1) - 1;
      node(3) = node(1) - 2;

      node(4) = ( 3 * j - 1 ) * nx + 2 * j + i - 1;
      node(8) = node(4) + 1;

      node(5) = ( 3 * j - 3 ) * nx + 2 * j + 2 * i - 3;
      node(6) = node(5) + 1;
      node(7) = node(5) + 2;
%
%  The density RHO can also be set to a random positive value.
%
      for krow = 1 : 8
        for kcol = 1 : 8

          if ( node(krow) < 1 || n < node(krow) )
            fprintf ( 1, '\n' );
            fprintf ( 1, 'WATHEN - Fatal error!\n' );
            fprintf ( 1, '  Index NODE(KROW) out of bounds.\n' );
            fprintf ( 1, '  I = %d\n', i );
            fprintf ( 1, '  J = %d\n', j );
            fprintf ( 1, '  KROW = %d\n', krow );
            fprintf ( 1, '  NODE(KROW) = %d\n', node(krow) );
            error ( 'WATHEN - Fatal error!' );
          elseif ( node(kcol) < 1 || n < node(kcol) )
            fprintf ( 1, '\n' );
            fprintf ( 1, 'WATHEN - Fatal error!\n' );
            fprintf ( 1, '  Index NODE(KCOL) out of bounds.\n' );
            fprintf ( 1, '  I = %d\n', i );
            fprintf ( 1, '  J = %d\n', j );
            fprintf ( 1, '  KCOL = %d\n', kcol );
            fprintf ( 1, '  NODE(KCOL) = %d\n', node(kcol) );
            error ( 'WATHEN - Fatal error!' );
          end

          rho = 1.0;

          a(node(krow),node(kcol)) = a(node(krow),node(kcol)) ...
            + 20.0 * rho * em(krow,kcol) / 9.0;

        end
      end

    end
  end

  return
end


function y = interpftn(x, sz, win)
%INTERPFTN Resample data using Fourier interpolation.
%
% DESCRIPTION:
%       interpftn resamples an N-D matrix to the size given in sz using
%       Fourier interpolation.
%
% USAGE:
%       y = interpftn(x, sz)
%       y = interpftn(x, sz, win)
%
% INPUTS:
%       x       - matrix to interpolate
%       sz      - new size
%
% OPTIONAL INPUTS:
%       win     - name of windowing function to use
%
% OUTPUTS:
%       y       - resampled matrix
%
% ABOUT:
%       author      - Bradley Treeby
%       date        - 20th October 2010
%       last update - 20th December 2011
%
% This function is part of the k-Wave Toolbox (http://www.k-wave.org)
% Copyright (C) 2009-2014 Bradley Treeby and Ben Cox
%
% See also interpft

% This file is part of k-Wave. k-Wave is free software: you can
% redistribute it and/or modify it under the terms of the GNU Lesser
% General Public License as published by the Free Software Foundation,
% either version 3 of the License, or (at your option) any later version.
%
% k-Wave 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 Lesser General Public License for
% more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with k-Wave. If not, see <http://www.gnu.org/licenses/>.

% check inputs for window
if nargin == 2
    win = 0;
end

% extract the size of the input matrix
x_sz = size(x);

% check enough coefficients have been given
if sum(x_sz ~= 1) ~= numel(sz)
    error('the number of scaling coefficients must equal the number of dimensions in x');
end

% compute the interpolation over each matrix dimension using the inbuilt
% interpft (dimensions with no interpolation required are skipped)
y = x;
for p = 1:numel(sz)
    if sz(p) ~= x_sz(p)
        y = interpft(y, sz(p), p, win);
    end
end

function y = interpft(x, ny, dim, win)
%INTERPFT 1-D interpolation using FFT method.
%   Y = INTERPFT(X,N) returns a vector Y of length N obtained
%   by interpolation in the Fourier transform of X.
%
%   If X is a matrix, interpolation is done on each column.
%   If X is an array, interpolation is performed along the first
%   non-singleton dimension.
%
%   INTERPFT(X,N,DIM) performs the interpolation along the
%   dimension DIM.
%
%   Assume x(t) is a periodic function of t with period p, sampled
%   at equally spaced points, X(i) = x(T(i)) where T(i) = (i-1)*p/M,
%   i = 1:M, M = length(X).  Then y(t) is another periodic function
%   with the same period and Y(j) = y(T(j)) where T(j) = (j-1)*p/N,
%   j = 1:N, N = length(Y).  If N is an integer multiple of M,
%   then Y(1:N/M:N) = X.
%
%   Example:
%      % Set up a triangle-like signal signal to be interpolated
%      y  = [0:.5:2 1.5:-.5:-2 -1.5:.5:0]; % equally spaced
%      factor = 5; % Interpolate by a factor of 5
%      m  = length(y)*factor;
%      x  = 1:factor:m;
%      xi = 1:m;
%      yi = interpft(y,m);
%      plot(x,y,'o',xi,yi,'*')
%      legend('Original data','Interpolated data')
%
%   Class support for data input x:
%      float: double, single
%
%   See also INTERP1.

%   Robert Piche, Tampere University of Technology, 10/93.
%   Copyright 1984-2006 The MathWorks, Inc.
%   $Revision: 5.15.4.4 $  $Date: 2006/12/15 19:27:56 $

% y = zeros(ny, 1);
% y(1:length(x)) = fftshift(fft(x));
% y = real(ifft(ifftshift(y)));

% Bug fix, additional documentation, and win option
% author        - Bradley Treeby
% date          - 20th October 2010
% last update   - 1st April 2011

% if nargin==2,
%
%     % this forces x to be a column vector, nshifts is so it can be changed
%     % back if necessary
%     [x, nshifts] = shiftdim(x);
%
%     % check if x is a scalar, and return a row vector if so
%     if isscalar(x)
%       nshifts = 1;
%     end
%
% elseif nargin==3,
%
%     % if the dimension to take the fft over is given, rearrange the matrix
%     % so this dimension is the first dimension
%     perm = [dim:max(length(size(x)),dim) 1:dim-1];
%     x = permute(x,perm);
% end

% rearrange the matrix so the fft is taken over the first dimension
perm = [dim:max(length(size(x)),dim) 1:dim-1];
x = permute(x,perm);

siz = size(x);
[m, n] = size(x);
if ~isscalar(ny)
  error('MATLAB:interpft:NonScalarN', 'N must be a scalar.');
end

% if necessary, increase ny by an integer multiple to make ny > m
% here m is the length of the dimension over which the fft is being taken,
% and ny is the number of points to use including zero padding
if ny > m
   incr = 1;
else
    % if ny is zero, return nothing
    if ny == 0
       y = [];
       return
    end
    % otherwise make ny bigger to allow downsampling from integer points
    incr = floor(m/ny) + 1;
    ny = incr*ny;
end

% take the fft of the input function
a = fft(x,[],1);

% calculate the nyquist frequency
nyqst = ceil((m+1)/2);

% window the Fourier coefficient if win is given
if win
    % create window using getWin
    wind = getWin(siz(1), win);

    % shift the window
    wind = circshift(wind, nyqst);

    % create repmat variable
    siz_rep = siz;
    siz_rep(1) = 1;

    % repeat
    wind = repmat(wind, siz_rep);

    % apply the window
    a = a.*wind;

end

% zero pad with the zeros in the middle
b = [a(1:nyqst,:) ; zeros(ny-m, prod(siz(2:end))) ; a(nyqst+1:m,:)];

if rem(m, 2) == 0
    % if the sequence has an even number of points, make the sequence
    % symmetric, e.g., turn
    % EP P P N   0 0 0 0 0 0   P P
    % into
    % EP P P N/2 0 0 0 0 0 N/2 P P
    % where EP = end point P = points, N = nyquist point
    b(nyqst,:) = b(nyqst,:)/2;
    b(nyqst+ny-m,:) = b(nyqst,:);
end

% take the inverse FFT
y = ifft(b,[],1);

% if the input was real, throw away any residual complex bits
if isreal(x)
    y = real(y);
end

% make sure the amplitudes are correct by scaling by the new length
y = y * ny / m;

% this gets a downsampled version from an upsampled version
y = y(1:incr:ny,:);  % Skip over extra points when oldny <= m.

% reshape
[y_length, num_signals] = size(y);
y = reshape(y, [y_length, siz(2:end)]);

% sort out the resizing
y = ipermute(y,perm);

% if nargin==2,
%   y = reshape(y,[ones(1,nshifts) size(y,1) siz(2:end)]);
% elseif nargin==3,
%   y = ipermute(y,perm);
% end


function [node,elem,u] = ellipsoidPoisson

maxIt = 5;
err = zeros(maxIt,1);
% ----------------------- generate initial mesh ---------------
node = [1,0,0; 0,1,0; -1,0,0; 0,-1,0; 0,0,1; 0,0,-1];
elem = [6,1,2; 6,2,3; 6,3,4; 6,4,1; 5,1,4; 5,3,4; 5,3,2; 5,2,1];
% node = [1,0,0; 0,1,0; -1,0,0; 0,-1,0; 0,0,1];
% elem = [5,1,4; 5,3,4; 5,3,2; 5,2,1];
showmesh(node,elem)
for i=1:3
    [node,elem] = uniformrefine(node,elem);
    %[node,elem] = uniformbisect(node,elem);
end
% project the mesh to the ellipsoid
r = sqrt(node(:,1).^2+node(:,2).^2+node(:,3).^2);
node = node./[r r r];
node(:,1) = 3*node(:,1); node(:,2) = 2*node(:,2);
showmesh(node,elem);

for k=1:maxIt
%    [u,A] = surfacePoisson(node,elem,[],@f,@g_D,[]);
    [u,A] = surfacePoisson(node,elem,[],@f,[],[]);
    uI = exactu(node);
    err(k) = sqrt(u-uI)'*A*(u-uI);
    [node,elem] = uniformbisect(node,elem);
    r = sqrt(node(:,1).^2+node(:,2).^2+node(:,3).^2);
    node = node./[r r r];
    node(:,1) = 3*node(:,1); node(:,2) = 2*node(:,2);
end
plot(err)
end

%---------------- Data of PDE-----------------------------
function z = f(p) % load data (right hand side function)
z = 2*p(:,1);
end
%--------------------------------------------------------------------------
function z = g_D(p) % Dirichlet boundary condition
z = exactu(p);
end
%--------------------------------------------------------------------------
function z = exactu(p)   % exact solution
z = p(:,1);
end
%--------------------------------------------------------------------------


function currentScaleFactor = estimate_scale( im_gray, pos, currentScaleFactor)

global para

% extract the test sample feature map for the scale filter
xs = get_scale_sample(im_gray, pos, para.app_sz, para.scaleFactors*currentScaleFactor, para.scale_window, para.scale_model_sz);

% calculate the correlation response of the scale filter
xsf = fft(xs,[],2);
scale_response = real(ifft(sum(para.sf_num .* xsf, 1) ./ (para.sf_den + para.lambda)));

% find the maximum scale response
recovered_scale = find(scale_response == max(scale_response(:)), 1);

currentScaleFactor = currentScaleFactor*para.scaleFactors(recovered_scale);
if currentScaleFactor < para.min_scale_factor
    currentScaleFactor = para.min_scale_factor;
elseif currentScaleFactor > para.max_scale_factor
    currentScaleFactor = para.max_scale_factor;
end

% update the scale model
%===========================
% extract the training sample feature map for the scale filter
xs = get_scale_sample(im_gray, pos, para.app_sz, currentScaleFactor * para.scaleFactors, para.scale_window, para.scale_model_sz);

% calculate the scale filter update
xsf = fft(xs,[],2);
new_sf_num = bsxfun(@times, para.ysf, conj(xsf));
new_sf_den = sum(xsf .* conj(xsf), 1);

para.sf_den = (1 - para.interp_factor) * para.sf_den + para.interp_factor * new_sf_den;
para.sf_num = (1 - para.interp_factor) * para.sf_num + para.interp_factor * new_sf_num;

end


% OFDM_basic.m
clear all
NgType=1; % NgType=1/2 for cyclic prefix/zero padding
if NgType==1
    nt='CP';
elseif NgType==2
    nt='ZP';
end
Ch=0;  % Ch=0/1 for AWGN/multipath channel
if Ch==0
    chType='AWGN';
    Target_neb=100;
else
    chType='CH';
    Target_neb=500;
end
figure(Ch+1), clf
PowerdB=[0 -8 -17 -21 -25]; % Channel tap power profile 'dB'
Delay=[0 3 5 6 8];          % Channel delay 'sample'
Power=10.^(PowerdB/10);     % Channel tap power profile 'linear scale'
Ntap=length(PowerdB);       % Chanel tap number
Lch=Delay(end)+1;           % Channel length
Nbps=4;
M=2^Nbps;                   % Modulation order=2/4/6 for QPSK/16QAM/64QAM
Nfft=64;                    % FFT size
Ng=Nfft/4;                  % Guard interval length
Nsym=Nfft+Ng;               % Symbol duration
Nvc=Nfft/4;                 % Nvc=0: no virtual carrier
Nused=Nfft-Nvc;
EbN0=[0:5:20];    % EbN0
N_iter=1e5;       % Number of iterations for each EbN0
Nframe=3;         % Number of symbols per frame
sigPow=0;         % Signal power initialization
file_name=['OFDM_BER_' chType '_' nt '_' 'GL' num2str(Ng) '.dat'];
fid=fopen(file_name, 'w+');
norms=[1 sqrt(2) 0 sqrt(10) 0 sqrt(42)];     % BPSK 4-QAM 16-QAM
for i=0:length(EbN0)
   randn('state',0);
   rand('state',0);
   %Ber2=ber(); % BER initialization
   Neb=0;
   Ntb=0; % Initialize the number of error/total bits
   for m=1:N_iter
      % Tx______________________________________________________________
      X= randint(1,Nused*Nframe,M); % bit: integer vector
      Xmod= qammod(X,M,0,'gray')/norms(Nbps);
      if NgType~=2
          x_GI=zeros(1,Nframe*Nsym);
       elseif NgType==2
           x_GI= zeros(1,Nframe*Nsym+Ng);
        % Extend an OFDM symbol by Ng zeros
      end
      kk1=[1:Nused/2];
      kk2=[Nused/2+1:Nused];
      kk3=1:Nfft;
      kk4=1:Nsym;
      for k=1:Nframe
         if Nvc~=0
             X_shift= [0 Xmod(kk2) zeros(1,Nvc-1) Xmod(kk1)];
         else
             X_shift= [Xmod(kk2) Xmod(kk1)];
         end
         x= ifft(X_shift);
         x_GI(kk4)= guard_interval(Ng,Nfft,NgType,x);
         kk1=kk1+Nused;
         kk2=kk2+Nused;
         kk3=kk3+Nfft;
         kk4=kk4+Nsym;
      end
      if Ch==0
          y= x_GI;  % No channel
      else  % Multipath fading channel
        channel=(randn(1,Ntap)+j*randn(1,Ntap)).*sqrt(Power/2);
        h=zeros(1,Lch);
        h(Delay+1)=channel; % cir: channel impulse response
        y = conv(x_GI,h);
      end
      if i==0 % Only to measure the signal power for adding AWGN noise
        y1=y(1:Nframe*Nsym);
        sigPow = sigPow + y1*y1';
        continue;
      end
      % Add AWGN noise________________________________________________
      snr = EbN0(i)+10*log10(Nbps*(Nused/Nfft)); % SNR vs. Eb/N0
      noise_mag = sqrt((10.^(-snr/10))*sigPow/2);
      y_GI = y + noise_mag*(randn(size(y))+j*randn(size(y)));
      % Rx_____________________________________________________________
      kk1=(NgType==2)*Ng+[1:Nsym];
      kk2=1:Nfft;
      kk3=1:Nused;
      kk4=Nused/2+Nvc+1:Nfft;
      kk5=(Nvc~=0)+[1:Nused/2];
      if Ch==1
         H= fft([h zeros(1,Nfft-Lch)]); % Channel frequency response
         H_shift(kk3)= [H(kk4) H(kk5)];
      end
      for k=1:Nframe
         Y(kk2)= fft(remove_GI(Ng,Nsym,NgType,y_GI(kk1)));
         Y_shift=[Y(kk4) Y(kk5)];
         if Ch==0
             Xmod_r(kk3) = Y_shift;
         else
             Xmod_r(kk3)= Y_shift./H_shift;  % Equalizer - channel compensation
         end
         kk1=kk1+Nsym;
         kk2=kk2+Nfft;
         kk3=kk3+Nused;
         kk4=kk4+Nfft;
         kk5=kk5+Nfft;
      end
      X_r=qamdemod(Xmod_r*norms(Nbps),M,0,'gray');
      Neb=Neb+sum(sum(de2bi(X_r,Nbps)~=de2bi(X,Nbps)));
      Ntb=Ntb+Nused*Nframe*Nbps;  %[Ber,Neb,Ntb]=ber(bit_Rx,bit,Nbps);
      if Neb>Target_neb
          break;
      end
   end
   if i==0
     sigPow= sigPow/Nsym/Nframe/N_iter;
     fprintf('Signal power= %11.3e\n', sigPow);
     fprintf(fid,'%%Signal power= %11.3e\n%%EbN0[dB]       BER\n', sigPow);
    else
     Ber = Neb/Ntb;
     fprintf('EbN0=%3d[dB], BER=%4d/%8d =%11.3e\n', EbN0(i), Neb,Ntb,Ber)
     fprintf(fid, '%d\t%11.3e\n', EbN0(i), Ber);
     if Ber<1e-6
         break;
     end
   end
end
if (fid~=0)
    fclose(fid);
end
disp('Simulation is finished');
plot_ber(file_name,Nbps);


% MAIN - Point Mass
%
% Finds the optimal trajectory to slide a point-mass across a 1d
% frictionless plane, using a variety of cost functions.
%
% Simple force-squared cost function  --  This is easy to optimize
%

clc; clear;
addpath ../../

% User-defined dynamics and objective functions
problem.func.dynamics = @(t,x,u)( dynamics(x,u) );
problem.func.pathObj = @(t,x,u)( obj_forceSquared(u) );

% Problem bounds
problem.bounds.initialTime.low = 0;
problem.bounds.initialTime.upp = 0;
problem.bounds.finalTime.low = 1.0;
problem.bounds.finalTime.upp = 1.0;

problem.bounds.state.low = [0; -inf];
problem.bounds.state.upp = [1; inf];
problem.bounds.initialState.low = [0;0];
problem.bounds.initialState.upp = [0;0];
problem.bounds.finalState.low = [1;0];
problem.bounds.finalState.upp = [1;0];

problem.bounds.control.low = -50; %-inf;
problem.bounds.control.upp = 50; %inf;

% Guess at the initial trajectory
problem.guess.time = [0,1];
problem.guess.state = [0, 0; 1, 0];
problem.guess.control = [1, -1];

% Options for fmincon
problem.options.nlpOpt = optimset(...
    'Display','iter',...
    'GradObj','on',...
    'GradConstr','on',...
    'DerivativeCheck','off');   %Fmincon automatically checks

problem.options.method = 'trapezoid';
% problem.options.method = 'rungeKutta';

% Solve the problem
soln = optimTraj(problem);
t = soln.grid.time;
q = soln.grid.state(1,:);
dq = soln.grid.state(2,:);
u = soln.grid.control;

% Plot the solution:
figure(1); clf;

subplot(3,1,1)
plot(t,q)
ylabel('pos')
title('Move Point Mass');

subplot(3,1,2)
plot(t,dq)
ylabel('vel')

subplot(3,1,3)
plot(t,u)
ylabel('force')