%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtDomRegularize3D.m                          |
%|    #    |   VERSION    : 0.50                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 25.11.2018                                    |
%| ( === ) |   SYNOPSIS   : Singular kernel regularization (in debug mode)|
%|  `---'  |                                                              |
%+========================================================================+

% Cleaning
clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Parameters
N   = 1e2;
gss = 3;

% Spherical mesh
sphere = mshSphere(N,1);

% Square mesh
square = mshSquare(2*N,[3 3]);

% Graphical representation
figure
plot(sphere)
hold on
plot(square)
axis equal

% Domain
sigma = dom(sphere,gss);

% Finite elements
u = fem(sphere,'P0');
v = fem(sphere,'P1');
w = fem(sphere,'RWG');

% Gren kernel
Gxy   = @(X,Y) femGreenKernel(X,Y,'[1/r]',[]);
dyGxy = @(X,Y) femGreenKernel(X,Y,'grady[1/r]1',[]);

% Single radiation P0
M  = integral(square.vtx,sigma,Gxy,u);
Mr = regularize(square.vtx,sigma,'[1/r]',u);
norm(M+Mr,'inf')

% Single radiation P1
M  = integral(square.vtx,sigma,Gxy,v);
Mr = regularize(square.vtx,sigma,'[1/r]',v);
norm(M+Mr,'inf')

% Single radiation RWG
M  = integral(square.vtx,sigma,Gxy,div(w));
Mr = regularize(square.vtx,sigma,'[1/r]',div(w));
norm(M+Mr,'inf')

% Single layer P0
M  = integral(sigma,sigma,u,Gxy,u);
Mr = regularize(sigma,sigma,u,'[1/r]',u);
norm(M+Mr,'inf')

% Single layer P1
M  = integral(sigma,sigma,v,Gxy,v);
Mr = regularize(sigma,sigma,v,'[1/r]',v);
norm(M+Mr,'inf')

% Single layer RWG
M  = integral(sigma,sigma,div(w),Gxy,div(w));
Mr = regularize(sigma,sigma,div(w),'[1/r]',div(w));
norm(M+Mr,'inf')

% Double radiation P0
M  = integral(square.vtx,sigma,dyGxy,u);
Mr = regularize(square.vtx,sigma,'grady[1/r]1',u);
norm(M+Mr,'inf')

% Double layer P0
M   = integral(sigma,sigma,u,dyGxy,u);
Mr  = regularize(sigma,sigma,u,'grady[1/r]1',u);
norm(M+Mr,'inf')

% Stokeslet P0
for i = 1:3
    for j = 1:3
        name  = ['[ij/r+rirj/r^3]',num2str(i),num2str(j)];
        green = @(X,Y) femGreenKernel(X,Y,name,[]);
        M     = integral(sigma,sigma,u,green,u);
        Mr    = regularize(sigma,sigma,u,name,u);
        norm(M+Mr,'inf')
    end
end

disp('~~> Michto gypsilab !')


function [xUpdate, SUpdate,innov,Szz,W]=sqrtKalmanUpdateWithPred(z,SR,zPred,otherInfo)
%%SQRTKALMANUPDATEWITHPRED Given the output of the measurement prediction
%           step from sqrtKalmanMeasPred and a measurement, complete the
%           measurement update step of the square-root Kalman filter.
%           Separating the measurement prediction step from the rest of the
%           update step can make the creation of multiple measurement
%           association hypotheses from a single target prediction more
%           efficient. The full measurement update function is
%           sqrtKalmanUpdate.
%
%INPUTS: z The zDimX1 measurement vector.
%       SR The zDimXzDim lower-triangular square root of the measurement
%          covariance matrix in the native coordinate system of the
%          measurement.
%    zPred The zDimXnumComp measurement predictions from the filter.
%   PzPred The zDimXzDimXnumComp covariance matrices associated with zPred.
% otherInfo The intermediate results returned in the otherInfo output of
%          the sqrtKalmanMeasPred function.
%
%OUTPUTS: xUpdate The xDimXnumComp updated state vectors.
%         SUpdate The updated xDimXxDimXnumComp lower-triangular square-
%                 root state covariance matrices.
%      innov, Szz The zDimXnumComp innovations and the zDimXzDimXnumComp
%                 square-root innovation covariance matrix are returned in
%                 case one wishes to analyze the consistency of the
%                 estimator or use those values in gating or likelihood
%                 evaluation.
%               W The xDimXzDimXnumComp gain used in the update. This can
%                 be useful when gating and using the function
%                 calcMissedGateCov.
%
%See the comments to the function sqrtKalmanMeasPred for an example of
%usage of this function. See the comments to sqrtKalmanUpdate for more
%information on the algorithm.
%
%June 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xPred=otherInfo.xPred;
SPred=otherInfo.SPred;
Pxz=otherInfo.Pxz;
H=otherInfo.H;

xDim=size(xPred,1);
numComp=size(xPred,2);
zDim=size(z,1);

xUpdate=zeros(xDim,numComp);
SUpdate=zeros(xDim,xDim,numComp);
innov=zeros(zDim,numComp);
Szz=zeros(zDim,zDim,numComp);
W=zeros(xDim,zDim,numComp);

for k=1:numComp
    Szz(:,:,k)=tria([H*SPred(:,:,k),SR]);
    W=(Pxz(:,:,k)/Szz(:,:,k)')/Szz(:,:,k);
    innov(:,k)=z-zPred(:,k);
    xUpdate(:,k)=xPred(:,k)+W(:,:,k)*innov(:,k);
    temp=W(:,:,k)*H;
    SUpdate(:,:,k)=tria([(eye(size(temp))-temp)*SPred,W(:,:,k)*SR]);
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.


function rinexe(ephemerisfile, outputfile)
%RINEXE Reads a RINEX Navigation Message file version 3.03 and
%	        reformats the data into a matrix with 21 rows and a column
%           for each satellite.  The matrix is stored in outputfile

%Typical call: rinexe('pta.96n','pta.nav')

%Kai Borre 04-18-96
%Copyright (c) by Kai Borre
%$Revision: 1.0 $  $Date: 1997/09/24  $
% Revision September 11, 2015 at Samara

% Units are either seconds, meters, or radians
fide = fopen(ephemerisfile);
head_lines = 0;
while 1  % We skip header
   head_lines = head_lines+1;
   line = fgetl(fide);
   answer = findstr(line,'END OF HEADER');
   if ~isempty(answer), break;	end;
end;
head_lines;
noeph = -1;
while 1
   noeph = noeph+1;
   line = fgetl(fide);
   if line == -1, break;  end
end;
noeph = noeph/8
frewind(fide);
for i = 1:head_lines, line = fgetl(fide); end;

% Set aside memory for the input
svprn	 = zeros(1,noeph);
weekno	 = zeros(1,noeph);
t0c	 = zeros(1,noeph);
tgd	 = zeros(1,noeph);
aodc	 = zeros(1,noeph);
toe	 = zeros(1,noeph);
af2	 = zeros(1,noeph);
af1	 = zeros(1,noeph);
af0	 = zeros(1,noeph);
aode	 = zeros(1,noeph);
deltan	 = zeros(1,noeph);
M0	 = zeros(1,noeph);
ecc	 = zeros(1,noeph);
roota	 = zeros(1,noeph);
toe	 = zeros(1,noeph);
cic	 = zeros(1,noeph);
crc	 = zeros(1,noeph);
cis	 = zeros(1,noeph);
crs	 = zeros(1,noeph);
cuc	 = zeros(1,noeph);
cus	 = zeros(1,noeph);
Omega0	 = zeros(1,noeph);
omega	 = zeros(1,noeph);
i0	 = zeros(1,noeph);
Omegadot = zeros(1,noeph);
idot	 = zeros(1,noeph);
accuracy = zeros(1,noeph);
health	 = zeros(1,noeph);
fit	 = zeros(1,noeph);

for i = 1:noeph
   line = fgetl(fide);	  %
   svprn(i) = str2num(line(2:3)); % a G for GPS in front is omitted
   year = line(5:8);
   month = line(10:11);
   day = line(13:14);
   hour = line(16:17);
   minute = line(19:20);
   second = line(22:23);
   af0(i) = str2num(line(24:42));
   af1(i) = str2num(line(43:61));
   af2(i) = str2num(line(62:80));
   line = fgetl(fide);	  %
   IODE = line(5:23);
   crs(i) = str2num(line(24:42));
   deltan(i) = str2num(line(43:61));
   M0(i) = str2num(line(62:80));
   line = fgetl(fide) ;	  %
   cuc(i) = str2num(line(5:23));
   ecc(i) = str2num(line(24:42));
   cus(i) = str2num(line(43:61));
   roota(i) = str2num(line(62:80));
   line=fgetl(fide);
   toe(i) = str2num(line(5:23));
   cic(i) = str2num(line(24:42));
   Omega0(i) = str2num(line(43:61));
   cis(i) = str2num(line(62:80));
   line = fgetl(fide);	    %
   i0(i) =  str2num(line(5:23));
   crc(i) = str2num(line(24:42));
   omega(i) = str2num(line(43:61));
   Omegadot(i) = str2num(line(62:80));
   line = fgetl(fide);	    %
   idot(i) = str2num(line(5:23));
   codes = str2num(line(24:42));
   weekno = str2num(line(43:61));
   L2flag = str2num(line(62:80));
   line = fgetl(fide);	    %
   svaccur = str2num(line(5:23));
   svhealth = str2num(line(24:42));
   tgd(i) = str2num(line(43:61));
   iodc = line(62:80);
   line = fgetl(fide);	    %
   tom(i) = str2num(line(5:23));
   spare = line(24:42);
%   spare = line(43:61);
%   spare = line(62:80);
end
status = fclose(fide);

%  Description of variable eph.
eph(1,:)  = svprn;
eph(2,:)  = af2;
eph(3,:)  = M0;
eph(4,:)  = roota;
eph(5,:)  = deltan;
eph(6,:)  = ecc;
eph(7,:)  = omega;
eph(8,:)  = cuc;
eph(9,:)  = cus;
eph(10,:) = crc;
eph(11,:) = crs;
eph(12,:) = i0;
eph(13,:) = idot;
eph(14,:) = cic;
eph(15,:) = cis;
eph(16,:) = Omega0;
eph(17,:) = Omegadot;
eph(18,:) = toe;
eph(19,:) = af0;
eph(20,:) = af1;
eph(21,:) = toe;

fidu = fopen(outputfile,'w');
count = fwrite(fidu,[eph],'double');
fclose all;
%%%%%%%%% end rinexe.m %%%%%%%%%


% Copyright 2013 The MathWorks, Inc
%% Ground-Based Monostatic Radar - Range and Doppler Estimation
% This example shows how to model a ground-based monostatic pulse radar to
% estimate the range and speed of fluctuating targets.
%% Model
maxrange  = 8000; % Maximum range (m)
range_res = 50;   % Range resolution (m)
pd        = 0.9;  % Probability of detection
pfa       = 1e-6; % Probability of false alarm
nint      = 48;   % Number of pulses to integrate

% The transmitted waveform is a chirp. An S-band pyramidal antenna array is
% used in this case. The antenna is mounted on a vehicle moving at 67 mph
% (30 m/s).
apos = [0; 0; 0];                  % Antenna position
avel = [30*cos(30);30*sin(30);0];  % Antenna velocity
fc   = 3e9;                        % Operating frequency
sAnt = lowProfileArray('FrequencyRange',[2/3*fc 4/3*fc],'ViewArray',false);
[sWav,sTx,sAntPlat,sRad,fs,prf] = setupTx(maxrange,range_res,pd,pfa,nint,sAnt,apos,avel,fc);

% Three targets are set in motion in a free space environment.
tgtRCS = [1.2 1.1 1.05];
tgtpos = [2000 4560 5825; 0 0 0; 0 0 0];
tgtvel = [100 -400 350;0 0 0; 0 0 0];    % m/s
[sTgt,sTgtMotion,sChan] = setupTheater(tgtRCS,tgtpos,tgtvel,fc,fs);

% On the receiver side, matched filter, time-varying gain control and
% non-coherent pulse integration are applied to improve SNR. A
% range-Doppler map is generated and a Neyman-Pearson (NP) decision rule is
% used to achieve the desired Pfa.
nf          = 0;                                   % Noise figure
fast_time   = 0:1/fs:1/prf-1/fs;                   % Fast time grid
range_gates = physconst('LightSpeed')*fast_time/2; % Range gates
pulses      = zeros(numel(fast_time),nint);        % Pre-allocate
intpulses   = zeros(numel(fast_time),1);
[sCol,sRx,sRD,sMFilt,sTVG,threshold] = setupRx(nint,nf,pfa,maxrange,range_gates,sWav,sAnt,fc);

%% Simulate
rsig = zeros(336,3);
ang  = zeros(2,3);
for m = 1:2000
    [s,tx_status] = step(sTx,step(sWav));               % Transmit pulse
    [apos,avel]   = step(sAntPlat,1/prf);               % Move antenna
    for n = 1:3                                         % For each target
        [tpos,tvel]   = step(sTgtMotion{n},1/prf);      % Move target
        [~, ang(:,n)] = rangeangle(tpos,apos);          % Angle between antenna and target
        tsig          = step(sRad,s,ang(:,n));          % Radiate signal
        tsig          = step(sChan{n},tsig,apos,tpos,avel,tvel); % Propagate two ways
        rsig(:,n)     = step(sTgt{n},tsig,true);        % Reflect off target
    end
    rsig = step(sCol,rsig,ang);                         % Collect
    rsig = sum(rsig,2);                                 % Beamform
    nn   = mod(m-1,nint)+1;
    pulses(:,nn) = step(sRx,rsig,~(tx_status>0));       % Receiver pre-amp
    pulses(:,nn) = step(sTVG,pulses(:,nn));             % Time varying gain
    [rdmap,rgrid,sgrid] = step(sRD,pulses,sMFilt.Coefficients); % Range-Doppler estimate
    pulses(:,nn) = step(sMFilt,pulses(:,nn));           % Matched filter
    if nn == nint
        intpulses  = pulsint(pulses,'noncoherent');     % Pulse integration
        [pmax,detect] = findpeaks(intpulses,'MinPeakHeight',sqrt(threshold)); % Detection
        tgtrange   = range_gates(detect-(numel(sMFilt.Coefficients)-1));      % Range estimation
    end
    viewSignals
end


function SO3F = conv(SO3F1,SO3F2,varargin)
% convolution of an SO3FunHarmonic with a function or a kernel on SO(3)
%
% 1) SO3Fun * SO3Fun
% There are two SO3Funs $f: _{S_f^L\backslash}SO(3)_{/S_f^R} \to \mathbb{C}$
% where $S_f^L$ is the Left symmetry and $S_f^R$ is the Right symmetry and
% $g: _{S_g^L\backslash}SO(3)_{/S_g^R} \to \mathbb{C}$ given.
% Then the convolution $ f *_L g : _{S_f^L\backslash}SO(3)_{/S_g^R} \to
% \mathbb{C}$ is defined by
%
% $$ (f *_L g)(R) = \frac1{8\pi^2} \int_{SO(3)} f(q) \cdot g(q^{-1}\,R) \, dq $$
%
% and the convolution $ f *_R g : _{S_g^L\backslash}SO(3)_{/S_f^R} \to
% \mathbb{C}$ is defined by
%
% $$ (f *_R g)(R) = \frac1{8\pi^2} \int_{SO(3)} f(q) \cdot g(R\,q^{-1}) \, dq $$.
%
% with $vol(SO(3)) = \int_{SO(3)} 1 \, dR = 8\pi^2$.
% The convolution $*_L$ is used as default.
% The convolution of matrices of SO3Functions with matrices of SO3Functions
% works elementwise.
%
% 2) SO3Fun * S2Fun
% The convolution of an SO3Fun  $f: _{S_f^L\backslash}SO(3)_{/S_f^R} \to \mathbb{C}$
% with an S2Fun $h: \mathbb S^2_{/S_h} \to \mathbb{C}$ yields
% $f*h:\mathbb S^2_{/S_f^L} \to \mathbb{C}$ with
%
% $$ (f * h)(\xi) =  \frac1{8\pi^2} \int_{SO(3)} f(q) \cdot h(q^{-1}\,\xi) \, dq $$.
%
% 3) In particular we convolute an SO3Fun with an SO3Kernel similar to the
% first case. Therefore the Right and Left sided convolution are
% equivalent. The convolution of an SO3Fun with an S2Kernel works analogue
% to case 2.
%
%
% Syntax
%   SO3F = conv(SO3F1,SO3F2)
%   SO3F = conv(SO3F1,SO3F2,'Right')
%   SO3F = conv(SO3F1,psi)
%   sF2 = conv(SO3F1,sF1)
%   sF2 = conv(SO3F1,phi)
%
% Input
%  SO3F1, SO3F2 - @SO3Fun
%  psi          - convolution @SO3Kernel
%  sF1          - @S2Fun
%  phi          - convolution @S2Kernel
%
% Output
%  SO3F - @SO3FunHarmonic
%  sF2  - @S2FunHarmonic
%
% See also
% SO3Kernel/conv SO3FunHarmonic/conv SO3FunRBF/calcFourier S2FunHarmonic/conv S2Kernel/conv

% The convolution is defined like above. But in MTEX the convolution of two
% SO3Funs is mostly calculated by
%                    conv(inv(conj(SO3F1)),SO3F2).
%

if isnumeric(SO3F1)
  SO3F = conv(SO3F2,SO3F1,varargin{:});
  return
end
if isnumeric(SO3F2)
  v = 2*(1:length(SO3F2))'-1;
  SO3F2 = SO3Kernel(SO3F2.*v);
end

% ------------------- convolution with a S2Kernel -------------------
if isa(SO3F2,'S2Kernel')
  psi = SO3F2;
  L = min(SO3F1.bandwidth,psi.bandwidth);

  fhat = zeros((L+1)^2,1);
  for l = 0:L
    fhat(l^2+1:(l+1)^2) = 2*sqrt(pi)/(2*l+1) * ...
          SO3F1.fhat(deg2dim(l)+(2*l+1)*l+(1:2*l+1)')*psi.A(l+1);
  end

  warning(['There is no symmetry given for the S2Kernel function. But for convolution the ' ...
      'right symmetry of the SO3Fun has to be compatible with the symmetry of the S2Fun.'])
  SO3F = S2FunHarmonic(fhat);
  return

end

% ------------------- convolution with a S2Fun -------------------
if isa(SO3F2,'S2Fun')

  sF = S2FunHarmonic(SO3F2);
  L = min(SO3F1.bandwidth,sF.bandwidth);

  fhat = zeros((L+1)^2,1);
  for l = 0:L
    fhat(l^2+1:(l+1)^2) = reshape(SO3F1.fhat(deg2dim(l)+1:deg2dim(l+1)),2*l+1,2*l+1) * ...
          sF.fhat(l^2+1:(l+1)^2) ./ sqrt(2*l+1);
  end

  % we need that SO3F1.SRight == SO3F.Sym
  if numProper(SO3F1.SLeft) == 1
    SO3F = S2FunHarmonicSym(fhat,SO3F1.SRight);
  elseif isa(SO3F2,'S2FunHarmonicSym')
    ensureCompatibleSymmetries(SO3F1,SO3F2);
    SO3F = S2FunHarmonicSym(fhat,SO3F1.SRight);
  else
    warning(['There is no symmetry of the S2Fun given. But for convolution the ' ...
      'right symmetry of the SO3Fun has to be compatible with the unknown symmetry.'])
    SO3F = S2FunHarmonic(fhat);
  end

  return

end

% ------------------- convolution with a SO3Kernel -------------------
% ( Here *L is the same like *R )
if isa(SO3F2,'SO3Kernel')

  L = min(SO3F1.bandwidth,SO3F2.bandwidth);
  SO3F1.bandwidth = L;
  s = size(SO3F1); SO3F1 = SO3F1.subSet(':');

  % multiply Wigner-D coefficients of SO3F1
  % with the Chebyshev coefficients A of SO3F2
  A = SO3F2.A;
  for l = 0:L
    SO3F1.fhat(deg2dim(l)+1:deg2dim(l+1),:) = ...
      A(l+1)./(2*l+1) * SO3F1.fhat(deg2dim(l)+1:deg2dim(l+1),:);
  end

  SO3F = reshape(SO3F1,s);
  return

end

% ------------------- convolution of SO3Fun's -------------------
% i) right sided convolution
if check_option(varargin,'Right')
  SO3F = inv(conv(inv(SO3F1),inv(SO3F2)));
  return
end

% ii) left sided convolution (default)
ensureCompatibleSymmetries(SO3F1,SO3F2,'conv_Left');

L = min(SO3F1.bandwidth,SO3F2.bandwidth);

% compute Fourier coefficients
fhat1 = SO3F1.calcFourier('bandwidth',L);
fhat2 = SO3F2.calcFourier('bandwidth',L);

% construct SO3FunHarmonic by multiplying the Fourier coefficients
SO3F = SO3FunHarmonic(convSO3(fhat1,fhat2),SO3F2.SRight,SO3F1.SLeft);

end


%% Bacterial foraging
% Animiation of bacteria movement to get the global minimum solution every chemotactic
%
% Author: Wael Mansour (wael192@yahoo.com)
%
% MSc Student, Electrical Enginering Dept,
% Faculty of Engineering Cairo University, Egypt

%%
%Initialization
clear all
clc
p=2;                         % dimension of search space
s=26;                        % The number of bacteria
Nc=50;                       % Number of chemotactic steps
Ns=4;                        % Limits the length of a swim
Nre=4;                       % The number of reproduction steps
Ned=2;                       % The number of elimination-dispersal events
Sr=s/2;                      % The number of bacteria reproductions (splits) per generation
Ped=0.25;                    % The probabilty that each bacteria will be eliminated/dispersed
c(:,1)=0.05*ones(s,1);       % the run length
for m=1:s                    % the initital posistions
    P(1,:,1,1,1)= 50*rand(s,1)';
    P(2,:,1,1,1)= .2*rand(s,1)';
   %P(3,:,1,1,1)= .2*rand(s,1)';
end

%%
%Main loop

%Elimination and dispersal loop
for ell=1:Ned

%Reprodution loop

    for K=1:Nre

%  swim/tumble(chemotaxis)loop

        for j=1:Nc

            for i=1:s
                J(i,j,K,ell)=Live_fn(P(:,i,j,K,ell));

% Tumble

                Jlast=J(i,j,K,ell);
                Delta(:,i)=(2*round(rand(p,1))-1).*rand(p,1);
                P(:,i,j+1,K,ell)=P(:,i,j,K,ell)+c(i,K)*Delta(:,i)/sqrt(Delta(:,i)'*Delta(:,i)); % This adds a unit vector in the random direction

% Swim (for bacteria that seem to be headed in the right direction)

                J(i,j+1,K,ell)=Live_fn(P(:,i,j+1,K,ell));
                m=0;         % Initialize counter for swim length
                    while m<Ns
                          m=m+1;
                          if J(i,j+1,K,ell)<Jlast
                             Jlast=J(i,j+1,K,ell);
                             P(:,i,j+1,K,ell)=P(:,i,j+1,K,ell)+c(i,K)*Delta(:,i)/sqrt(Delta(:,i)'*Delta(:,i)) ;
                             J(i,j+1,K,ell)=Live_fn(P(:,i,j+1,K,ell));
                          else
                             m=Ns ;
                          end

                    end
                J(i,j,K,ell)=Jlast;
                sprintf('The value of interation i %3.0f ,j = %3.0f  , K= %3.0f, ell= %3.0f' , i, j, K ,ell );

            end % Go to next bacterium

            x = P(1,:,j,K,ell);
            y = P(2,:,j,K,ell);
            clf
            plot(x, y , 'h')
            axis([-5 5 -5 5]);
            pause(.1)
        end  % Go to the next chemotactic

%Reprodution
        Jhealth=sum(J(:,:,K,ell),2);              % Set the health of each of the S bacteria
        [Jhealth,sortind]=sort(Jhealth);          % Sorts the nutrient concentration in order of ascending
        P(:,:,1,K+1,ell)=P(:,sortind,Nc+1,K,ell);
        c(:,K+1)=c(sortind,K);                    % And keeps the chemotaxis parameters with each bacterium at the next generation

%Split the bacteria (reproduction)
            for i=1:Sr
                P(:,i+Sr,1,K+1,ell)=P(:,i,1,K+1,ell); % The least fit do not reproduce, the most fit ones split into two identical copies
                c(i+Sr,K+1)=c(i,K+1);
            end
        end %  Go to next reproduction

%Eliminatoin and dispersal
        for m=1:s
            if  Ped>rand % % Generate random number
                P(1,:,1,1,1)= 50*rand(s,1)';
                P(2,:,1,1,1)= .2*rand(s,1)';
               %P(3,:,1,1,1)= .2*rand(s,1)';
            else
                P(:,m,1,1,ell+1)=P(:,m,1,Nre+1,ell); % Bacteria that are not dispersed
            end
        end
    end % Go to next elimination and disperstal

%Report
           reproduction = J(:,1:Nc,Nre,Ned);
           [jlastreproduction,O] = min(reproduction,[],2);  % min cost function for each bacterial
           [Y,I] = min(jlastreproduction)
           pbest=P(:,I,O(I,:),K,ell)


function [L,a,b] = rgb2lab(R,G,B)
% function [L, a, b] = RGB2Lab(R, G, B)
% RGB2Lab takes matrices corresponding to Red, Green, and Blue, and
% transforms them into CIELab.  This transform is based on ITU-R
% Recommendation  BT.709 using the D65 white point reference.
% The error in transforming RGB -> Lab -> RGB is approximately
% 10^-5.  RGB values can be either between 0 and 1 or between 0 and 255.
% By Mark Ruzon from C code by Yossi Rubner, 23 September 1997.
% Updated for MATLAB 5 28 January 1998.

if (nargin == 1)
  B = double(R(:,:,3));
  G = double(R(:,:,2));
  R = double(R(:,:,1));
end

if ((max(max(R)) > 1.0) | (max(max(G)) > 1.0) | (max(max(B)) > 1.0))
  R = R/255;
  G = G/255;
  B = B/255;
end

[M, N] = size(R);
s = M*N;

% Set a threshold
T = 0.008856;

RGB = [reshape(R,1,s); reshape(G,1,s); reshape(B,1,s)];

% RGB to XYZ
MAT = [0.412453 0.357580 0.180423;
       0.212671 0.715160 0.072169;
       0.019334 0.119193 0.950227];
XYZ = MAT * RGB;

X = XYZ(1,:) / 0.950456;
Y = XYZ(2,:);
Z = XYZ(3,:) / 1.088754;

XT = X > T;
YT = Y > T;
ZT = Z > T;

fX = XT .* X.^(1/3) + (~XT) .* (7.787 .* X + 16/116);

% Compute L
Y3 = Y.^(1/3);
fY = YT .* Y3 + (~YT) .* (7.787 .* Y + 16/116);
L  = YT .* (116 * Y3 - 16.0) + (~YT) .* (903.3 * Y);

fZ = ZT .* Z.^(1/3) + (~ZT) .* (7.787 .* Z + 16/116);

% Compute a and b
a = 500 * (fX - fY);
b = 200 * (fY - fZ);

L = reshape(L, M, N);
a = reshape(a, M, N);
b = reshape(b, M, N);

if ((nargout == 1) | (nargout == 0))
  L = cat(3,L,a,b);
end


% ****************************************************************
% This is a function used for robotics project
%
% MAE 505: Robotic Final Project.
% Created By: Gan Tao
% Date: 10 Dec 2003.
% ****************************************************************

function [X_dot] = good_lang2(t, q)

% Xc                ***X coordinate of center mass of the mobile platform
% Yc                ***Y coordinate of center mass of the mobile platform
% fai               ***heading angle of the platform measured from the X axis of the world coordinates
% theta_R           ***angular displacement of the right wheel
% theta_L           ***angular displacement of the left wheel
% theta_dot_R       ***angular velocity of the right wheel??
% theta_dot_L       ***
% Xr                ***X coordinate of world coordinates of the reference point
% Yr                ***Y coordinate of world coordinates of the reference point
% Xr_c              ***
% Yr_c              ***
% c                 ***c=(r/2*b)
% b                 ***the distance between the driving wheels and the axis of symmetry
% d                 ***the distance from Po(the intersection of the axis of symmetry with the driving wheel axis) to Pc(the center
%                   ***of mass of the platform

Xr_c=5;
Yr_c=0;
c   =0.5;
b   =0.75;
d   =1;
Kp  =2;
Kd  =5;

Xc          = q(1); % xc
Yc          = q(2); % yc
fai         = q(3); % fai
theta_R     = q(4); % theta right
theta_L     = q(5); % theta left
theta_dot_R = q(6); % theta dot right
theta_dot_L = q(7); % theta dot left

% ****************** The path to trace:**************************************************************************

y_desire = t;       %line case ii x=y;
x_desire = t;

xy_dot_desire = [1; 1]; %2x1 matrix         line x=y case ii;

xy_2_dot_desire = [0; 0]; %2x1 matrix

xy_desire =[x_desire y_desire]';

%****************** To get the new input V(y2_dot) **************************************************************

Xr = Xc+Xr_c*cos(fai)-Yr_c*sin(fai);
Yr = Yc+Xr_c*sin(fai)+Yr_c*cos(fai);

xy = [Xr;Yr];

S = [c*(b*cos(fai)-d*sin(fai)) c*(b*cos(fai)+d*sin(fai));
     c*(b*sin(fai)+d*cos(fai)) c*(b*sin(fai)-d*cos(fai));
     c                         -c                       ;
     1                         0                        ;
     0                         1                        ]; %5*2;

faifunction = [ c*(b*cos(fai)-d*sin(fai))+(-Xr_c*sin(fai)-Yr_c*cos(fai))*c,c*(b*cos(fai)+d*sin(fai))-(-Xr_c*sin(fai)-Yr_c*cos(fai))*c;
                c*(b*sin(fai)+d*cos(fai))+(Xr_c*cos(fai)-Yr_c*sin(fai))*c,c*(b*sin(fai)-d*cos(fai))-(Xr_c*cos(fai)-Yr_c*sin(fai))*c ]; %2*2;

%*** faifunction_dot = diff(faifunction,fai);*****
niu = [theta_dot_R;theta_dot_L];

faifunction_dot =[ c*(-b*sin(fai)-d*cos(fai))+(-Xr_c*cos(fai)+Yr_c*sin(fai))*c,c*(-b*sin(fai)+d*cos(fai))-(-Xr_c*cos(fai)+Yr_c*sin(fai))*c;
             c*(b*cos(fai)-d*sin(fai))+(-Xr_c*sin(fai)-Yr_c*cos(fai))*c,c*(b*cos(fai)+d*sin(fai))-(-Xr_c*sin(fai)-Yr_c*cos(fai))*c ]; %2*2;

xy_dot  = faifunction*niu;%2*1;

xy2_dot = xy_2_dot_desire+Kd*(xy_dot_desire - xy_dot)+Kp*(xy_desire - xy);%2*1;

%**************** Using V(xy2_dot) to get the U input ************************************************************

U = inv(faifunction)*(xy2_dot-faifunction_dot*niu); %2*1

%**************** Using U to get the X_dot ***********************************************************************

f1 = S*niu;%5*1;

X_dot = [f1(1);f1(2);f1(3);f1(4);f1(5); 0; 0] + [0 0;0 0;0 0;0 0;0 0;1 0;0 1]*[U(1);U(2)];


function [c, ceq] = ConstraintFCN_models(u,uold,x,N,LBo,UBo,LBdu,UBdu,p,select_model)
%% Constraint function of nonlinear MPC for F8 system
%
% Inputs:
%   u:      optimization variable, from time k to time k+N-1
%   x:      current state at time k
%   Ts:     controller sample time
%   N:      prediction horizon length
%   uold:   latest applied control input
%   LBo:    Lower bound of output x
%   UBo:    Upper bound of output x
%   LBdu:   Lower bound for input difference uk - uk-1
%   UBdu:   Upper bound for input difference uk - uk-1
%   p:      Parameters for model
%   select_model: Selects model future-state prediction
%
% Output:
%   c:      inequality constraints applied across prediction horizon
%   ceq:    equality constraints  (empty)
%
Nvar = length(x);
%% Nonlinear MPC design parameters
% range of angle of attack
zMin = LBo(1);
zMax = UBo(1);

%% Integrate system
if strcmp(select_model,'DelayDMDc')
     [xk,~] = lsim(p.sys,[p.udelay(1:N);u'],[0:N-1].*p.dt,[p.xdelay(:,1); x]-[p.xmean; p.xmean]);
    xk = xk(:,Nvar+1:2*Nvar);
    xk = xk + repmat(p.xmean',[N 1]); xk = xk';
elseif strcmp(select_model,'DMDc')
    [xk,~] = lsim(p.sys,[u',0],[0:N].*p.dt,x-p.xmean);
    xk = xk(2:end,:) + repmat(p.xmean',[N 1]); xk = xk';
elseif strcmp(select_model,'SINDYc')
    Ns = size(x,1);
    xk = zeros(Ns,N+1); xk(:,1) = x;
    for ct=1:N
        % Obtain plant state at next prediction step.
        xk(:,ct+1) = rk4u(@sparseGalerkinControl_Discrete,xk(:,ct),u(ct),p.dt,1,[],p);
    end
    xk = xk(:,2:N+1);
elseif strcmp(select_model,'NARX')
    Hu = [u',0];
    Hx = zeros(Nvar,length(Hu)); Hx(:,1) = x;
    [Us,Ui,Si] = preparets(p.net,con2seq(Hu),{},con2seq(Hx));
    xk = p.net(Us,Ui,Si);
    xk = cell2mat(xk);
end

%% Inequality constraints calculation
c = zeros(2*N,1);

% Apply N population size constraints across prediction horizon, from time
% k+1 to k+N
duk = u(1)-uold;
for ct=1:N
    c(2*ct-1) = -duk+LBdu;
    c(2*ct) = duk-UBdu;
    if ct<N
        duk = u(ct+1)-u(ct);
    end
end

c1 = zeros(2*N,1);
c2 = zeros(2*N,1);
c3 = zeros(2*N,1);
for ct=1:N
    c1(2*ct-1) = -xk(1,ct)+zMin;
    c2(2*ct-1) = -xk(2,ct)+LBo(2);
    c3(2*ct-1) = -xk(3,ct)+LBo(3);
    c1(2*ct) = xk(1,ct)-zMax;
    c2(2*ct) = xk(2,ct)-UBo(2);
    c3(2*ct) = xk(3,ct)-UBo(3);
end

c = [c;c1;c2;c3];

%% No equality constraints
ceq = [];


  function [xs, precon] = eml_osem(x, Gb, yi, ci, ri, varargin)
%|function xs = eml_osem(x, Gb, yi, ci, ri, [options])
%| E-ML-OSEM algorithm for image reconstruction from Poisson emission data
%| (ordered subsets expectation maximization)
%| model: Y_i ~ Poisson(c_i [G x]_i + r_i)
%|
%| in
%|	x	[np 1]		initial estimate
%|	Gb	[nd np]		Gblock object (see eml_osem_test.m)
%|	yi,ci,ri [nb na]	see em_fbp.m (for model too)
%|
%| option
%|	'niter', int		# iterations (default: 1+1)
%|	'isave', []		which iterations to archive (default: 'all')
%|	'pixmax', float		upper constraint for pixel values
%|	'precon [np {1|nblock}]	preconditioners; recommend: 'classic'
%|	'relax0' [1] or [2]	relax0 or (relax0, relax_rate)
%|	userfun @		user-defined function handle
%| out
%|	xs [np length(isave)]	updated image vectors each iteration
%|
%| Copyright 2002-3-14, Jeff Fessler, University of Michigan

if nargin < 3, ir_usage, end

% backward compatability with old usage
% eml_osem(x, Gb, yi, ci, ri, niter, pixmax, precon, relax0)
if length(varargin) && isnumeric(varargin{1})
	[xs, precon] = eml_osem_orig(x, Gb, yi, ci, ri, varargin{:});
return
end

% defaults
arg.niter = 1;
arg.isave = 'all';
arg.pixmax = inf;
arg.chat = false;
arg.relax0 = 1;
arg.relax_rate = 0;
arg.precon = 'classic';
arg.userfun = [];
arg = vararg_pair(arg, varargin);

arg.isave = iter_saver(arg.isave, arg.niter);

Gb = block_ob(Gb, 'ensure'); % make it a block object (if not already)
nblock = block_ob(Gb, 'n');
starts = subset_start(nblock);

if ~isvar('ci') || isempty(ci)
	ci = ones(size(yi));
end
if ~isvar('ri') || isempty(ri)
	ri = zeros(size(yi));
end

if length(arg.relax0) == 1
	arg.relax_rate = 0;
elseif length(arg.relax0) == 2
	arg.relax_rate = arg.relax0(2);
	arg.relax0 = arg.relax0(1);
else
	error relax
end

eml_check(yi, ci, ri, 'os', nblock);
[nb na] = size(yi);

%
% precompute the preconditioners for classic OSEM
%
if streq(arg.precon, 'classic')
	precon = zeros(size(Gb,2), nblock);
	for iset=1:nblock
		ticker([mfilename ' : precon'], iset, nblock)
		istart = starts(iset);
		ia = istart:nblock:na;
		Asum = Gb{istart}' * col(ci(:,ia));
		Asum(Asum == 0) = Inf; % avoid divide by 0
		precon(:, iset) = 1 ./ Asum;
	end, clear Asum

%
% This 'fast' approach uses the same preconditioner for each subset
% which helps convergence (if diminishing relaxation is used).
% Of course, convergence is not really essential in the unregularized case,
% but this saves a bit of memory by using just one preconditioner, which
% ought to work when the subsets are reasonably balanced.
% But, if diminishing relaxation is not used, then this can cause problems
% because pixels can get stuck at zero. So I no longer recommend this.
%
elseif streq(arg.precon, 'fast')
	warning 'Using fast preconditioner rather than classic OSEM'
	precon = Gb' * ci(:); % complete backprojection
	precon(precon == 0) = Inf; % avoid divide by 0
	precon = nblock ./ precon;

else % user-supplied precon
	precon = arg.precon;
	if ncol(precon) ~= nblock && ncol(precon) ~= 1
		error 'precon columns must be 1 or nblock'
	end
end

%
% loop over iterations
%
xs = zeros(numel(x), length(arg.isave));
if any(x <= 0), error 'need x > 0', end
x = min(x, arg.pixmax);
if any(arg.isave == 0)
	xs(:,find(arg.isave == 0)) = x;
end

for iter = 1:arg.niter
	relax = arg.relax0 / (1 + arg.relax_rate * (iter-2));

	%
	% loop over subsets
	%
	for iset=1:nblock
		ticker(mfilename, [iter iset], [arg.niter nblock])

		istart = starts(iset);
		ia = istart:nblock:na;

		% predicted measurements
		yp = reshape(Gb{istart} * x, nb, length(ia));
		yp = ci(:,ia) .* yp + ri(:, ia);
		yp(yp == 0) = inf;	% avoids /0 error

		pre = precon(:,min(iset,ncol(precon)));	% 1 or iset

		dhi = ci(:,ia) .* (yi(:,ia) ./ yp - 1);
		grad = Gb{istart}' * dhi(:);

		x = x + relax * (x .* pre) .* grad;

		% fix: implement ahn's shift trick? no, just use classic.
		nneg = sum(x < 0);
		if nneg, printm('oh no! %d negatives', nneg), minmax(x), end

		x = max(x,0);	% caution: if x becomes zero, it is always zero!
		x = min(x,arg.pixmax);
	end

	if arg.chat, printm('Range %g %g', min(x), max(x)), end
	if any(arg.isave == iter)
		xs(:,find(arg.isave == iter)) = x;
	end

	if ~isempty(arg.userfun)
		feval(arg.userfun);
	end
end

%
% eml_osem_orig()
% arguments: pixmax, precon, relax0
%
function [xs, precon] = eml_osem_orig(x, Gb, yi, ci, ri, niter, varargin)
arg = {'niter', niter};
opt = {'pixmax', 'precon', 'relax0'};
for ii=1:length(varargin)
	arg = {arg{:}, opt{ii}, varargin{ii}};
end
[xs, precon] = eml_osem(x, Gb, yi, ci, ri, arg{:});


function [y, w] = pts_tps_map( s, t, x, w, FAST, PROGRESS )
% PTS_TPS_MAP  Interpolate/warp/map N-dimensional points using a thin-plate
% spline transformation
%
% [Y, W] = PTS_TPS_MAP(S, T, X, W)
%
%    S is a (P,Ds,N)-volume where each (:,:,i)-matrix has the coordinates
%    of the source points that define the warp.
%
%    T is a (P,Dt,N)-volume where each (:,:,i)-matrix has the coordinates
%    of the target points that define the warp.
%
%      (P is the number of points, Ds and Dt are the dimension and N is the
%      number of configurations).
%
%    X is a (P,Ds,N)-volume where each (:,:,i)-matrix has the coordinates
%    of the points to be interpolated.
%
%    Y is a (P,Dt,N)-volume with the interpolated points.
%
%    X can also be a struct with 3 volumes: X.in (P1,Ds,N), X.endo (P2,Ds,N),
%    X.epi (P3,Ds,N). In this case, 3 warps are computed: With all points
%    for X.in; with the first half of S and T points for X.endo; and with
%    the second half of S and T points for X.epi.
%
%    W is a D2-column matrix where each column has the weight and affine
%    weights vector computed with PTS_TSP_WEIGHTS (if W is empty or
%    missing, then W is computed internally).
%
%    If X is a struct, then W has to be a similar struct with the weights
%    for each of the warps.
%
% ... = PTS_TPS_MAP(S, T, X, W, FAST)
%
%    To speed up computations, all points in the same configuration are
%    warped together using vector and matrix operations.
%
%    This is more memory expensive. If for some reason memory becomes a
%    problem, using FAST=false it is possible to run the algorithm using
%    a "for" loop to warp 1 point at a time. This is roughly 14 times
%    slower. By default the faster method is used, FAST=true.
%
% ... = PTS_TPS_MAP(S, T, X, W, FAST, PROGRESS)
%
%    Making PROGRESS=true, and if you have function STATUSBAR() by
%    Leutenegger Marcel, you will see a progress bar with an estimate of
%    the time remaining to completion if you are using the slow method.
%
%
% See also: pts_tps_weights.

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2006-2011 University of Oxford
% Version: 0.6.4
% $Rev: 434 $
% $Date: 2011-06-03 17:16:33 +0100 (Fri, 03 Jun 2011) $
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
error(nargchk(3, 6, nargin));
error(nargoutchk(0, 2, nargout));

% defaults
if (nargin < 5 || isempty(FAST))
    FAST = true;
end
if (nargin < 6 || isempty(PROGRESS))
    PROGRESS = false;
end

% struct and sizes check
P = size(s, 1); % number of control points
if (P ~= size(t, 1))
    error('S and T must have the same number of points')
end

% compute weights if needed
N = size(s, 3); % number of frames
if (nargin < 4 || isempty(w))
    if isstruct(x) % triple warp

        for I = 1:N
            w(I).in = pts_tps_weights(s(:, :, I), t(:, :, I));
            w(I).endo = pts_tps_weights(s(1:(P/2), :, I), ...
                t(1:(P/2), :, I));
            w(I).epi = pts_tps_weights(s((P/2+1):end, :, I), ...
                t((P/2+1):end, :, I));
        end

    else
        w = pts_tps_weights(s, t);
    end
end

% deal with empty point sets
if isempty(x)
    y = [];
    return;
end

if isstruct(x) % triple warp
    if any(~isfield(x, {'endo', 'epi', 'in'}))
        error('Invalid fields for struct in X')
    end

    % compute each warp separately
    y(N) = struct( ...
        'in', zeros(size(x(1).in)), ...
        'endo', zeros(size(x(1).endo)), ...
        'epi', zeros(size(x(1).epi)));
    for I = 1:N
        y(I).in = pts_tps_map(s(:, :, I), t(:, :, I), ...
            x(I).in, w(I).in, FAST);
        y(I).endo = pts_tps_map(s(1:(P/2), :, I), ...
            t(1:(P/2), :, I), x(I).endo, w(I).endo, FAST);
        y(I).epi = pts_tps_map(s((P/2+1):end, :, I), ...
            t((P/2+1):end, :, I), x(I).epi, w(I).epi, FAST);
    end

    return
end

% dimensionality of points
D = size(s, 2);

if (D ~= size(x, 2))
    error(['Dimensionality of S is ' num2str(D) ', of X is ' num2str(size(x, 2))])
end
if (N ~= size(t, 3) || N ~= size(x, 3))
    error('S, T and X must have the same number of point configurations')
end

% init output
y = zeros(size(x, 1), size(w, 2));

PP = size(x, 1); % PP: number of points to be interpolated

if FAST

    % warp every input point
    for I = 1:N % loop frames

        % init aux matrix
        u = zeros(P * PP, D);

        % interleave points to be warped
        for J = 1:D
            u(:, J) = reshape(repmat(x(:, J, I), 1, P)', P * PP, 1);
        end

        % compute norm(Pi - (x,y)).^2
        u = sum((u - repmat(s(:, :, I), PP, 1)) .^ 2, 2);

        % reshape to have 1 row vector per point to be interpolated
        u = reshape(u, P, PP)';

        % thin-plate spline distance function
        % U(r) = r^2 log10(r)
        % note: it's faster to compute the log10 this way than directly
        warning('off', 'MATLAB:log:logOfZero');
        u = 0.5 * u .* log(u) * (1/log(10));
        u(isnan(u)) = 0;
        warning('on', 'MATLAB:log:logOfZero');

        % factor by weights f(x,y) = a1 + ax*x + ay*y + sum(wi*U(|Pi - (x,y)|))
        y(:, :, I) = [u , ones(PP, 1) , x(:, :, I)] * w(:, :, I);

    end

else % loop throug each point; this is slower but less memory consuming

    % show status bar
    if PROGRESS
        delete(statusbar)
        bar = statusbar('Progress bar...');
    else
        bar = [];
    end

    % warp every input point
    for I = 1:N % loop frames
        for J = 1:PP
            % compute norm(Pi - (x,y)).^2
            % U(r) = r^2 log10(r)
            % note: it's faster to compute the log10 this way than directly
            u = sum(...
                (repmat(x(J, :, I), P, 1) - s(:, :, I)) .^ 2, 2)';
            warning('off', 'MATLAB:log:logOfZero');
            u = 0.5 * u .* log(u) * (1/log(10));
            u(isnan(u)) = 0;
            warning('on', 'MATLAB:log:logOfZero');

            % factor by weights f(x,y) = a1 + ax*x + ay*y + sum(wi*U(|Pi - (x,y)|))
            y(J, :, I) = ...
                [u , ...
                1 , ...
                x(J, :, I)] ... % x, y, z, etc
                * w(:, :, I);

            % update progress bar every 10th of the total number of points
            if mod(J, round(PP/10))
                if (PROGRESS && isempty(statusbar(((I-1)*PP+J)/(N*PP), bar)))
                    break
                end
            end

        end

%         % update progress bar every frame
%         if isempty(statusbar(I/N, bar))
%             break
%         end

    end

    % delete progress bar
    delete(bar);

end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

% used market data
load 'MODEL_Hist';
% load 'MODEL_Hist_new';

% Discretization parameters
NTime = 12;                    % number of time steps
NSim = 100;                  % number of simulations
NBatches = 1;                  % number of batches

NHist = 251;                     % number of historic scenarios

NModels = 10;                  % number of models

% Options
%K = 6800;                              % Strike
%C = 1;                                 % call (1) or put (0)
LF = 0; LC = 0.04; GF = 0; GC = 0.15;   % cliquet params
U = Spot * 0.9;
L = Spot * 1.1;
T= 1;                                   % maturity of option

% Comment in the option you wish to price
option1 = 'Arithmetic Asian Call';
option2 = 'Arithmetic Asian Put';
exotic_price_c = @(x,y,r) Price_ArithmeticAsian(x,y,1,r,T);
exotic_price_p = @(x,y,r) Price_ArithmeticAsian(x,y,0,r,T);

%option1 = 'FixedStrike Lookback Call';
%option2 = 'FixedStrike Lookback Put';
%exotic_price_c = @(x,y,r) Price_LookBackFixedStrike(x,y,1,r,T);
%exotic_price_p = @(x,y,r) Price_LookBackFixedStrike(x,y,0,r,T);

%option1 = 'FloatingStrike Lookback Call';
%option2 = 'FloatingStrike Lookback Put';
%exotic_price_c = @(x,y,r) Price_LookBackFloatingStrike(x,y,1,r,T);
%exotic_price_p = @(x,y,r) Price_LookBackFloatingStrike(x,y,0,r,T);

% option1 = 'Arithmetic Cliquet';
% exotic_price_c = @(x,r) Price_ArithmeticCliquet(x,LF,LC,GF,GC,r,T);

% option1 = 'Knock-Out Call';
% option2 = 'Knock-Out Put';
% exotic_price_c = @(x,y,r) Price_KnockOut(x,x(1,1),y,1,r,T);
% exotic_price_p = @(x,y,r) Price_KnockOut(x,x(1,1),y,0,r,T);

vanilla_price_c = @(x,y,r) Price_CallPut(x,y,1,r,T);
vanilla_price_p = @(x,y,r) Price_CallPut(x,y,0,r,T);

% output
ep1c = zeros(NHist,NModels); %ep2c = ep1c; ep3c = ep1c;
ep1p = ep1c; %ep2p = ep1c; ep3p = ep1c;

vp_c = ep1c; vp_p = ep1c;
qp_c = ep1c; qp_p = ep1c;

vc = zeros(NHist,1); vp = vc;

% Black Scholes
for l = 1: NHist
    pathS = MC_B(Spot(l),r1y(l),0,T,BS_Par_Hist(l),NTime,NSim,NBatches);
    ep1c(l,1) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,1) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,1) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,1) = vanilla_price_p(pathS,Spot(l),r1y(l));
    [vc(l), vp(l)] = blsprice(Spot(l), Spot(l), r1y(l), T, BS_Par_Hist(l),0);
    qp_c(l,1) = ep1c(l,1)/vp_c(l,1);
    qp_p(l,1) = ep1p(l,1)/vp_p(l,1);
end

% Merton
for l = 1: NHist
    pathS = MC_M(Spot(l),r1y(l),0,T,...
        MJ_Par_Hist(l,1),MJ_Par_Hist(l,2),MJ_Par_Hist(l,3),MJ_Par_Hist(l,4),...
        NTime,NSim,NBatches);
    ep1c(l,2) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,2) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,2) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,2) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,2) = ep1c(l,2)/vp_c(l,2);
    qp_p(l,2) = ep1p(l,2)/vp_p(l,2);
end

% Heston
for l = 1: NHist
    [pathS, pathV] = MC_QE(Spot(l),r1y(l),0,T,...
        HESTON_Par_Hist(l,1),HESTON_Par_Hist(l,2),HESTON_Par_Hist(l,4),...
        HESTON_Par_Hist(l,5),HESTON_Par_Hist(l,3),...
        NTime,NSim,NBatches);
    ep1c(l,3) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,3) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,3) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,3) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,3) = ep1c(l,3)/vp_c(l,3);
    qp_p(l,3) = ep1p(l,3)/vp_p(l,3);
end

% Bates
for l = 1: NHist
    [pathS, pathV] = MC_QE_j(Spot(l),r1y(l),0,T,...
        BATES_Par_Hist(l,1),BATES_Par_Hist(l,2),BATES_Par_Hist(l,4),BATES_Par_Hist(l,5),...
        BATES_Par_Hist(l,3),BATES_Par_Hist(l,7),BATES_Par_Hist(l,7),BATES_Par_Hist(l,8),...
        NTime,NSim,NBatches);
    ep1c(l,4) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,4) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,4) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,4) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,4) = ep1c(l,4)/vp_c(l,4);
    qp_p(l,4) = ep1p(l,4)/vp_p(l,4);
end

% Variance Gamma CGM
for l = 1:NHist
    pathS  = MC_VG_CGM(Spot(l),r1y(l),0,T,...
        VG_Par_Hist(l,1),VG_Par_Hist(l,2),VG_Par_Hist(l,3),...
        NTime,NSim,NBatches);
    ep1c(l,5) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,5) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,5) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,5) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,5) = ep1c(l,5)/vp_c(l,1);
    qp_p(l,5) = ep1p(l,5)/vp_p(l,1);
end

% Normal Inverse Gaussian
for l = 1:NHist
    pathS  = MC_NIG(Spot(l),r1y(l),0,T,...
        NIG_Par_Hist(l,1),NIG_Par_Hist(l,2),NIG_Par_Hist(l,3),...
        NTime,NSim,NBatches);
    ep1c(l,6) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,6) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,6) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,6) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,6) = ep1c(l,1)/vp_c(l,1);
    qp_p(l,6) = ep1p(l,6)/vp_p(l,1);
end

% Variance Gamma - GOU
for l = 1:NHist
    pathS  = MC_VGGOU(Spot(l),r1y(l),0,T,...
        VGGOU_Par_Hist(l,1), VGGOU_Par_Hist(l,2), VGGOU_Par_Hist(l,3),...
        VGGOU_Par_Hist(l,6), VGGOU_Par_Hist(l,4), VGGOU_Par_Hist(l,5),...
        NTime,NSim,NBatches);
    ep1c(l,7) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,7) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,1) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,7) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,7) = ep1c(l,7)/vp_c(l,1);
    qp_p(l,7) = ep1p(l,7)/vp_p(l,1);
end

% % Variance Gamma - CIR
for l = 1: NHist
    pathS =  MC_VGCIR(Spot(l),r1y(l),0,T,...
        VGCIR_Par_Hist(l,1),VGCIR_Par_Hist(l,2),VGCIR_Par_Hist(l,3), ...
        VGCIR_Par_Hist(l,4),VGCIR_Par_Hist(l,5),VGCIR_Par_Hist(l,6),...
        NTime,NSim,NBatches);
    ep1c(l,8) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,8) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,8) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,8) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,8) = ep1c(l,8)/vp_c(l,1);
    qp_p(l,8) = ep1p(l,8)/vp_p(l,1);
end

% NIG - GOU
for l = 1: NHist
    pathS =  MC_NIGGOU(Spot(l),r1y(l),0,T,...
        NIGGOU_Par_Hist(l,1),NIGGOU_Par_Hist(l,2),NIGGOU_Par_Hist(l,3), ...
        NIGGOU_Par_Hist(l,4),NIGGOU_Par_Hist(l,5),NIGGOU_Par_Hist(l,6),NTime,NSim,NBatches);
    ep1c(l,9) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,9) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,9) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,9) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,9) = ep1c(l,9)/vp_c(l,1);
    qp_p(l,9) = ep1p(l,9)/vp_p(l,1);
end

% NIG - CIR
for l = 1: NHist
    pathS =  MC_NIGCIR(Spot(l),r1y(l),0,T,...
        NIGCIR_Par_Hist(l,1),NIGCIR_Par_Hist(l,2),NIGCIR_Par_Hist(l,3), ...
        NIGCIR_Par_Hist(l,5),NIGCIR_Par_Hist(l,6),NIGCIR_Par_Hist(l,4),NTime,NSim,NBatches);
    ep1c(l,10) = exotic_price_c(pathS,Spot(l),r1y(l));
    ep1p(l,10) = exotic_price_p(pathS,Spot(l),r1y(l));
    vp_c(l,10) = vanilla_price_c(pathS,Spot(l),r1y(l));
    vp_p(l,10) = vanilla_price_p(pathS,Spot(l),r1y(l));
    qp_c(l,10) = ep1c(l,10)/vp_c(l,1);
    qp_p(l,10) = ep1p(l,10)/vp_p(l,1);
end
m1 = 'BS'; m2 = 'MJ'; m3 = 'Heston'; m4 = 'Bates';
m5 = 'VG'; m6 = 'NIG'; m7 = 'VGGOU'; m8 = 'VGCIR';
m9 = 'NIGGOU'; m10 =  'NIGCIR';

% plot the results
plotit(ep1c,option1,m1, m2, m3, m4, m5, m6 ,m7, m8, m9, m10);
plotit(ep1p,option2,m1, m2, m3, m4, m5, m6 ,m7, m8, m9, m10);
plotit(qp_c,strcat('Normalized ',option1),m1, m2, m3, m4, m5, m6 ,m7, m8, m9, m10);
plotit(qp_p,strcat('Normalized ',option2),m1, m2, m3, m4, m5, m6 ,m7, m8, m9, m10);

modelriskc = repmat(max(ep1c,[],2),1,NModels)-ep1c;
modelriskp = -repmat(max(ep1p,[],2),1,NModels)+ep1p;


function [V,F,TV,TF,obj] = unbake_normal_map(V,F,TV,TF,nim,varargin)
  % UNBAKE_NORMAL_MAP Given a uv-mapped (TV,TF) 3D surface (V,F) mesh and a
  % corresponding normal map image (nim), recover a high-resolution 3D geometry
  % that "explains" the normal map. Resolution (and runtime) is controlled by
  % the resolution of the normal map image (hint: start with a downsampled
  % image).
  %
  % [V,F,TV,TF,obj] = unbake_normal_map(V,F,TV,TF,nim,varargin)
  %
  % Inputs:
  %   V  #V by 3 list of 3D vertex positions
  %   F  #F by 3 list of triangle indices into V
  %   TV  #TV by 3 list of 2D uv vertex positions
  %   TF  #F by 3 list of triangle indices into TV
  %   nim  #nim by #nim by 3 image of *object-space* normals (in range [0,1])
  % Outupts:
  %   V  #V by 3 list of 3D vertex positions
  %   F  #F by 3 list of triangle indices into V
  %   TV  #TV by 3 list of 2D uv vertex positions
  %   TF  #F by 3 list of triangle indices into TV
  %
  % See also: normal_to_displacement_map, tangent_to_object_normal_map
  %

  function [sel,V,F,TV,TF] = upsample_helper(V,F,TV,TF)
    mean_area = mean(doublearea(TV,TF));
    pixel_area = 1./(size(nim,1)*size(nim,2));
    dblA = doublearea(TV,TF);
    sel = find(dblA>pixel_area);
    if isempty(sel)
      return;
    end
    % Selected edges on 3D mesh (V,F)
    [uE,~,EMAP] = unique(sort(reshape(F(:,[2 3 1 3 1 2]),[],2),2),'rows');
    uEM = sparse(EMAP,1,repmat(sparse(sel,1,1,size(F,1),1),3,1),size(uE,1),1)>0;
    MF = full(reshape(uEM(EMAP),[],3));
    % Selected edges on texture mesh (TV,TF)
    [uE,~,EMAP] = unique(sort(reshape(TF(:,[2 3 1 3 1 2]),[],2),2),'rows');
    uEM = sparse(EMAP,1,repmat(sparse(sel,1,1,size(TF,1),1),3,1),size(uE,1),1)>0;
    MTF = full(reshape(uEM(EMAP),[],3));
    M = MF | MTF;
    [TV,TF] = upsample(TV,TF,'OnlySelected',M);
    mprev = size(F,1);
    [V,F] = upsample(V,F,'OnlySelected',M);
    if ~quiet
      fprintf('#F: %g -> %g\n',mprev,size(F,1));
    end
  end

  max_iters = 1000;
  quiet = true;
  quiet = false;
  max_step_size_seen = -inf;
  method = 'lbfgs';
  progressive_upsampling = true;
  fix_symmetry = false;

  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'FixSymmetry','Method','Quiet','Progressive'}, ...
    {'fix_symmetry','method','quiet','progressive_upsampling'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  % Progressive Upsampling is better: similar amount of time for a far better
  % solution.
  if ~progressive_upsampling
    while true
      [sel,V,F,TV,TF] = upsample_helper(V,F,TV,TF);
      if isempty(sel)
        break;
      end
    end
  end

  cross2 = @(a,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)];
  nrmlev = @(p1,p2,p3) cross2(p2-p1,p3-p1);
  unrml = @(V,F) normalizerow(nrmlev(V(F(:,1),:),V(F(:,2),:),V(F(:,3),:)));

  while true
    % Compute normal averaged over all texture-space pixels landing in each
    % triangle: ~/uniform quadrature in texture space
    [X,Y] = meshgrid(linspace(0,1,size(nim,2)),linspace(1,0,size(nim,1)));
    I = in_element_aabb(TV(:,1:2),TF,[X(:) Y(:)]);
    Nnim = reshape(nim,[],3);
    TNsum = sparse( ...
      repmat(I(I>0),1,3),repmat(1:3,sum(I>0),1),Nnim(I>0,:),size(TF,1),3);
    TNcount =  sparse(I(I>0),1,1,size(TF,1),1);
    TN = normalizerow(full(TNsum./TNcount)*2-1);
    % Some triangles might not have received any samples
    N = unrml(V,F);
    TN(any(isnan(TN),2),:) = N(any(isnan(TN),2),:);
    A = diag(sparse(doublearea(V,F)));
    if fix_symmetry
      % flip if aggregate normal points in opposite direction from initial
      % normal
      D = A*sum(TN.*N,2);
      [C,CF] = connected_components(TF);
      CD = accumarray(CF',D)./accumarray(CF',diag(A));
      %tsurf(TF,[TV(:,1:2) C'],'CData',1*(CD(CF)<0.8));
      %T = normals(TV(:,1:2)*eye(2,3),TF);
      %tsurf(TF,[TV(:,1:2) C'],'CData',1*(T(:,3)>0));
      flipped = (CD(CF)<0.8);
      tsurf(TF(flipped,:),[TV(:,1:2) C'],'FaceVertexCData',N(flipped,:)*0.5+0.5);
      hold on;
      surf(X,Y,0*X,falpha(1,0),'FaceColor','texturemap','CData',nim);
      hold off;
      %TN(D<0,:) = -TN(D<0,:);
      error
    end
    ATN = A*TN;

    vec = @(X) X(:);
    Asqr = @(X) sum(sum(X.*(A*X)));
    obj_fun = @(V) Asqr(unrml(V,F)-TN);
    grad_fun = @(V) normal_gradient(V,F,vec(A*unrml(V,F))-vec(ATN));

    r3 = @(X) reshape(X,[],3);
    % This is awful...
    %options = optimoptions('fminunc','SpecifyObjectiveGradient',true,'Display','iter');
    %[V,obj1] = fminunc(@(V) deal(obj_fun(r3(V)),grad_fun(r3(V))),vec(V),options);

    switch method
    case 'lbfgs'
      % This thing actually seems to work
      [V,obj1] = fminlbfgs( ...
        @(V) fun_and_grad(V,@(V) obj_fun(r3(V)),@(V) grad_fun(r3(V))),vec(V), ...
        struct('Display','final','GradObj','on','TolFun',1e-6,'TolX',1e-5));
    case 'gd'
      %% My gradient descent is faster
      %[V,obj1] = fminlbfgs( ...
      %  @(V) fun_and_grad(V,@(V) obj_fun(r3(V)),@(V) grad_fun(r3(V))),vec(V), ...
      %  struct('HessUpdate','steepdesc','Display','final','GradObj','on','TolX',5e-5));
      [V,obj1] = fmingd( ...
        @(V) fun_and_grad(V,@(V) obj_fun(r3(V)),@(V) grad_fun(r3(V))),vec(V));
    end
    V = r3(V);

    %obj1 = obj_fun(V);
    %prev_step = 0;
    %step_size = 1;
    %prev_V = V;
    %for iter = 1:max_iters
    %  %method = 'gradient-descent';
    %  %method = 'stochastic-gradient-descent';
    %  %method = 'momentum';
    %  method = 'nesterov';
    %  %method = 'gauss-newton';
    %  %method = 'levenberg-marquardt';
    %  switch method
    %  case {'gradient-descent','gauss-newton','levenberg-marquardt','momentum'}
    %    % Recompute current normals and gradients
    %    dNdVT = normal_gradient(V,F);
    %    grad = dNdVT*(vec(A*unrml(V,F))-vec(ATN));
    %    switch method
    %    case {'gauss-newton','levenberg-marquardt'}
    %      max_step_size = 100;
    %      % This works really well for small meshes but takes forever/diverges for
    %      % big meshes
    %      K = (dNdVT*A*dNdVT');
    %      switch method
    %      case 'levenberg-marquardt'
    %        K = (dNdVT*A*dNdVT');
    %        T = diag(diag(K));
    %        lambda = 1;
    %        K = K + lambda*T;
    %      end
    %      % This works really well for small meshes but takes forever/diverges for
    %      % big meshes
    %      step = reshape(K\(-grad),[],3);
    %    case 'gradient-descent'
    %      % Gradient descent
    %      step = -reshape(grad,[],3);
    %      max_step_size = 1;
    %    case 'momentum'
    %      % Fairly sensitive to mu
    %      mu = 0.5;
    %      step = mu*prev_step + (1-mu)*(-reshape(grad,[],3));
    %      % works better if I use the non line search step here...
    %      prev_step = step;
    %      max_step_size = 1;
    %    end
    %  case 'nesterov'
    %    % following AQP paper
    %    eta = 10;
    %    theta = (1-sqrt(1/eta))/(1+sqrt(1/eta));
    %    V = (1+theta)*V - theta*prev_V;
    %    dNdVT = normal_gradient(V,F);
    %    grad = dNdVT*(vec(A*unrml(V,F))-vec(ATN));
    %    step = -reshape(grad,[],3);
    %    max_step_size = 1;
    %  case 'stochastic-gradient-descent'
    %    % not impressed with this... maybe it'll be worthwhile if to keep things
    %    % in memory
    %    %
    %    % Especially doesn't seem to play well with the "stalling" criteria.
    %    batch_size = min(size(F,1),10000);
    %    P = randperm(size(F,1));
    %    P = P(1:batch_size);
    %    dNdVT = normal_gradient(V,F(P,:));
    %    grad = dNdVT*(vec(A(P,P)*unrml(V,F(P,:)))-vec(ATN(P,:)));
    %    step = -reshape(grad,[],3);
    %    max_step_size = 1;
    %  end
    %  prev_V = V;
    %  [step_size,V,obj,step] = line_search(obj_fun,@(V) V,V,step,max_step_size);
    %  max_step_size_seen = max(max_step_size_seen,step_size);
    %  V = reshape(V,[],3);
    %  if step_size == 0
    %    warning('step size == 0');
    %    break;
    %  end
    %
    %  obj0 = obj1;
    %  obj1 = obj_fun(V);
    %  if ~quiet
    %    fprintf('  iter: % 5d, obj: %g\n',iter,obj1);
    %  end
    %  if obj0 - obj1 < 1e-5
    %    break
    %  end
    %end
    %if ~quiet
    %  fprintf('converged after %d iterations to %g\n',iter,obj1);
    %end

    if ~quiet
      tsurf(F,V,fsoft,'FaceVertexCData',repmat(orange,size(F,1),1),falpha(1,0.1));
      view(2);axis equal;camlight;
      %apply_ambient_occlusion([],'AddLights',false,'Factor',1);
      drawnow;
    end

    if progressive_upsampling
      [sel,V,F,TV,TF] = upsample_helper(V,F,TV,TF);
      if isempty(sel)
        break;
      end
    else
      % only one iteration needed: we upsampled everything at the beginning
      break;
    end
  end

  obj = obj_fun(V);
  if ~quiet
    fprintf('Final objective: %g\n',obj);
  end

  if ~quiet
    fprintf('max_step_size_seen: %g\n',max_step_size_seen);
  end

end


function S = globMatrixVIFE3DStiff(fun,mesh,fem1,fem2)

%% USAGE: generate stiffness global matrix on a 3D mesh
%
% INPUTS:
% fun --- coefficient function
% mesh --- a struct data contains very rich mesh information.
% fem1 --- global DoF for test function space
% fem2 --- global DoF for trial function space
%
% OUTPUTS:
% [IN JN XN] --- triplets of the sparse matrix from regular elements.
% [II JI XI] --- triplets of the sparse matrix from interface elements.

% Last Modified: 08/07/2020 by Xu Zhang

%% 0. Initializaiton
if strcmp(fem1.type,'P1')||strcmp(fem1.type,'DGP1')||strcmp(fem1.type,'CR')
    feEvalBas1 = @evalP1Bas3D;
elseif strcmp(fem1.type,'P2')||strcmp(fem1.type,'DGP2')
    feEvalBas1 = @evalP2Bas3D;
end

if strcmp(fem2.type,'P1')||strcmp(fem2.type,'DGP1')||strcmp(fem2.type,'CR')
    feEvalBas2 = @evalP1Bas3D;
elseif strcmp(fem2.type,'P2')||strcmp(fem2.type,'DGP2')
    feEvalBas2 = @evalP2Bas3D;
end

%% 1. Matrix on noninterface elements
dof1 = fem1.ldof; dof2 = fem2.ldof; nloc = dof1*dof2;
ntID = find(mesh.tLoc > 0); ntN = length(ntID);
AN = fem1.area(ntID);
gxN = fem1.gx(ntID,:); gyN = fem1.gy(ntID,:); gzN = fem1.gz(ntID,:); gw = fem1.gw;
XN = zeros(nloc*ntN, 1);

coefN = feval(fun,gxN,gyN,gzN);
Ibasx = cell(dof1,1); Ibasy = cell(dof1,1); Ibasz = cell(dof1,1);
Jbasx = cell(dof2,1); Jbasy = cell(dof2,1); Jbasz = cell(dof2,1);
for i = 1:dof1
    Ibasx{i} = feEvalBas1(fem1.bas(ntID,:,i), gxN, gyN, gzN, [1,0,0]);
    Ibasy{i} = feEvalBas1(fem1.bas(ntID,:,i), gxN, gyN, gzN, [0,1,0]);
    Ibasz{i} = feEvalBas1(fem1.bas(ntID,:,i), gxN, gyN, gzN, [0,0,1]);
end
for j = 1:dof2
    Jbasx{j} = feEvalBas2(fem2.bas(ntID,:,j), gxN, gyN, gzN, [1,0,0]);
    Jbasy{j} = feEvalBas2(fem2.bas(ntID,:,j), gxN, gyN, gzN, [0,1,0]);
    Jbasz{j} = feEvalBas2(fem2.bas(ntID,:,j), gxN, gyN, gzN, [0,0,1]);
end

IN = reshape(repmat(fem1.t(ntID,:),4,1),nloc*ntN,1);
JN = repmat(reshape(fem2.t(ntID,:),dof2*ntN,1),4,1);
ind = 0;
for i = 1:dof1
    for j = 1:dof2
        XN(ind+1:ind+ntN) = AN.*(sum(((Ibasx{i}.*(coefN.*Jbasx{j})).*gw'),2) + ...
            sum(((Ibasy{i}.*(coefN.*Jbasy{j})).*gw'),2) + ...
            sum(((Ibasz{i}.*(coefN.*Jbasz{j})).*gw'),2));
        ind = ind + ntN;
    end
end
ID = find(XN~=0);
Ndof = size([mesh.p;mesh.eIntP],1);
S = sparse(IN(ID),JN(ID),XN(ID),Ndof,Ndof);


function y = atanh(x)
%ATANH        Implements  atanh(x)  for intervals
%
%   y = atanh(x)
%
%interval standard function implementation
%

% written  12/30/98     S.M. Rump
% modified 08/31/99     S.M. Rump  complex allowed, sparse input,
%                                  major revision, improved accuracy
% modified 09/02/00     S.M. Rump  rounding unchanged after use
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 12/04/05     S.M. Rump  'realstdfctsexcptnignore' added and some
%                                     improvements, tocmplx replaced by cintval
% modified 09/06/07     S.M. Rump  approximate std fcts removed, exceptional arguments
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 10/18/08     S.M. Rump  StdFctsException ignore/NaN
% modified 08/26/12     S.M. Rump  global variables removed
% modified 10/13/12     S.M. Rump  INTLAB_INTVAL_STDFCTS
%

  if issparse(x)
    [ix,jx,sx] = find(x);
    [m,n] = size(x);
    y = sparse(ix,jx,atanh(full(sx)),m,n);
    return
  end

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

  if x.complex
    y = log( 2./(1-x) - 1 )/2;
    if rndold
      setround(rndold)
    end
    return
  end

  % input x real and full
  % real range of definition:  [-1,1]
  INTLAB_STDFCTS_EXCPTN = getappdata(0,'INTLAB_STDFCTS_EXCPTN');
  indexneg = ( x.inf<-1 );               % (partially) exceptional indices
  indexpos = ( x.sup>1 );                % (partially) exceptional indices
  if ~isempty(find(indexneg)) | ~isempty(find(indexpos)) % handle input out-of-range
    if INTLAB_STDFCTS_EXCPTN<=1   % out-of-range input handled as complex
      if INTLAB_STDFCTS_EXCPTN==1
        warning('ATANH: Real interval input out of range changed to be complex')
      end
      y = x;
      index = indexneg | indexpos;
      %VVVV  y(index) = atanh(cintval(x(index)));
      s.type = '()'; s.subs = {index}; y = subsasgn(y,s,atanh(cintval(subsref(x,s))));
      %AAAA  Matlab bug fix
      index = ~index;
      if any(index(:))
        %VVVV  y(index) = atanh(x(index));
        s.type = '()'; s.subs = {index}; y = subsasgn(y,s,atanh(subsref(x,s)));
        %AAAA  Matlab bug fix
      end
      if rndold
        setround(rndold)
      end
      return
    end
    setappdata(0,'INTLAB_STDFCTS_EXCPTN_',1);
    if INTLAB_STDFCTS_EXCPTN==3    % ignore input out of range (ignore-mode)
      x.inf(indexneg) = -1;               % completely exceptional indices treated below
      x.sup(indexpos) = 1;
      index = ( x.sup<-1 ) | ( x.inf>1 ); % completely exceptional indices
    end
  else
    index = [];                           % make sure Index is not undefined
  end

  % input x real and full
  y = x;
  wng = warning;
  warning off

  % treat non-exceptional arguments
  xinf = x.inf(:);
  xsup = x.sup(:);

  IndexInfPos = ( xinf>=0 );
  len1 = sum(IndexInfPos);
  IndexSupNeg = ( xsup<=0 );
  len2 = sum(IndexSupNeg);

  Y = atanh_pos( [ xinf(IndexInfPos) ; -xsup(IndexSupNeg) ] , -1 );
  y.inf(IndexInfPos) = Y(1:len1);
  y.sup(IndexSupNeg) = -Y( len1+1 : end );

  IndexInfNeg = ( xinf<0 );
  len1 = sum(IndexInfNeg);
  IndexSupPos = ( xsup>0 );
  len2 = sum(IndexSupPos);

  Y = atanh_pos( [ -xinf(IndexInfNeg) ; xsup(IndexSupPos) ] , 1 );
  y.inf(IndexInfNeg) = -Y(1:len1);
  y.sup(IndexSupPos) = Y( len1+1 : len1+len2 );

  y.inf(xinf==-1) = -inf;
  y.sup(xsup==-1) = -inf;
  y.inf(xinf==1) = inf;
  y.sup(xsup==1) = inf;

  if INTLAB_STDFCTS_EXCPTN==3      % ignore input out of range (ignore-mode)
    if ~isempty(find(index))              % completely exceptional arguments to NaN
      y.inf(index) = NaN;
      y.sup(index) = NaN;
    end
  else                                    % any input out of range to NaN (NaN-mode)
    index = indexneg | indexpos;
    if ~isempty(find(index))              % exceptional arguments to NaN
      y.inf(index) = NaN;
      y.sup(index) = NaN;
    end
  end

  setround(rndold)
  warning(wng)


% acsobiro() - A. Chickocki's robust Second-Order Blind Identification (SOBI)
%              by joint diagonalization of the time-delayed covariance matrices.
%              NOTE: THIS CODE ASSUMES TEMPORALLY CORRELATED SIGNALS.
%              Thus, the estimated time-delayed covariance matrices
%              for at least some time delays must be nonsingular.
%
% Usage:  >>   [H] = acsobiro(X);
%         >> [H,S] = acsobiro(X,n,p);
% Inputs:
%         X - data matrix of dimension [m,N] where
%                    m is the number of sensors
%                    N is the number of samples
%         n - number of sources {Default: n=m}
%         p - number of correlation matrices to be diagonalized {default: 100}
%             For noisy data, use at least 100 time delays.
% Outputs:
%         H - matrix of dimension [m,n] an estimate of the *mixing* matrix
%         S - matrix of dimension [n,N] an estimate of the source activities
%             where  >> X [m,N] = H [m,n] * S [n,N]
%
% Authors: Implemented and improved by A. Cichocki on the basis of
%          the classical SOBI algorithm of Belouchrani and publications of:
%            A. Belouchrani et al., F. Cardoso et al.,
%            S. Choi, S. Cruces, S. Amari, and P. Georgiev
%          For references: see function body
%
% Note: Extended by Arnaud Delorme and Scott Makeig to process data epochs
%       (computes the correlation matrix respecting epoch boundaries).

% REFERENCES:
%  A. Belouchrani, K. Abed-Meraim, J.-F. Cardoso, and E. Moulines, ``Second-order
%  blind separation of temporally correlated sources,'' in Proc. Int. Conf. on
%  Digital Sig. Proc., (Cyprus), pp. 346--351, 1993.
%
%  A. Belouchrani, and A. Cichocki,
%  Robust whitening procedure in blind source separation context,
%  Electronics Letters, Vol. 36, No. 24, 2000, pp. 2050-2053.
%
%  A. Cichocki and S. Amari,
%  Adaptive Blind Signal and Image Processing, Wiley,  2003.

function [H,S,D]=acsobiro(X,n,p),

[m,N,ntrials]=size(X);
if nargin<1 | nargin > 3

  help acsobiro

elseif nargin==1,

 DEFAULT_LAGS = 100;
 n=m; % source detection (hum...)
 p=min(DEFAULT_LAGS,ceil(N/3)); % number of time delayed correlation matrices to be diagonalized
                                % Note: For noisy data, use at least p=100.
elseif nargin==2,

 p=min(DEFAULT_LAGS,ceil(N/3)); % number of correlation matrices to be diagonalized

end;

X(:,:)=X(:,:)-(mean(X(:,:)')'*ones(1,N*ntrials));        % Remove data means

for t = 1:ntrials
    if t == 1
        Rxx=(X(:,1:N-1,t)*X(:,2:N,t)')/(N-1)/ntrials; % Estimate the sample covariance matrix
                                          % for the time delay p=1, to reduce influence
                                          % of white noise.
    else
        Rxx=Rxx+(X(:,1:N-1,t)*X(:,2:N,t)')/(N-1)/ntrials; % Estimate the sample covariance matrix
                                          % for the time delay p=1, to reduce influence
                                          % of white noise.
    end;
end;

[Ux,Dx,Vx]=svd(Rxx);
        Dx=diag(Dx);

if n<m, % under assumption of additive white noise and when the number
         % of sources is known, or can be estimated a priori
  Dx=Dx-real((mean(Dx(n+1:m))));
  Q= diag(real(sqrt(1./Dx(1:n))))*Ux(:,1:n)';
else    % under assumption of no additive noise and when the
        % number of sources is unknown
   n=max(find(Dx>1e-99)); % detect the number of sources
   fprintf('acsobiro(): Estimated number of sources is %d\n',n);
   Q= diag(real(sqrt(1./Dx(1:n))))*Ux(:,1:n)';
end;
Xb = zeros(size(X));
Xb(:,:)=Q*X(:,:); % prewhitened data

% Estimate the time delayed covariance matrices:
 k=1;
 pn=p*n; % for convenience
 for u=1:m:pn,
   k=k+1;
   for t = 1:ntrials
       if t == 1
           Rxp=Xb(:,k:N,t)*Xb(:,1:N-k+1,t)'/(N-k+1)/ntrials;
       else
           Rxp=Rxp+Xb(:,k:N,t)*Xb(:,1:N-k+1,t)'/(N-k+1)/ntrials;
       end;
   end;
   M(:,u:u+m-1)=norm(Rxp,'fro')*Rxp;  % Frobenius norm =
 end;                                  % sqrt(sum(diag(Rxp'*Rxp)))

% Approximate joint diagonalization:
eps=1/sqrt(N)/100; encore=1; U=eye(n);
while encore, encore=0;
 for p=1:n-1,
  for q=p+1:n,
    % Givens rotations:
    g=[ M(p,p:n:pn)-M(q,q:n:pn)  ;
        M(p,q:n:pn)+M(q,p:n:pn)  ;
        i*(M(q,p:n:pn)-M(p,q:n:pn))];
   [Ucp,D] = eig(real(g*g')); [la,K]=sort(diag(D));
   angles=Ucp(:,K(3));angles=sign(angles(1))*angles;
   c=sqrt(0.5+angles(1)/2);
   sr=0.5*(angles(2)-j*angles(3))/c; sc=conj(sr);
   asr = abs(sr)>eps ;
   encore=encore | asr ;
   if asr , % Update the M and U matrices:
     colp=M(:,p:n:pn);
     colq=M(:,q:n:pn);
     M(:,p:n:pn)=c*colp+sr*colq;
     M(:,q:n:pn)=c*colq-sc*colp;
     rowp=M(p,:);
     rowq=M(q,:);
     M(p,:)=c*rowp+sc*rowq;
     M(q,:)=c*rowq-sr*rowp;
     temp=U(:,p);
     U(:,p)=c*U(:,p)+sr*U(:,q);
     U(:,q)=c*U(:,q)-sc*temp;
   end  %% if
  end  %% q loop
 end  %% p loop
end  %% while

% Estimate the mixing matrix H
H= pinv(Q)*U(1:n,1:n);

% Estimate the source activities S
if nargout>1
  S=[];
  W=U(1:n,1:n)'*Q;
  S= W*X(:,:);
end


function r = times(a,b)
%TIMES        Gradient multiplication  a .* b
%

% written  10/16/98     S.M. Rump
% modified 11/30/98     S.M. Rump  array/scalar operations
% modified 04/04/04     S.M. Rump  set round to nearest for safety
%                                    improved performance
%                                    complete redesign
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 12/05/05     S.M. Rump  improved performance (thanks to J. Kubitz)
% modified 09/28/08     S.M. Rump  check for rounding to nearest improved
% modified 08/26/12     S.M. Rump  global variables removed
%

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

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');

  if ~isa(a,'gradient')
    m = prod(size(a));
    if m==1                     % non-gradient scalar .* gradient
      r.x = a * b.x;
      r.dx = a * b.dx;
    else                        % non-gradient array .* gradient
      n = prod(size(b.x));
      if n==1                   % non-gradient array .* gradient scalar
        r.x = a * b.x;
        ax = a(:);
        if issparse(b.dx)
          ax = sparse(ax);
        end
        r.dx = ax * b.dx;
      else                      % non-gradient array .* gradient array
        if ~isequal(size(a),size(b))
          error('gradient .* : dimensions not compatible')
        end
        r.x = a .* b.x;
        if issparse(b.dx)
          [ib,jb,sb] = find(b.dx);
          r.dx = sparse(ib,jb,reshape(a(ib),size(sb)).*sb,n,N);
        else
          r.dx = b.dx .* repmat(a(:),1,N);
        end
      end
    end
  elseif ~isa(b,'gradient')      % gradient times non-gradient
    r = b .* a;
    if rndold
      setround(rndold)
    end
    return
  else                          % both factors gradient
    m = prod(size(a.x));
    n = prod(size(b.x));
    sparse_ = issparse(a.dx) | issparse(b.dx);
    if m==1                     % scalar gradient .* gradient
      if n==1                   % scalar gradient .* scalar gradient
        r.x = a.x * b.x;
        r.dx = a.dx * b.x + a.x * b.dx;
      else                      % scalar gradient .* array gradient
        r.x = a.x * b.x;
        if sparse_
          r.dx = sparse(b.x(:)) * a.dx + b.dx * a.x;
        else
          r.dx = b.x(:) * a.dx + b.dx * a.x;
        end
      end
    else                        % array gradient .* gradient
      if n==1                   % array gradient .* scalar gradient
        r.x = a.x * b.x;
        if sparse_
          r.dx = sparse(a.x(:)) * b.dx + a.dx * b.x;
        else
          r.dx = a.x(:) * b.dx + a.dx * b.x;
        end
      else                      % array gradient .* array gradient
        if ~isequal(size(a),size(b))
          error('dimensions not compatible for gradient .*');
        end
        r.x = a.x .* b.x;
        if issparse(a.dx) | issparse(b.dx)
          r.dx = sparse(1:n,1:n,b.x(:))*a.dx + sparse(1:n,1:n,a.x(:))*b.dx;
        else
          r.dx = repmat(b.x(:),1,N) .* a.dx + b.dx .* repmat(a.x(:),1,N);
        end
      end
    end
  end

  r = class(r,'gradient');

  if rndold
    setround(rndold)
  end


function [theta,P,res]=JPLAY(X,Y,G,L,k,d,sigma,alfa,beta,gama,rho,maxiter,eta,epsilon)

% Joint & Progressive Learning Strategy (J-PLAY)

% Usage:
%       [theta,P] = JPLAY(X,Y,G,L,k,d,sigma,alfa,beta,gama,maxiter,eta)

%% Input:
%       X      -Input data: d*N (d: dimension, N: the number of sample)
%       Y      -Label matrix: c*N (c: the number of class),
%                             e.g. if class==1 then y=[1,0,0,...,0]'
%                                  if class==2 then y=[0,1,0,...,0]'
%       G      -Adjacency matrix: N*N
%       L      -Laplacian matrix: N*N
%       k      -The number of nearest neighbor
%       d      -Reduced dimension
%     sigma    -Gaussian kernel parameter used in constructing graph
%     alfa     -Reconstruction loss parameter (default: 1)
%     beta     -Manifold regularization parameter (default: 0.1)
%     gama     -Ridge regression parameter  (default: 0.1)
%    maxiter   -Maximum number of iterations (default: 1000)
%     eta      -Eta in AutoRULe (default: 0.1)

%% Output:
%     theta    -A series of coupled-projection matrices
%       P      -Property-labeled projection matrix

% (c) Danfeng Hong, Remote Sensing Technology Institute (IMF), German Aerospace Center (DLR), Germany.
%                   Singnal Processing in Earth Oberservation (SiPEO), Technical University of Munich (TUM), Germany.
%     Naoto Yokoya, Remote Sensing Technology Institute (IMF), German Aerospace Center (DLR), Germany.
%                   Singnal Processing in Earth Oberservation (SiPEO), Technical University of Munich (TUM), Germany.
%                   Research Center for Advanced Science and Technology, The Univerisity of Tokyo, Japan.
%     danfeng.hong@dlr.de
%     naoto.yokoya@dlr.de; yokoya@sal.rcast.u-tokyo.ac.jp

if nargin < 2
    error('Please specify label information!')
end;

if nargin < 3
    error('Please construct adjacency matrix!')
end;

if nargin < 4
    error('Please construct Laplacian matrix!')
end;

if nargin < 5
    error('Please specify the number of nearest neighbor!')
end;

if nargin < 6
    error('Please specify subspace dimensions!')
end;

if nargin < 7
    error('Please specify Gaussian kernel parameter used in constructing graph!')
end;

if nargin < 8 || isempty(alfa)
    alfa=1;
end;

if nargin < 9 || isempty(beta)
    beta=0.1;
end;

if nargin < 10 || isempty(gama)
    gama=0.1;
end;

if nargin < 11 || isempty(maxiter)
    maxiter=1000;
end;

if nargin < 12 || isempty(eta)
    eta=0.1;
end;

%% Parameters Setting
% epsilon = 1e-3; % Tolerance error 1e-3Houston2018
iter=1;
stop = false;
res=zeros(1,maxiter); % Residuals
Num=length(d); % The number of layers

%% JPL: Initialization-step
theta=cell(1,1);
X0=X;
for i=1:Num
       theta0=DR_LPP(X0,k,d(i),sigma,G);
        theta_init=AutoRULe(theta0'*X0,X0,L,eta,theta0',maxiter);
        X0=theta_init*X0;
        theta{1,i}=theta_init;
end

%% JPL: Fine-tuning parameters
while ~stop && iter < maxiter+1

    %% Solve W
    D=X;
    for i=1:Num
        D=theta{1,i}*D;
    end
    P=(alfa*(Y*D'))/(alfa*(D*D')+gama*eye(size(D*D')));

    %% Solve the group of theta
    for j=1:Num

        %give W
        Pi=P;
        if j<Num
            for z=Num:-1:j+1
                Pi=Pi*theta{1,z};
            end
        end

        %give H
        H=X;
        for m=1:j
            H=theta{1,m}*H;
        end

        %give X
        Xi=X;
%         XWi=XW;
        if j>1
           for n=1:j-1
               Xi=theta{1,n}*Xi;
%                XWi=theta{1,n}*XWi;
           end
        end

        theta{1,j}=Theta_ADMM(Y,Pi,H,Xi,L,alfa,beta,theta{1,j},rho,maxiter);
    end

    %% Compute the vaules of objection function
     ErrorTerm=X;
%      ErrorTerm1=XW;
     ManifoldTerm=0;
%      ManifoldTerm1=0;
     ReconstructionTerm=0;
     for r=1:Num
         ReconstructionTerm=ReconstructionTerm+(norm(ErrorTerm-theta{1,r}'*theta{1,r}*ErrorTerm,'fro')^2);
         ErrorTerm=theta{1,r}*ErrorTerm;
%          ErrorTerm1=theta{1,r}*ErrorTerm1;
         ManifoldTerm=ManifoldTerm+trace(ErrorTerm*L*ErrorTerm');
%          ManifoldTerm1=ManifoldTerm1+trace(ErrorTerm1*L1*ErrorTerm1');
     end
     res(1,iter)=0.5*alfa*(norm(Y-P*ErrorTerm,'fro')^2)+0.5*beta*(ManifoldTerm)+0.5*gama*(norm(P,'fro')^2)+0.5*(ReconstructionTerm);

    %% Check the convergence condition
    if iter>1
       r_Obj=abs(res(1,iter)-res(1,iter-1))/res(1,iter-1);
       if r_Obj<epsilon
            stop = true;
            fprintf(' i = %f,res_Obj= %f\n',iter,r_Obj);
            break;
       end

       if mod(iter,10) == 1
           fprintf(' i = %f,res_Obj= %f\n',iter,r_Obj);
       end
    end

    iter=iter+1;
end
end


% 5g ldpc encoding
% input: s, bit sequence of dimension K * 1
% output: encoded_bits, bit sequence
% reference: 3GPP TS 38.212 section 5.3.2
% author: Xiao, Shaoning 萧少宁
% license: MIT

function [encoded_bits, H, Z_c, encoded_bits_original] = ldpc_encode(s, base_graph_index)

K = length(s);

encoded_bits = zeros(3*K, 1);

if base_graph_index == 1
    a = 4;
    b = 22;
    c = 26;
    d = 42;
    e = 46;
    z = K/b;
    Z_c = z;
    N = 66 * Z_c;
    z = K/b;
    set_index = lifting_size_table_lookup(z);
    load parity_check_matrices_protocol_1
    BG = parity_check_matrices_protocol_1(:, :, set_index); %#ok<NODEF>
elseif base_graph_index == 2
    a = 4;
    b = 10;
    c = 14;
    d = 38;
    e = 42;
    z = K/b;
    Z_c = z;
    N = 50 * Z_c;
    set_index = lifting_size_table_lookup(z);
    load parity_check_matrices_protocol_2
    BG = parity_check_matrices_protocol_2(:, :, set_index); %#ok<NODEF>
else
  error('wrong base graph index in ldpc encoding.');
end

BG(BG ~= -1) = mod(BG(BG ~= -1), Z_c);

for k = (2*Z_c):(K-1)
  if s(k+1) ~= -1
    encoded_bits(k-2*Z_c+1) = s(k+1);
  else
    s(k+1) = 0;
    encoded_bits(k-2*Z_c+1) = -1;
  end
end

% set_index = lifting_size_table_lookup(Z_c);
% BG = parity_check_matrices_protocol(:, :, set_index);

A_prime = BG(1:a, 1:b);
B_prime = BG(1:a, (b+1):c);
C_prime = BG((a+1):e, 1:b);
D_prime = BG((a+1):e, (b+1):c);

z = Z_c;

A = spalloc(a*z, b*z, nnz(A_prime + ones(size(A_prime))));

for row_index = 1:a
    for column_index = 1:b
        if A_prime(row_index, column_index) ~= -1
            A((row_index-1)*z+1:row_index*z, (column_index-1)*z+1:column_index*z) = sparse(1:z, [(mod(A_prime(row_index, column_index), z)+1):z, 1:mod(A_prime(row_index, column_index), z)], ones(1, z), z, z);
        end
    end
end

B = spalloc(a*z, a*z, nnz(B_prime + ones(size(B_prime))));

for row_index = 1:a
    for column_index = 1:a
        if B_prime(row_index, column_index) ~= -1
            B((row_index-1)*z+1:row_index*z, (column_index-1)*z+1:column_index*z) = sparse(1:z, [(mod(B_prime(row_index, column_index), z)+1):z, 1:mod(B_prime(row_index, column_index), z)], ones(1, z), z, z);
        end
    end
end

C = spalloc(d*z, b*z, nnz(C_prime + ones(size(C_prime))));

for row_index = 1:d
    for column_index = 1:b
        if C_prime(row_index, column_index) ~= -1
            C((row_index-1)*z+1:row_index*z, (column_index-1)*z+1:column_index*z) = sparse(1:z, [(mod(C_prime(row_index, column_index), z)+1):z, 1:mod(C_prime(row_index, column_index), z)], ones(1, z), z, z);
        end
    end
end

D = spalloc(d*z, a*z, nnz(D_prime + ones(size(D_prime))));

for row_index = 1:d
    for column_index = 1:a
        if D_prime(row_index, column_index) ~= -1
            D((row_index-1)*z+1:row_index*z, (column_index-1)*z+1:column_index*z) = sparse(1:z, [(mod(D_prime(row_index, column_index), z)+1):z, 1:mod(D_prime(row_index, column_index), z)], ones(1, z), z, z);
        else
            D((row_index-1)*z+1:row_index*z, (column_index-1)*z+1:column_index*z) = spalloc(z, z, 0);
        end
    end
end

B_inv = spalloc(a*z, a*z, 20*z);

if (base_graph_index == 1) && (set_index ~= 7)
    B_inv(1:z, 1:z)             = speye(z);
    B_inv(1:z, 1+z:2*z)         = speye(z);
    B_inv(1:z, 1+2*z:3*z)       = speye(z);
    B_inv(1:z, 1+3*z:4*z)       = speye(z);
    B_inv(1+z:2*z, 1:z)         = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+z:2*z)     = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+2*z:3*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+3*z:4*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1:z)       = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+z:2*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+2*z:3*z) = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+3*z:4*z) = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1:z)       = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+z:2*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+2*z:3*z) = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+3*z:4*z) = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
elseif (base_graph_index == 2) && ((set_index ~= 4) && (set_index ~= 8))
    B_inv(1:z, 1:z)             = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1:z, 1+z:2*z)         = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1:z, 1+2*z:3*z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1:z, 1+3*z:4*z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1:z)         = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+z:2*z)     = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+2*z:3*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+3*z:4*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1:z)       = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+z:2*z)   = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+2*z:3*z) = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+3*z:4*z) = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1:z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+z:2*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+2*z:3*z) = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+3*z:4*z) = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
elseif (base_graph_index == 1) && (z == 208)
    B_inv(1:z, 1:z)             = sparse(circshift(eye(208), 105));
    B_inv(1:z, 1+z:2*z)         = sparse(circshift(eye(208), 105));
    B_inv(1:z, 1+2*z:3*z)       = sparse(circshift(eye(208), 105));
    B_inv(1:z, 1+3*z:4*z)       = sparse(circshift(eye(208), 105));
    B_inv(1+z:2*z, 1:z)         = speye(208) + sparse(circshift(eye(208), 105));
    B_inv(1+z:2*z, 1+z:2*z)     = sparse(circshift(eye(208), 105));
    B_inv(1+z:2*z, 1+2*z:3*z)   = sparse(circshift(eye(208), 105));
    B_inv(1+z:2*z, 1+3*z:4*z)   = sparse(circshift(eye(208), 105));
    B_inv(1+2*z:3*z, 1:z)       = sparse(circshift(eye(208), 105));
    B_inv(1+2*z:3*z, 1+z:2*z)   = sparse(circshift(eye(208), 105));
    B_inv(1+2*z:3*z, 1+2*z:3*z) = speye(208) + sparse(circshift(eye(208), 105));
    B_inv(1+2*z:3*z, 1+3*z:4*z) = speye(208) + sparse(circshift(eye(208), 105));
    B_inv(1+3*z:4*z, 1:z)       = sparse(circshift(eye(208), 105));
    B_inv(1+3*z:4*z, 1+z:2*z)   = sparse(circshift(eye(208), 105));
    B_inv(1+3*z:4*z, 1+2*z:3*z) = sparse(circshift(eye(208), 105));
    B_inv(1+3*z:4*z, 1+3*z:4*z) = speye(208) + sparse(circshift(eye(208), 105));
elseif (base_graph_index == 1) && ((z ~= 208) && (set_index == 7))
    B_inv(1:z, 1:z)             = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1:z, 1+z:2*z)         = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1:z, 1+2*z:3*z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1:z, 1+3*z:4*z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1:z)         = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+z:2*z)     = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+2*z:3*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+3*z:4*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1:z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+z:2*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+2*z:3*z) = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+3*z:4*z) = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1:z)       = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+z:2*z)   = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+2*z:3*z) = sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+3*z:4*z) = speye(z) + sparse(1:z, [z, 1:(z-1)], ones(1, z), z, z);
elseif (base_graph_index == 2) && ((set_index == 4) || (set_index == 8))
    B_inv(1:z, 1:z)             = speye(z);
    B_inv(1:z, 1+z:2*z)         = speye(z);
    B_inv(1:z, 1+2*z:3*z)       = speye(z);
    B_inv(1:z, 1+3*z:4*z)       = speye(z);
    B_inv(1+z:2*z, 1:z)         = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+z:2*z)     = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+2*z:3*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+z:2*z, 1+3*z:4*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1:z)       = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+z:2*z)   = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+2*z:3*z) = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+2*z:3*z, 1+3*z:4*z) = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1:z)       = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+z:2*z)   = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+2*z:3*z) = sparse(1:z, [2:z, 1], ones(1, z), z, z);
    B_inv(1+3*z:4*z, 1+3*z:4*z) = speye(z) + sparse(1:z, [2:z, 1], ones(1, z), z, z);
end

s = s(:);

p_1 = mod(B_inv * (A * s), 2);
p_2 = mod(C * s + D * p_1, 2);

w = [p_1; p_2];

for k = K:(N+2*Z_c-1)
   encoded_bits(k-2*Z_c+1) = w(k-K+1);
end

H = [A, B, spalloc(a*z, d*z, 0); C, D, speye(d*z)];

encoded_bits_original = [s; w];

% mod(H * [s; p_1; p_2]) = 0

clear A
clear B
clear C
clear D
clear B_inv
clear A_prime
clear B_prime
clear C_prime
clear D_prime

end


function [ftAllNew,transMdl] = ftTrans_mida(ftAll,domainFtAll,target,...
	maLabeled,param)
%Maximum Independence Domain Adaptation (MIDA)
%
%	A feaure-level transfer learning (domain adaptation) algorithm which
% augments the features then learns a domain-invariant subspace.
% Application scope:
%	+ two or multiple discrete domains
%	+ continuous distributional change
%	+ labeled or unlabeled or partially labeled source domain
%	+ labeled or unlabeled or partially labeled target domain
%	+ label type: classification or regression
%	There is no need to distinguish which domain a sample is from. The domain
% information is contained in the variable domainFtAll, which contains the
% domain features of all samples.
%	Domain features indicate the background of samples. For example, if
% the samples are from md discrete domains, then domainFtAll can be a
% n-by-md matrix, domainFtAll(i,j)=1 if sample i is from the j'th domain, 0
% else. If the samples have a time order and their distribution changes
% continuously along with time, then domainFtAll can be n-by-1 and
% domainFtAll(i) is the "time" of sample i. Multiple background info can be
% integrated into domainFtAll in the similar way. See the ref for details.
%
% ftAll:	All samples in all domains. n-by-m matrix, n is the number of
%	samples, m is the dimension of features.
% domainFtAll:	Domain features of all samples. n-by-md matrix, md is the
%	dimension of domain features.
% target:	When some samples in any domain are labeled, their labels can
%	be provided in this variable to enhance the discriminative power of the
%	the learned features. ntr-by-1 matrix, ntr is the number of labeled
%	samples. Classification problems should use class indices as labels,
%	i.e. target(i)=j if the i'th labeled sample is from the j'th class.
% maLabeled:	Mask for the labeled samples. n-by-1 matrix,
%	maLabeled(i)=true if sample i is labeled, false else.
%
% param: Struct of hyper-parameters, please see the first cell of this
%	program ("default parameters") for details. You can set parameter p to
%	x by setting param.p = x. For parameters that are not set, default
%	values will be used.

% ftAllNew:	All samples in the learned subspace.
% transMdl:	A struct containing the model, transMdl.W is the projection
%	matrix.

% ref: Ke Yan, Lu Kou, and David Zhang, "Domain Adaptation via Maximum
%	Independence of Domain Features," http://arxiv.org/abs/1603.04535
% Copyright 2016 Ke YAN, Tsinghua Univ. http://yanke23.com , xjed09@gmail.com

%% default parameters
isRegress = 0; % 0 for classification problem, 1 for regression
kerName = 'lin'; % kernel name, see the next cell ("kernels")
kerSigma = 10; % kernel parameter, see the next cell ("kernels")
bSmida = true; % 0 for MIDA if no label information is considered for all
	% samples, 1 for semisupervised MIDA (SMIDA) if some labels are
	% considered.
doSample = false; % when there are too many unlabeled data, eigenvalue
	% decomposition can be slow. Setting this variable to true makes the
	% code to sample some unlabeled data.
nSmpRatio = 1; % if doSample=true, the number of unlabeled data to sample
	% will be ceil(ntr*nSmpRatio)
mu = 1; % the weight of the variance term, see the ref
m = 30; % the dimension of the subspace
gamma = .1; % the weight of the supervised term, only useful in SMIDA, see
	% the ref
ftAugType = 1; % feature augmentation, 0: no aug; 1: aug with domainFt;
	% 2: frustratingly easy aug, only for discrete domains, see the ref

defParam % set user-defined hyper-parameters

%% kernels
nm = @(X,p)repmat(sum(X.^2,2),1,p);
linKer = @(X1,X2)X1*X2';
rbfKer = @(X1,X2)exp(-(nm(X1,size(X2,1))+nm(X2,size(X1,1))'-2*X1*X2')/2/kerSigma^2);
lapKer = @(X1,X2)exp(-pdist2(X1,X2)/kerSigma);
polyKer = @(X1,X2)(1+kerSigma*X1*X2').^2;
if strcmpi(kerName,'lin'), kerFun = linKer; % linear kernel
elseif strcmpi(kerName,'poly'), kerFun = polyKer; % polynomial kernel
elseif strcmpi(kerName,'rbf'), kerFun = rbfKer;
elseif strcmpi(kerName,'lap'), kerFun = lapKer; % Laplacian kernel
else error('unknown kernel'); end

%% sort samples
ntr = nnz(maLabeled);
nAll = size(ftAll,1);
Df = double(domainFtAll);
% Df = zscore(domainFtAll); % bad
if doSample && nSmpRatio<inf % sample some unlabeled data
	rng(0)
	nUnlabeledUsed = min(nAll-ntr,ceil(ntr*nSmpRatio));
	id = randperm(nAll-ntr,nUnlabeledUsed); % rand sel
	idTest = find(~maLabeled);
	ftUsed = [ftAll(maLabeled,:);ftAll(idTest(id),:)];
	dfUsed = [Df(maLabeled,:);Df(idTest(id),:)];
	nUsed = ntr+nUnlabeledUsed;
else
	ftUsed = [ftAll(maLabeled,:);ftAll(~maLabeled,:)];
	dfUsed = [Df(maLabeled,:);Df(~maLabeled,:)];
	nUsed = nAll;
end
Kd = linKer(dfUsed,dfUsed);

%% feature augmentation
if ftAugType==0
	ftUsedAug = ftUsed;
elseif ftAugType==1
	ftUsedAug = [ftUsed,dfUsed];
elseif ftAugType==2
	nFt = size(ftUsed,2);
	nDomain = size(dfUsed,2);
	ftUsedAug = zeros(nUsed,nFt*(nDomain+1));
	ftUsedAug(:,1:nFt) = ftUsed;
	for p = 1:nDomain
		ftUsedAug(dfUsed(:,p)==1,nFt*p+1:nFt*(p+1)) = ftUsed(dfUsed(:,p)==1,:);
	end
end

%% compute
Kx = kerFun(ftUsedAug,ftUsedAug);
H = eye(nUsed)-ones(nUsed)/(nUsed);

if ~bSmida % MIDA

	A = Kx*(mu*H-H*Kd*H)*Kx;

else % SMIDA

	target = double(target);
	if isRegress==0 && all(target==floor(target)) % classification
		Ky = repmat(target,1,ntr)==repmat(target,1,ntr)';
% 		Ky = (Kyy-.5)*2; % no need
	else % regression
		target = zscore(target);
		Ky = target*target'; % linear ker
	end

	Kyy_tilde = zeros(nUsed);
	Kyy_tilde(1:ntr,1:ntr) = Ky;

	A = Kx*(H*(-Kd+gamma*Kyy_tilde)*H+mu*H)*Kx;
end

A = (A+A')/2; % to compensate float num error
[V,D] = eig(A);
[D,I] = sort(diag(real(D)),'descend');
transMdl.W = real(V(:,I(1:m)));

%% project the samples
if ftAugType==0
	ftAllAug = ftAll;
elseif ftAugType==1
	ftAllAug = [ftAll,Df];
elseif ftAugType==2
	ftAllAug = zeros(nUsed,nFt*(nDomain+1));
	ftAllAug(:,1:nFt) = ftAll;
	for p = 1:nDomain
		ftAllAug(Df(:,p)==1,nFt*p+1:nFt*(p+1)) = ftAll(Df(:,p)==1,:);
	end
end

KxAll = kerFun(ftAllAug,ftUsedAug);
ftAllNew = KxAll*transMdl.W;

end


function [ahm, AHM_rf, AHM_sf, AHM_sk, AHM_sc, AHM_u, AHM_rho] = ...
    retroProjAhmPntFromOmniCamOnRob(Rf, Sf, Sk, Sc, u, n)

% RETROPROJAHMPNTFROMOMNICAMONROB Retro-project ahm from omnicam on robot.
%
%   AHM = RETROPROJAHMPNTINTOOMNICAMONROB(RF, SF, SK, SC, U, N) gives the
%   retroprojected AHM in World Frame from an observed pixel U. RF and SF
%   are Robot and Sensor Frames, Sk and Sc are camera calibration and
%   distortion correction parameters. U is the pixel coordinate and N is
%   the non-observable inverse depth. AHM is a 7-vector :
%     AHM = [X Y Z U V W IDepth]'
%
%   [AHM, AHM_rf, AHM_sf, AHM_k, AHM_c, AHM_u, AHM_n] = ... returns the
%   Jacobians wrt RF.x, SF.x, SK, SC, U and N.
%
%   See also FROMFRAMEAHM.
%

%   Copyright 2012 Grigory Abuladze @ ASL-vision
%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

% Frame World -> Robot  :  Rf
% Frame Robot -> Sensor :  Sf

  % AHM in Sensor Frame
  [ahms, AHMS_u, AHMS_rho, AHMS_sk, AHMS_sc] = invOmniCamAhm(u, n, Sk, Sc) ;

  [ahmr, AHMR_sf, AHMR_ahms] = fromFrameAhm(Sf,ahms);
  [ahm , AHM_rf , AHM_ahmr]  = fromFrameAhm(Rf,ahmr);

  AHM_ahms = AHM_ahmr*AHMR_ahms;
  AHM_sk   = AHM_ahms*AHMS_sk ;
  AHM_sf   = AHM_ahmr*AHMR_sf;

  AHM_sc  = AHM_ahms*AHMS_sc ;

  AHM_u   = AHM_ahms*AHMS_u ;
  AHM_rho = AHM_ahms*AHMS_rho ;

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function tr=tau_rest(veh,p)

% tr=tau_rest(veh,p); calculates restoring forces from
% vehicle variables and generalized position p

% Hydrostatic force and moment
FB_e=-veh.vol*veh.rho*veh.g_e;
FB_b=rpy2R_eb(p(4:6))*FB_e;
MB_b=vp(veh.B_b,FB_b);
tb=[FB_b;MB_b];

% Gravitational force and moment
FG_e=veh.m*veh.g_e;
FG_b=rpy2R_eb(p(4:6))*FG_e;
MG_b=vp(veh.G_b,FG_b);
tg=[FG_b;MG_b];

tr=tb+tg;