function [Hdraw,HvarsDraw,phi_Hdraw,Adraw,h0]=sampleH_MHstep(yData,Psi,A,B,phi_H,HvarsOld,priorValues,dataValues,n,h0)

%yData=YData
%Psi=PsiDraw_prop
%A=Adraw
%B=Bdraw
%phi_H=phi_Hdraw'
%HvarsOld=HvarsDraw
%% Initialize
offset_c=priorValues.offset_c; %constant for log transformation

[T,M]=size(yData);
p = size(B,1)/M; %lags of the B vector that has dimensions MxM*p (no constant), each column is one regression

% obtain prior data
startMeanVector=priorValues.mean_ln_h0;  %log mean of the initial state (variance scaling parameters) as residuals from an AR(4) in the training sample
startVarVector=priorValues.var_ln_h0;    %variance of the initial state for the elements of lambda

% obtain prior data
b0=priorValues.mean_ln_h0;  %log mean of the initial state (variance scaling parameters) as residuals from an AR(4) in the training sample
a0=eye(M)/priorValues.var_ln_h0(1,1);    %variance of the initial state for the elements of lambda

priorPhi_H=priorValues.phi_h;            %centering parameter for the inverse gamma of the variance of the innovations governing the random walk for lambda
priorD_H  =priorValues.d_h;              %scaling parameter for the inverse gamma distribution for the variance of the innovations governing the random walk for lambda

kappa= priorValues.kappa;
gamma= 1; %priorValues.gamma;

S_h = priorPhi_H*ones(n,1);
nu_h = priorD_H*ones(n,1);

Y_Psi=yData-Psi;  %subtract the local mean
%Y_Psi(1:p,:)=yData(1:p,:)-ones(p,1)*mean(yData(1:p,:)); %also generate initial conditions for the construction of the lagmatrix
Y_Psi(1:p,:)=yData(1:p,:)-ones(p,1)*(Psi(p+1,:)); %also generate initial conditions for the construction of the lagmatrix

% X_Psi = lagmatrix(Y_Psi,1:p); %create RHS of the VAR part
% X_Psi = X_Psi(p+1:end,:);     %remove the first p rows of RHS
X_Psi = bear.lagx(Y_Psi,p-1);
X_Psi = X_Psi(1:end-1,:);
Y_Psi=Y_Psi(p+1:end,:);       %and do so for LHS

E=Y_Psi-X_Psi*B;              %VAR residuals
Escaled=E*A';                 %transform such that VAR residuals have variance Lambda

   for jj=1:T-p
   epst(:,:,jj)=E(jj,:)'; %generate period specific errors
   end

   scaling = ones(n,1);
   Abelowdiag = cell(n,1);
   for kk=2:size(E,2)
       Abelowdiag{kk,1} = A(kk,1:kk-1)';
   end

  %logHvarsOld = HvarsOld;
   logHvarsOld = log(HvarsOld);
   logHdraw = logHvarsOld(p+1:end,:);
%% draw the series lambda_i,t from their conditional posteriors, i=1,...,n and t=1,...,T

% consider variables in turn
   for jj=1:n
      % consider periods in turn
      for kk=p+1:T
      % a candidate value will be drawn from N(lambdabar,phibar)
      % the definitions of lambdabar and phibar varies with the period, thus define them first
         % if the period is the first period
         if kk==p+1
         %lambdabar=(gamma/(1+gamma^2))*((startMeanVector(jj,1))+logHvarsOld(kk+1,jj));
         lambdabar=(gamma*(h0(jj,1)+logHvarsOld(kk+1,jj)))/(1/kappa+gamma^2);
         phibar=phi_H(1,jj)/(1/kappa+gamma^2);
         %phibar=startVarVector(jj,1)+phi_H(1,jj)/(1/kappa+gamma^2);
         %if the period is the final period
         elseif kk==T
         lambdabar=gamma*logHdraw(end-1,jj);
         phibar=phi_H(1,jj);
         % if the period is any period in-between
         else
         lambdabar=(gamma/(1+gamma^2))*(logHdraw(kk-p-1,jj)+logHvarsOld(kk+1,jj));
         phibar=phi_H(1,jj)/(1+gamma^2);
         end
      % now draw the candidate
      cand=lambdabar+phibar^0.5*randn;

%       end
      % compute the acceptance probability
      prob=bear.mhprob2(jj,cand,logHvarsOld(kk,jj),scaling(jj,1),epst(:,1,kk-p),Abelowdiag{jj,1});
      % draw a uniform random number
      draw=rand;
         % keep the candidate if the draw value is lower than the prob
         if draw<=prob
         logHdraw(kk-p,jj)=cand;
%          else
%          logHdraw(kk-p,jj)=logHvarsOld(kk,jj);
%          % if not, just keep the former value
          end
      end
   end

G=speye(T-p)-sparse(diag(gamma*ones(T-p-1,1),-1));
% set the value for omega
omega=10;
% compute I_omega
I_o=sparse(diag([1/kappa;ones(T-p-1,1)]));
GIG=G'*I_o*G;
alpha0=0.01;
alphabar=T-p+alpha0;
delta0=0.01;

% draw the parameters in turn
   for jj=1:n
   % estimate deltabar
   deltabar=logHdraw(1:end,jj)'*GIG*logHdraw(1:end,jj)+delta0;
   % draw the value phi_i
   phi_Hdraw(jj)=bear.igrandn(alphabar/2,deltabar/2);
   end

   %% draw A
   Hvars=exp([zeros(n,p) logHdraw']');

   [Adraw]=bear.sampleA_H(yData,Psi,B,Hvars,T,priorValues,dataValues);
   AdrawInv=Adraw\eye(n);

   %% sample the initial value
Kh0 = a0 + sparse(1:n,1:n,1./phi_Hdraw');
h0_hat = Kh0\(a0*b0 + log(Hvars(p+1,:)')./phi_Hdraw');
h0 = h0_hat + chol(Kh0,'lower')'\randn(n,1);

   %% Construct H
Hdraw=zeros(n,n,T);

for t=1:T
    Hdraw(:,:,t)=AdrawInv*diag(Hvars(t,:))*AdrawInv';
end

HvarsDraw=Hvars;


function [ y, m, d, f ] = jed_to_ymdf_alexandrian ( jed )

%*****************************************************************************80
%
%% JED_TO_YMDF_ALEXANDRIAN converts a JED to an Alexandrian YMDF date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 July 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, integer Y, M, D, real F,
%    the YMDF date.
%

%
%  Determine the computational date (Y'/M'/D').
%
  j = floor ( jed + 0.5 );
  f = ( jed + 0.5 ) - j;

  j_prime = j + 124;

  y_prime = floor ( ( 4 * j_prime + 3 ) / 1461 );
  t_prime = floor ( mod ( 4 * j_prime + 3, 1461 ) / 4 );
  m_prime = floor ( t_prime / 30 );
  d_prime = mod ( t_prime, 30 );
%
%  Convert the computational date to a calendar date.
%
  d = d_prime + 1;
  m = mod ( m_prime, 13 ) + 1;
  y = y_prime - 4690 + floor ( ( 13 - m ) / 13 );

  return
end


function [Pro,FreeNodec] = nodeisoP2TansferUniform(elemc,elemf,FreeNode)
%  Only works for uniformrefinered.
%
%
% Created by Jie Zhou,Jan,17,2013.
% Talk with Long Chen
%% Data structure.
%% Data structure.
if ~exist('FreeNode','var'), FreeNode = []; end
[elem2dofc,edgec] = dofP2(elemc);
[elem2doff,edgef] = dofP2(elemf);

Nc = max(elemc(:));  Nf = max(elemf(:));
Ndofc = Nc + size(edgec,1);Ndoff = Nf + size(edgef,1);
NTc = size(elemc,1);  %NTc:coarse elements number.
%NTf = size(elemf,1);

elem2node2f = sparse(NTc,9); %very important, map from coarse space to fine space
%we just consider the middle points in fine edges.
elem2node2f(:,1)=elem2doff(1:NTc,4);
elem2node2f(:,2)=elem2doff(1:NTc,5);
elem2node2f(:,3)=elem2doff(1:NTc,6);

elem2node2f(:,4)=elem2doff(NTc+1:2*NTc,4);
elem2node2f(:,5)=elem2doff(NTc+1:2*NTc,5);
elem2node2f(:,6)=elem2doff(NTc+1:2*NTc,6);

elem2node2f(:,7)=elem2doff(2*NTc+1:3*NTc,4);
elem2node2f(:,8)=elem2doff(2*NTc+1:3*NTc,5);
elem2node2f(:,9)=elem2doff(2*NTc+1:3*NTc,6);

%% Assembel the transfer matrix
%        1       2         3        4       5       6         7           8          9
c0=1/2;
locPij=[ 0    1/2*c0    1/2*c0      0        0        0          0         0        0;...
         0    0         0         1/2*c0     0       1/2*c0      0         0        0;...
         0    0         0           0        0        0         1/2*c0     1/2*c0   0;...
         0    0         0         1/2*c0     1/2      0         1/2*c0     0        1/2; ...
         1/2  1/2*c0    0           0        0        0          0         1/2*c0   1/2; ...
         1/2  0         1/2*c0      0        1/2      1/2*c0     0         0        0];
%%Corase grid order          fine grid order
% 3                          *
% * *                        *  7
% *   *                      8    *
% *     *                    *      *
% 5       4                  ***9******
% *         *                *      *    4
% *           *              2  1   5      *
% *             *            *      *        *
% 1*****6********2           ***3********6*****

ii = zeros(40*NTc,1); jj = zeros(40*NTc,1); ss = zeros(40*NTc,1);
index = 0;
for i = 1:6
    for j = 1:9
        if locPij(i,j)~=0
            %Pij = locPij(i,j)*elem2edgeSignc(:,i).*elem2edgesignc2f(:,j);
            ii(index+1:index+NTc) =  elem2dofc(:,i);
            jj(index+1:index+NTc) =  elem2node2f(:,j);
            ss(index+1:index+NTc) =  locPij(i,j);
            index = index + NTc;
        end
    end
end
ii(ii==0)=[];
jj(jj==0)=[];
ss(ss==0)=[];

% Modification for boundary middle points.
s = accumarray([elem2doff(:,4);elem2doff(:,5);elem2doff(:,6)],1,[Ndoff 1]);
bdEdgeidxf = (s == 1);
bdEdgeidxfinjj = bdEdgeidxf(jj);
ss(bdEdgeidxfinjj) = 2*ss(bdEdgeidxfinjj);
ii = [(1:Ndofc)'; ii];
jj = [(1:Ndofc)'; jj];
ss = [ones(Ndofc,1); ss];
Pro     =  sparse(jj,ii,ss,Ndoff,Ndofc);
% Pro(1:Ndofc,1:Ndofc) = speye(Ndofc,Ndofc);
% The transfer for  is a identical matrix, so the local matrix is 6 by 9.

%% Truncated to free nodes
if ~isempty(FreeNode)
    FreeNodec = FreeNode(FreeNode<=Ndofc);
    Pro = Pro(FreeNode,FreeNodec);
end


function outdata = jatmos70(indata)

% Jacchia 1970 atmosphere main driver

% input

%  indata(1)  = geodetic altitude (kilometers)
%  indata(2)  = geodetic latitude (radians)
%  indata(3)  = geographic longitude (radians)
%  indata(4)  = calendar year (all digits)
%  indata(5)  = calendar month
%  indata(6)  = calendar day
%  indata(7)  = utc hours
%  indata(8)  = utc minutes
%  indata(9)  = geomagnetic index type
%               (1 = indata(12) is Kp, 2 = indata(12) is Ap)
%  indata(10) = solar radio noise flux (jansky)
%  indata(11) = 162-day average F10 (jansky)
%  indata(12) = geomagnetic activity index

% output

%  outdata(1)  = exospheric temperature (deg K)
%  outdata(2)  = temperature at altitude (deg K)
%  outdata(3)  = N2 number density (per meter-cubed)
%  outdata(4)  = O2 number density (per meter-cubed)
%  outdata(5)  = O number density (per meter-cubed)
%  outdata(6)  = A number density (per meter-cubed)
%  outdata(7)  = He number density (per meter-cubed)
%  outdata(8)  = H number density (per meter-cubed)
%  outdata(9)  = average molecular weight
%  outdata(10) = total density (kilogram/meter-cubed)
%  outdata(11) = log10(total density)
%  outdata(12) = total pressure (pascals)

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bfh = 440;

rgas = 8314.32;

% unload input data

z = indata(1);
xlat = indata(2);
xlng = indata(3);
iyr = indata(4);
mn = indata(5);
ida = indata(6);
ihr = indata(7);
min = indata(8);
i1 = indata(9);
f10 = indata(10);
f10b = indata(11);
gi = indata(12);

% compute solar coordinates

[sda, sha, dd, dy] = jtme(mn, ida, iyr, ihr, min, xlng);

% calculate exospheric temperature

te = jtinf(f10, f10b, gi, xlat, sda, sha, dy, i1);

% evaluate jacchia model

[dens, dl, em, tz, a(1), a(2), a(3), a(4), a(5), a(6)] = jacchia(z, te);

denlg = 0;
dummy = dl;
den = dl;

if (z <= 170)
   dummy = jslv(z, xlat, dd);
   denlg = dummy;
end

if (z >= 500)
    den = jslvh(den, a(5), xlat, sda);
    dl = den;
elseif (z > bfh)
    dhel1 = a(5);
    dhel2 = a(5);
    dlg1 = dl;
    dlg2 = dl;
    dlg2 = jslvh(dlg2, dhel2, xlat, sda);
    ih = z;
    [fdhel, fdlg] = jfair(dhel1, dhel2, dlg1, dlg2, ih);
    dl = fdlg;
    a(5) = fdhel;
end

dl = dl + denlg;

dens = 10 ^ dl;

% load output data array

outdata(1) = te;

outdata(2) = tz;

for i = 1:1:6
    outdata(i + 2) = 1000000 * (10 ^ a(i));
end

outdata(9) = em;
outdata(10) = dens * 1000;
outdata(11) = dl;

p = outdata(10) * rgas * tz / em;

outdata(12) = p;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [dens, dl, em, tz, an, ao2, ao, aa, ahe, ah] = jacchia (z, t)

% jacchia 1970 atmosphere

global alpha ei

av = 6.02257e+23;
qn = 0.7811;
qo2 = 0.20955;
qa = 9.343e-03;
qhe = 0.00001289;
rgas = 8.31432;
t0 = 183;

tx = 444.3807 + 0.02385 * t - 392.8292 * exp(-0.0021357 * t);
a2 = 2 * (t - tx) / pi;
txt0 = tx - t0;
t1 = 1.9 * txt0 / 35;
t3 = -1.7 * txt0 / (35 ^ 3);
t4 = -.8 * txt0 / (35 ^ 4);
tz = jtemp(z, tx, t1, t3, t4, a2);

a = 90;
d = min(z, 105);

r = jgauss(a, d, 1, tx, t1, t3, t4, a2);

em = jmweight(d);

td = jtemp(d, tx, t1, t3, t4, a2);

dens = 0.000000021926 * em * exp(-r / rgas) / td;

factor = av * dens;
par = factor / em;
factor = factor / 28.96;

if (z <= 105)
   dl = log10(dens);
   an = log10(qn * factor);
   aa = log10(qa * factor);
   ahe = log10(qhe * factor);
   ao = log10(2 * par * (1 - em / 28.96));
   ao2 = log10(par * (em * (1 + qo2) / 28.96 - 1));
   ah = 0;
   return;
end

di(1) = qn * factor;
di(2) = par * (em * (1 + qo2) / 28.96 - 1);
di(3) = 2 * par * (1 - em / 28.96);
di(4) = qa * factor;
di(5) = qhe * factor;

r = jgauss(d, z, 2, tx, t1, t3, t4, a2);

for i = 1:1:5
    dit(i) = di(i) * (td / tz) ^ (1 + alpha(i)) * exp(-ei(i) * r / rgas);

    if (dit(i) <= 0)
       dit(i) = 0.000001;
    end
end

if (z > 500)
   a1 = 500;
   s = jtemp(a1, tx, t1, t3, t4, a2);
   di(6) = 10 ^ (73.13 - 39.4 * log10(s) + 5.5 * log10(s) * log10(s));
   r = jgauss(a1, z, 7, tx, t1, t3, t4, a2);
   dit(6) = di(6) * (s / tz) * exp(-ei(6) * r / rgas);
else
   dit(6) = 1;
end

dens = 0;

for i = 1:1:6
    dens = dens + ei(i) * dit(i) / av;
end

em = dens * av / (dit(1) + dit(2) + dit(3) + dit(4) + dit(5) + dit(6));

dl = log10(dens);

an = log10(dit(1));
ao2 = log10(dit(2));
ao = log10(dit(3));
aa = log10(dit(4));
ahe = log10(dit(5));
ah = log10(dit(6));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [sda, sha, dd, dy] = jtme (mn, ida, iyr, ihr, min, xlng)

% solar declination and hour angle

dtr = pi / 180;

atr = dtr / 3600;

% compute julian date on january 1

jdate = julian(1, 1, iyr);

% compute julian date on day of interest

xmjd = julian(mn, ida, iyr);

% compute day number of the year

dd = xmjd - jdate;

% compute fraction of tropical year

dy = dd / 365.2422;

% compute greenwich mean time

gmt = 60 * ihr + min;

% compute solar coordinates

a = xmjd + gmt / 1440 - 2451545;

b = a / 36525 + 1;

o = r2r(0.056531 + 0.00023080893 * a);
m = r2r(0.140023 + 0.00445036173 * a);
l = r2r(0.779072 + 0.00273790931 * a);
h = r2r(0.606434 + 0.0366011013 * a);
n = r2r(0.053856 + 0.00145561327 * a);
f = r2r(0.993126 + 0.0027377785 * a);
r = r2r(0.347343 - 0.00014709391 * a);

obliq = atr * (84428 - 47 * b + 9 * cos(r));

p = 6910 * sin(f) + 72 * sin(2 * f) - 17 * b * sin(f) - 7 * cos(f - o) ...
    + 6 * sin(h - l) + 5 * sin(4 * f - 8 * n + 3 * o) ...
    - 5 * cos(2 * (f - m)) - 4 * sin(f - m) + 4 * cos(4 * f - 8 * n ...
    + 3 * o) + 3 * sin(2 * (f - m)) - 3 * sin(o) - 3 * sin(2 * (f - o));

p = l + atr * (p - 17 * sin(r));

a = sin(p) * cos(obliq);

b = cos(p);

% compute solar right ascension and declination

ras = atan3(a, b);

sda = asin(sin(obliq) * sin(p));

% compute greenwich sidereal time

gst = gast1(xmjd + ihr / 24 + min / 1440);

% compute right ascension of point of interest

rap = mod(gst + xlng, 2.0 * pi);

% compute hour angle

sha = mod(rap - ras, 2.0 * pi);

if (sha > pi)
   sha = sha - 2.0 * pi;
end

if (sha < -pi)
   sha = sha + 2.0 * pi;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function te = jtinf (f10, f10b, gi, xlat, sda, sha, dy, i1)

% exospheric temperature

c1 = 383;
c2 = 3.32;
c3 = 1.8;

d1 = 28;
d2 = 0.03;
d3 = 1;
d4 = 100;
d5 = -0.08;

e1 = 2.41;
e2 = 0.349;
e3 = 0.206;
e4 = 6.2831853;
e5 = 3.9531708;
e6 = 12.5663706;
e7 = 4.3214352;
e8 = 0.1145;
e9 = 0.5;
e10 = 6.2831853;
e11 = 5.974262;
e12 = 2.16;

beta = -0.6457718;
gamma = 0.7504916;
p = 0.1047198;
re = 0.31;

tc = c1 + c2 * f10b + c3 * (f10 - f10b);

eta = 0.5 * abs(xlat - sda);
theta = 0.5 * abs(xlat + sda);
tau = sha + beta + p * sin(sha + gamma);

if (tau > pi)
   tau = tau - 2 * pi;
end

if (tau < -pi)
   tau = tau + 2 * pi;
end

a1 = (sin(theta)) ^ 2.5;
a2 = (cos(eta)) ^ 2.5;
a3 = (cos(tau / 2)) ^ 3;
b1 = 1 + re * a1;
b2 = (a2 - a1) / b1;
tv = b1 * (1 + re * b2 * a3);
tl = tc * tv;

if (i1 == 1)
   tg = d1 * gi + d2 * exp(gi);
else
   tg = d3 * gi + d4 * (1 - exp(d5 * gi));
end

g3 = 0.5 * (1 + sin(e10 * dy + e11));

g3 = g3 ^ e12;

tau1 = dy + e8 * (g3 - e9);

g1 = e2 + e3 * (sin(e4 * tau1 + e5));

g2 = sin(e6 * tau1 + e7);

ts = e1 + f10b * g1 * g2;

te = tl + tg + ts;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function den = jslv (alt, xlat, day)

den = 0;

if (alt > 170)
   return;
end

z = alt - 90;
x = -0.0013 * z * z;
y = 0.0172 * day + 1.72;
p = sin(y);
sp = (sin(xlat)) ^ 2;
s = 0.014 * z * exp(x);
d = s * p * sp;

if (xlat < 0)
   d = -d;
end

den = d;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function den = jslvh (den, denhe, xlat, sda)

ezero = 10 ^ denhe;

a = abs(0.65 * (sda / 0.40909079));

b = 0.5 * xlat;

if (sda < 0)
   b = -b;
end

x = 0.7854 - b;

y = sin(x) ^ 3;

dhe = a * (y - 0.35356);

denhe = denhe + dhe;

d1 = 10 ^ denhe;

del = d1 - ezero;

rho = 10 ^ den;

drho = 6.646e-24 * del;

rho = rho + drho;

den = log10(rho);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [fdhel, fdlg] = jfair (dhel1, dhel2, dlg1, dlg2, ih)

% fair density

global cz

bfh = 440;

i = fix((ih - bfh) / 10) + 1;

czi = cz(i);
szi = 1 - czi;

fdlg = (dlg1 * czi) + (dlg2 * szi);

fdhel = (dhel1 * czi) + (dhel2 * szi);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function r = jgauss (z1, z2, nmin, tx, t1, t3, t4, a2)

% gaussian quadrature

global ng xgauss cgauss altmin

r = 0;

for k = nmin:1:8
    ngauss = ng(k);
    a = altmin(k);
    d = min(z2, altmin(k + 1));
    rr = 0;
    del = 0.5 * (d - a);
    j = ngauss - 2;

    for i = 1:1:ngauss
        z = del * (xgauss(i, j) + 1) + a;

        rr = rr + cgauss(i, j) * jmweight(z) * jgrav(z) ...
             / jtemp(z, tx, t1, t3, t4, a2);
    end

    rr = del * rr;
    r = r + rr;

    if (d == z2)
       break;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function agrav = jgrav(altitude)

% acceleration of gravity

agrav = 9.80665 / ((1 + altitude / 6356.766) ^ 2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function mweight = jmweight (a)

% molecular weight function

global bdata

if (a > 105)
   mweight = 1;
else
   u = a - 100;

   wttmp = bdata(1);

   for i = 2:1:7
       wttmp = wttmp + bdata(i) * u ^ (i - 1);
   end

   mweight = wttmp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function temp = jtemp (alt, tx, t1, t3, t4, a2)

% temperature function

bb = 0.0000045;

u = alt - 125.0;

if (u > 0)
   temp = tx + a2 * atan(t1 * u * (1 + bb * (u ^ 2.5)) / a2);
else
   temp = tx + t1 * u + t3 * (u ^ 3) + t4 * (u ^ 4);
end


function [ qy, qty, b, rsd, ab, info ] = dqrsl ( a, lda, n, k, qraux, y, job )

%*****************************************************************************80
%
%% DQRSL computes transformations, projections, and least squares solutions.
%
%  Discussion:
%
%    DQRSL requires the output of DQRDC.
%
%    For K <= min(N,P), let AK be the matrix
%
%      AK = ( A(JPVT(1)), A(JPVT(2)), ..., A(JPVT(K)) )
%
%    formed from columns JPVT(1), ..., JPVT(K) of the original
%    N by P matrix A that was input to DQRDC.  If no pivoting was
%    done, AK consists of the first K columns of A in their
%    original order.  DQRDC produces a factored orthogonal matrix Q
%    and an upper triangular matrix R such that
%
%      AK = Q * (R)
%               (0)
%
%    This information is contained in coded form in the arrays
%    A and QRAUX.
%
%    The parameters QY, QTY, B, RSD, and AB are not referenced
%    if their computation is not requested and in this case
%    can be replaced by dummy variables in the calling program.
%    To save storage, the user may in some cases use the same
%    array for different parameters in the calling sequence.  A
%    frequently occuring example is when one wishes to compute
%    any of B, RSD, or AB and does not need Y or QTY.  In this
%    case one may identify Y, QTY, and one of B, RSD, or AB, while
%    providing separate arrays for anything else that is to be
%    computed.
%
%    Thus the calling sequence
%
%      call dqrsl ( a, lda, n, k, qraux, y, dum, y, b, y, dum, 110, info )
%
%    will result in the computation of B and RSD, with RSD
%    overwriting Y.  More generally, each item in the following
%    list contains groups of permissible identifications for
%    a single calling sequence.
%
%      1. (Y,QTY,B) (RSD) (AB) (QY)
%
%      2. (Y,QTY,RSD) (B) (AB) (QY)
%
%      3. (Y,QTY,AB) (B) (RSD) (QY)
%
%      4. (Y,QY) (QTY,B) (RSD) (AB)
%
%      5. (Y,QY) (QTY,RSD) (B) (AB)
%
%      6. (Y,QY) (QTY,AB) (B) (RSD)
%
%    In any group the value returned in the array allocated to
%    the group corresponds to the last member of the group.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,P), contains the output of DQRDC.
%
%    Input, integer LDA, the leading dimension of the array A.
%
%    Input, integer N, the number of rows of the matrix AK.  It must
%    have the same value as N in DQRDC.
%
%    Input, integer K, the number of columns of the matrix AK.  K
%    must not be greater than min(N,P), where P is the same as in the
%    calling sequence to DQRDC.
%
%    Input, real QRAUX(P), the auxiliary output from DQRDC.
%
%    Input, real Y(N), a vector to be manipulated by DQRSL.
%
%    Input, integer JOB, specifies what is to be computed.  JOB has
%    the decimal expansion ABCDE, with the following meaning:
%      if A /= 0, compute QY.
%      if B /= 0, compute QTY.
%      if C /= 0, compute QTY and B.
%      if D /= 0, compute QTY and RSD.
%      if E /= 0, compute QTY and AB.
%    Note that a request to compute B, RSD, or AB automatically triggers
%    the computation of QTY, for which an array must be provided in the
%    calling sequence.
%
%    Output, real QY(N), contains Q * Y, if requested.
%
%    Output, real QTY(N), contains Q' * Y, if requested.
%
%    Output, real B(K), the solution of the least squares problem
%      minimize norm2 ( Y - AK * B),
%    if its computation has been requested.  Note that if pivoting was
%    requested in DQRDC, the J-th component of B will be associated with
%    column JPVT(J) of the original matrix A that was input into DQRDC.
%
%    Output, real RSD(N), the least squares residual Y - AK * B,
%    if its computation has been requested.  RSD is also the orthogonal
%    projection of Y onto the orthogonal complement of the column space
%    of AK.
%
%    Output, real AB(N), the least squares approximation Ak * B,
%    if its computation has been requested.  AB is also the orthogonal
%    projection of Y onto the column space of A.
%
%    Output, integer INFO, is zero unless the computation of B has
%    been requested and R is exactly singular.  In this case, INFO is the
%    index of the first zero diagonal element of R, and B is left unaltered.
%

%
%  Set info flag.
%
  info = 0;
%
%  Determine what is to be computed.
%
  cqy =  floor       ( job / 10000         ) ~= 0;
  cqty =         mod ( job,  10000 )         ~= 0;
  cb =   floor ( mod ( job,   1000 ) / 100 ) ~= 0;
  cr =   floor ( mod ( job,    100 ) /  10 ) ~= 0;
  cab =          mod ( job,     10 )       ~= 0;

  ju = min ( k, n - 1 );
%
%  Special action when N = 1.
%
  if ( ju == 0 )

    qy(1) = y(1);
    qty(1) = y(1);
    ab(1) = y(1);

    if ( a(1,1) == 0.0 )
      info = 1;
    else
      b(1) = y(1) / a(1,1);
    end

    rsd(1) = 0.0;

    return

  end
%
%  Set up to compute QY or QTY.
%
  qy(1:n) = y(1:n);
  qty(1:n) = y(1:n);
%
%  Compute QY.
%
  if ( cqy )

    for jj = 1 : ju

      j = ju - jj + 1;

      if ( qraux(j) ~= 0.0 )
        temp = a(j,j);
        a(j,j) = qraux(j);
        t = -ddot ( n-j+1, a(j:n,j), 1, qy(j:n), 1 ) / a(j,j);
        qy(j:n) = daxpy ( n-j+1, t, a(j:n,j)', 1, qy(j:n), 1 );
        a(j,j) = temp;
      end

    end

  end
%
%  Compute Q'*Y.
%
  if ( cqty )

    for j = 1 : ju
      if ( qraux(j) ~= 0.0 )
        temp = a(j,j);
        a(j,j) = qraux(j);
        t = -ddot ( n-j+1, a(j:n,j), 1, qty(j:n), 1 ) / a(j,j);
        qty(j:n) = daxpy ( n-j+1, t, a(j:n,j), 1, qty(j:n), 1 );
        a(j,j) = temp;
      end
    end

  end
%
%  Set up to compute B, RSD, or AB.
%
  b(1:k) = qty(1:k);
  ab(1:k) = qty(1:k);
  rsd(k+1:n) = qty(k+1:n);
  ab(k+1:n) = 0.0;
  rsd(1:k) = 0.0;
%
%  Compute B.
%
  if ( cb )

    for jj = 1 : k

      j = k - jj + 1;

      if ( a(j,j) == 0.0 )
        info = j;
        break
      end

      b(j) = b(j) / a(j,j);

      if ( j ~= 1 )
        t = -b(j);
        b(1:j-1) = daxpy ( j-1, t, a(1:j-1,j), 1, b(1:j-1), 1 );
      end

    end

  end
%
%  Compute RSD or AB as required.
%
  if ( cr | cab )

    for jj = 1 : ju

      j = ju - jj + 1;

      if ( qraux(j) ~= 0.0 )

        temp = a(j,j);
        a(j,j) = qraux(j);

        if ( cr )
          rsd(j:n) = -ddot ( n-j+1, a(j:n,j), 1, rsd(j:n), 1 ) / a(j,j);
          rsd(j:n) = daxpy ( n-j+1, t, a(j:n,j), 1, rsd(j:n), 1 );
        end

        if ( cab )
          t = -ddot ( n-j+1, a(j:n,j), 1, ab(j:n), 1 ) / a(j,j);
          ab(j:n) = daxpy ( n-j+1, t, a(j:n,j), 1, ab(j:n), 1 );
        end

        a(j,j) = temp;

      end
    end
  end

  return
end


%MIT IAP Radar Course 2011
%Resource: Build a Small Radar System Capable of Sensing Range, Doppler,
%and Synthetic Aperture Radar Imaging
%
%Gregory L. Charvat

%Process Range vs. Time Intensity (RTI) plot

%NOTE: set up-ramp sweep from 2-3.2V to stay within ISM band
%change fstart and fstop bellow when in ISM band

clear all;
close all;

%read the raw data .wave file here
[Y,FS,NBITS] = wavread('running_outside_20ms.wav');

%constants
c = 3E8; %(m/s) speed of light

%radar parameters
Tp = 20E-3; %(s) pulse time
N = Tp*FS; %# of samples per pulse
fstart = 2260E6; %(Hz) LFM start frequency for example
fstop = 2590E6; %(Hz) LFM stop frequency for example
%fstart = 2402E6; %(Hz) LFM start frequency for ISM band
%fstop = 2495E6; %(Hz) LFM stop frequency for ISM band
BW = fstop-fstart; %(Hz) transmti bandwidth
f = linspace(fstart, fstop, N/2); %instantaneous transmit frequency

%range resolution
rr = c/(2*BW);
max_range = rr*N/2;

%the input appears to be inverted
trig = -1*Y(:,1);
s = -1*Y(:,2);
clear Y;

%parse the data here by triggering off rising edge of sync pulse
count = 0;
thresh = 0;
start = (trig > thresh);
for ii = 100:(size(start,1)-N)
    if start(ii) == 1 & mean(start(ii-11:ii-1)) == 0
        %start2(ii) = 1;
        count = count + 1;
        sif(count,:) = s(ii:ii+N-1);
        time(count) = ii*1/FS;
    end
end
%check to see if triggering works
% plot(trig,'.b');
% hold on;si
% plot(start2,'.r');
% hold off;
% grid on;

%subtract the average
ave = mean(sif,1);
for ii = 1:size(sif,1);
    sif(ii,:) = sif(ii,:) - ave;
end

zpad = 8*N/2;

%RTI plot
figure(10);
v = dbv(ifft(sif,zpad,2));
S = v(:,1:size(v,2)/2);
m = max(max(v));
imagesc(linspace(0,max_range,zpad),time,S-m,[-80, 0]);
colorbar;
ylabel('time (s)');
xlabel('range (m)');
title('RTI without clutter rejection');

%2 pulse cancelor RTI plot
figure(20);
sif2 = sif(2:size(sif,1),:)-sif(1:size(sif,1)-1,:);
v = ifft(sif2,zpad,2);
S=v;
R = linspace(0,max_range,zpad);
for ii = 1:size(S,1)
    %S(ii,:) = S(ii,:).*R.^(3/2); %Optional: magnitude scale to range
end
S = dbv(S(:,1:size(v,2)/2));
m = max(max(S));
imagesc(R,time,S-m,[-80, 0]);
colorbar;
ylabel('time (s)');
xlabel('range (m)');
title('RTI with 2-pulse cancelor clutter rejection');

% %2 pulse mag only cancelor
% figure(30);
% clear v;
% for ii = 1:size(sif,1)-1
%     v1 = abs(ifft(sif(ii,:),zpad));
%     v2 = abs(ifft(sif(ii+1,:),zpad));
%     v(ii,:) = v2-v1;
% end
% S=v;
% R = linspace(0,max_range,zpad);
% for ii = 1:size(S,1)
%     S(ii,:) = S(ii,:).*R.^(3/2); %Optional: magnitude scale to range
% end
% S = dbv(S(:,1:size(v,2)/2));
% m = max(max(S));
% imagesc(R,time,S-m,[-20, 0]);
% colorbar;
% ylabel('time (s)');
% xlabel('range (m)');
% title('RTI with 2-pulse mag only cancelor clutter rejection');


function [varargout] = applyTransM(varargin)
%"applyTransM"
%   Apply transformation matrix transM to points defined in xyz by xV, yV,
%   zV.  In order to run as fast as possible, first computes the rotation
%   component and then adds the translation portion of transM last.
%
%JRA 1/18/04
%
%Usage:
%   function [xT, yT, zT] = applyTransM(transM, xV, yV, zV);
%   function [pointsT]    = applyTransM(transM, pointsM);
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR 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.
%
% CERR 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 CERR.  If not, see <http://www.gnu.org/licenses/>.

%Parse input arguments.
if nargin == 2
    usingPointMatrix = 1;
    transM  = varargin{1};
    pointsM = varargin{2}';
elseif nargin == 4
    usingPointMatrix = 0;
    transM  = varargin{1};
    xV      = varargin{2};
    yV      = varargin{3};
    zV      = varargin{4};

    if length(xV) ~= length(yV) | length(xV) ~= length(zV)
        error('xV, yV, and zV must be vectors of the same length.');
    end

    pointsM = [reshape(xV,[],1), reshape(yV,[],1) reshape(zV,[],1)]';

else
    error('Invalid number of input arguments to applyTransM.');
end

%If blank transformation matrix, return originals.
if isempty(transM)
    if usingPointMatrix
        varargout{1} = pointsM;
    else
        varargout{1} = xV;
        varargout{2} = yV;
        varargout{3} = zV;
    end
    return;
end

nPts = size(pointsM, 2);
%If no points passed in, return empty.
if nPts == 0
    if usingPointMatrix
        varargout{1} = [];
    else
        varargout{1} = [];
        varargout{2} = [];
        varargout{3} = [];
    end
    return;
end

%Split the rotation and translation portions of transM.  This is done for
%speed, to avoid allocating a 4th column of ones to pointsM.

%Rotation.
pointsM = transM(1:3,1:3) * pointsM;

%Translation.
pointsM(1,:) = pointsM(1,:) + transM(1,4);
pointsM(2,:) = pointsM(2,:) + transM(2,4);
pointsM(3,:) = pointsM(3,:) + transM(3,4);

if usingPointMatrix
    varargout{1} = pointsM';
else
    varargout{1} = pointsM(1,:);
    varargout{2} = pointsM(2,:);
    varargout{3} = pointsM(3,:);
end


%   ****** See nozzle.m for instructions ******
%   solver.m

%   Solver for the macormack method
function [Q] = solver(Q)
global gamma R

[u,v,rho,p,e,T,ss,F,G] = flowvars(Q);

%   Take one MacCormack step
[Q] = mac(Q);

end

%%%%%%%%    flowvars    %%%%%%%%
function [u,v,rho,p,e,T,ss,F,G] = flowvars(Q)
global gamma R

%	Calculate the actual flow variables at each time step
rho = Q(:,:,1);
u = Q(:,:,2)./rho;
v = Q(:,:,3)./rho;
e = Q(:,:,4);
p = (gamma-1)*(e-(1/2)*rho.*(u.*u+v.*v));
T = p./(R*rho);
ss = sqrt(abs(gamma*R*T));

F(:,:,1) = rho.*u;
F(:,:,2) = rho.*u.*u + p;
F(:,:,3) = rho.*u.*v;
F(:,:,4) = (e+p).*u;

G(:,:,1) = rho.*v;
G(:,:,2) = rho.*u.*v;
G(:,:,3) = rho.*v.*v + p;
G(:,:,4) = (e+p).*v;

end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%    mac    %%%%%%%%

function [Q] = mac(Q)
global gamma R

Q0 = Q;

%	Forward flux
[Qflux,dt] = flux_mc(Q,-1);
Qbar = Q - dt*Qflux;
Q = Qbar;

[Q] = boundary(Q);

%	Backward flux
[Qflux,dt] = flux_mc(Q,0);
Q = (1/2)*(Q0 + Qbar - dt*Qflux );

[Q] = boundary(Q);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%    flux_mc    %%%%%%%%

function [Qflux,dt] = flux_mc(Q,dd)
global x y Vol cfl

[u,v,rho,p,e,T,ss,F,G] = flowvars(Q);
nx = size(x,1);
ny = size(x,2);

a(1:nx+1,1:ny+1) = 0;
b(1:nx+1,1:ny+1) = 0;
c(1:nx+1,1:ny+1) = 0;
Qflux(1:nx+1,1:ny+1,1:4) = 0;

%	Get the fluxes
for i = 2: size(x,1)
    for j = 2: size(x,2)

	ii = i-1;
    jj = j-1;

    %	Right face
	sfpx = y(ii+1,jj+1)-y(ii+1,jj);
    sfpy = -( x(ii+1,jj+1)-x(ii+1,jj) );

	%	Left face
    sfmx = -( y(ii,jj+1) - y(ii,jj) );
    sfmy = ( x(ii,jj+1)-x(ii,jj) );

    %	Top face
    sgpx = -( y(ii+1,jj+1) - y(ii,jj+1) );
    sgpy = x(ii+1,jj+1) - x(ii,jj+1);

    %	Bottom face
    sgmx = ( y(ii+1,jj)-y(ii,jj) );
    sgmy = -( x(ii+1,jj) - x(ii,jj) );

	%	Get the flux
	Qflux(i,j,:) = ( F(i+1+dd,j,:)*sfpx + G(i+1+dd,j,:)*sfpy + ...
   		F(i+dd,j,:)*sfmx + G(i+dd,j,:)*sfmy + F(i,j+1+dd,:)*sgpx ...
        + G(i,j+1+dd,:)*sgpy + F(i,j+dd,:)*sgmx + G(i,j+dd,:)*sgmy );

	%	Normalize by Volume
    Qflux(i,j,:) = Qflux(i,j,:)./Vol(i,j);

	%	CFL terms
    a(i,j) = abs(u(i,j)*sfpx + v(i,j)*sfpy);
    b(i,j) = abs(u(i,j)*sgpx + v(i,j)*sgpy);
    c(i,j) = ss(i,j)*sqrt(abs( sfpx^2 + sfpy^2) ...
        + abs( sgpx^2 + sgpy^2) );

    end
end

dt = max(max((a+b+c)./Vol));
dt = cfl/dt;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%    boundary    %%%%%%%%

function [Q] = boundary(Q)
global x y gamma R P_amb T_c
[u,v,rho,p,e,T,ss,F,G] = flowvars(Q);

%   Problem boundary conditions here
nx = size(x,1);
ny = size(x,2);

%	Top Wall
p(:,ny+1) = p(:,ny);
v(:,ny+1) = 0;
u(:,ny+1) = 0;
rho(:,ny+1) = rho(:,ny);

%   Symmetry line
p(:,1) = p(:,2);
v(:,1) = -v(:,2);
u(:,1) = u(:,2);
rho(:,1) = rho(:,2);

%	Inflow-shouldn't change from initialization
u(1,:) = sqrt(gamma*R*T_c);
v(1,:) = v(2,:);

%	Out flow - set to upstream cells
u(nx+1,:) = u(nx,:);
v(nx+1,:) = v(nx,:);
p(nx+1,:) = p(nx,:);  %P_amb;
rho(nx+1,:) = rho(nx,:);

%	EOS
e = p/(gamma-1) + (1/2)*rho.*(u.*u + v.*v);

Q(:,:,1) = rho;
Q(:,:,2) = rho.*u;
Q(:,:,3) = rho.*v;
Q(:,:,4) = e;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


% DEMTOYUNSUPERVISED A script to run unsupervised deep GP on toy hierarchical data.
%
% DESC A script to run unsupervised deep GP on toy hierarchical data. The script provides
% the option of parametrising the model and initialisation in many many
% different ways... but the core of the demo (define a deep GP and train
% it) is actually not that big, if you decide to use the default options.
%
% COPYRIGHT: Andreas C. Damianou, 2013
%
% SEE ALSO: demToyRegression.m
%
% DEEPGP

experimentNo = 1;
toyType = 'hgplvmSampleTr1';
baseKern='rbfardjit'; % The mapping kernel between the layers
Q = {6,4}; % Dimensionality of the latent space in each layer
initSNR = {100, 50}; % Initial Signal to Noise ration per layer
% How to initialise X when multiple output modalities are present. See
% hsvargplvm_init for details
initial_X = 'separately';
% How to initialise X (more specifically, means of q(X)) for each layer.
% The selected method (potentially
% different per layer) will be applied in a sequential fashion, eg if we
% use PCA we obtain X_1 from PCA on Y, then X_2 from PCA on X_1 etc. Deep
% GPs are completely different than a stacked method, since X's will be
% integrated out (the initial X's above are actually the initial means of the var.
% distribution) and everythin will be optimised jointly.
% Here we opt for a Bayesian GPLVM that  gives the initial X.
% See hsvargplvm_init for other options (eg pca).
initX = 'vargplvm';
%- options for the BayesianGPLVM used to initialise the variational means
stackedInitIters = 200;
stackedInitVardistIters = 100;
stackedInitSNR = 100;
initVardistIters = 100;
demToyHsvargplvm1; % Run the actual demo

%% --- Plot true data
subplot(3,2,1)
myPlot(Z{3},'X2',[],[],{3,8},0)
subplot(3,2,3)
myPlot(Z{1},'XA',[],[],{3,8},0)
subplot(3,2,4)
myPlot(Z{2},'XB',[],[],{3,8},0)
subplot(3,2,5)
plot(Ytr{1},'x-'); title('YA');
subplot(3,2,6)
plot(Ytr{2},'x-'); title('YB');

%% -- Plot spaces discovered by deep GPs (two most dominant dimensions for
%% top layer and similarly for each of the two modalities of layer 1)
figure
hsvargplvmShowScales(model);

s2 = sort(vargplvmRetainedScales(model.layer{2}.comp{1}));
sA =  sort(vargplvmRetainedScales(model.layer{1}.comp{1}));
sB =  sort(vargplvmRetainedScales(model.layer{1}.comp{2}));

figure
subplot(2,2,1)
myPlot(model.layer{2}.vardist.means(:,s2(1:2)),'deepGP_X2',[],[],{3,8},0)
subplot(2,2,3)
myPlot(model.layer{1}.vardist.means(:,sA(1:2)),'deepGP_XA',[],[],{3,8},0)
subplot(2,2,4)
myPlot(model.layer{1}.vardist.means(:,sB(1:2)),'deepGP_XB',[],[],{3,8},0)

%% --- Compare with stacked Bayesian GP-LVM % TODO
%[XA, s, WA, modelA] = vargplvmEmbed(Ytr{1}, 5, initXOptions{1}{:});
%[XB, s, WB, modelB] = vargplvmEmbed(Ytr{2}, 5, initXOptions{1}{:});
%[X2, s, W2, model2]  = vargplvmEmbed([XA XB], 5, initXOptions{2}{:});

%% --- Compare with stacked PCA and isomap

figure
pcaXA = ppcaEmbed(Ytr{1}, 2);
pcaXB = ppcaEmbed(Ytr{2},2);
pcaX2 = ppcaEmbed([pcaXA pcaXB],2);
subplot(2,2,1)
myPlot(pcaX2,'pcaX2',[],[],{3,8},0)
subplot(2,2,3)
myPlot(pcaXA,'pcaXA',[],[],{3,8},0)
subplot(2,2,4)
myPlot(pcaXB,'pcaXB',[],[],{3,8},0)

figure
isomapXA = isomap2Embed(Ytr{1}, 2);
isomapXB = isomap2Embed(Ytr{2},2);
isomapX2 = isomap2Embed([isomapXA isomapXB],2);
subplot(2,2,1)
myPlot(isomapX2,'isomapX2',[],[],{3,8},0)
subplot(2,2,3)
myPlot(isomapXA,'isomapXA',[],[],{3,8},0)
subplot(2,2,4)
myPlot(isomapXB,'isomapXB',[],[],{3,8},0)


function P=rayleighpdf(M,S)

P= (((M./(S.^2)).*exp((-1.*(M.^2))./(2.*S^2)))).*(M>=0);

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


function triangulation_orient ( prefix )

%*****************************************************************************80
%
%% MAIN is the main program for TRIANGULATION_ORIENT.
%
%  Discussion:
%
%    TRIANGULATION_ORIENT forces a triangulation to have positive orientation.
%
%    The user supplies a node file and a triangle file, containing
%    the coordinates of the nodes, and the indices of the nodes that
%    make up each triangle.  Either 3-node or 6-node triangles may
%    be used.
%
%    The program reads the data, and for each triangle, determines
%    whether the triangle has positive orientation.  This essentially
%    means that the vertices are listed in counter clockwise order.
%    If the vertices are listed in the wrong order, they are reordered.
%    The reordered triangle file is written out.
%
%    Note that for an order 6 triangulation, the vertices are listed
%    in the first three positions.
%
%  Usage:
%
%    triangulation_orient ( 'prefix' )
%
%    where 'prefix' is the common filename prefix:
%
%    * 'prefix'_nodes.txt contains the node coordinates,
%    * 'prefix'_elements.txt contains the element definitions.
%    * 'prefix'_orient_elements.txt will contain the oriented element definitions.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 October 2009
%
%  Author:
%
%    John Burkardt
%
  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGULATION_ORIENT\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Read a node dataset of NODE_NUM points in 2 dimensions.\n' );
  fprintf ( 1, '  Read an associated triangle file of TRIANGLE_NUM \n' );
  fprintf ( 1, '  triangles using 3 or 6 nodes.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Ensure that every triangle has positive orientation.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Write the reoriented triangle file.\n' );
%
%  The command line argument is the common filename prefix.
%
  if ( nargin < 1 )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_ORIENT:\n' );

    prefix = input ( ...
      'Please enter the filename prefix:' );

  end
%
%  Create the filenames.
%
  node_filename = strcat ( prefix, '_nodes.txt' );
  element_filename = strcat ( prefix, '_elements.txt' );
  element_orient_filename = strcat ( prefix, '_orient_elements.txt' );
%
%  Read the node data.
%
  [ dim_num, node_num ] = r8mat_header_read (  node_filename );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the header of "%s".\n', node_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Spatial dimension DIM_NUM = %d\n', dim_num );
  fprintf ( 1, '  Number of points NODE_NUM = %d\n', node_num );

  node_xy(1:dim_num,1:node_num) = r8mat_data_read ( node_filename, ...
    dim_num, node_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the data in "%s".\n', node_filename );

  r8mat_transpose_print_some ( dim_num, node_num, node_xy, 1, 1, dim_num, 5, ...
    '  First 5 nodes:' );
%
%  Read the element data.
%
  [ triangle_order, triangle_num ] = i4mat_header_read ( ...
    element_filename );

  if ( triangle_order ~= 3 && triangle_order ~= 6 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_ORIENT - Fatal error!\n' );
    fprintf ( 1, '  Data is not for a 3-node or 6-node triangulation.\n' );
    error ( 'TRIANGULATION_L2Q - Fatal error!' );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the header of "%s".\n', element_filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Triangle order = %d\n', triangle_order );
  fprintf ( 1, '  Number of triangles TRIANGLE_NUM  = %d\n', triangle_num );

  triangle_node(1:triangle_order,1:triangle_num) = i4mat_data_read ( ...
    element_filename, triangle_order, triangle_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Read the data in "%s".\n', element_filename );

  i4mat_transpose_print_some ( triangle_order, triangle_num, triangle_node, ...
    1, 1, triangle_order, 5, '  First 5 triangles:' );
%
%  Detect and correct 0-based indexing.
%
  triangle_node = mesh_base_one ( node_num, triangle_order, triangle_num, ...
    triangle_node );
%
%  Compute the area, and reorient if necessary.
%
  triangle_negative_num = 0;
  triangle_zero_num = 0;

  for triangle = 1 : triangle_num

    t3(1:2,1:3) = node_xy(1:2,triangle_node(1:3,triangle));

    area = triangle_area_2d ( t3 );

    if ( area < 0.0 )

      triangle_negative_num = triangle_negative_num + 1;

      node                      = triangle_node(2,triangle);
      triangle_node(2,triangle) = triangle_node(3,triangle);
      triangle_node(3,triangle) = node;

      if ( triangle_order == 6 )
        node                      = triangle_node(4,triangle);
        triangle_node(4,triangle) = triangle_node(6,triangle);
        triangle_node(6,triangle) = node;
      end
%
%  As a check, repeat the area calculation.
%  Now, we expect to get a positive result.
%
      t3(1:2,1:3) = node_xy(1:2,triangle_node(1:3,triangle));

      area = triangle_area_2d ( t3 );

      if ( area < 0.0 )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'TRIANGULATION_ORIENT - Fatal error!\n' );
        fprintf ( 1, '  I thought I fixed the area but I did not!\n' );
        error ( 'TRIANGULATION_L2Q - Fatal error!' );
      end

    elseif ( area == 0.0 )

      triangle_zero_num = triangle_zero_num + 1;

    end

  end

  if ( 0 < triangle_zero_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_ORIENT - Warning!\n' );
    fprintf ( 1, '  You have %d triangles with\n', triangle_zero_num );
    fprintf ( 1, '  area equal to zero.\n' );
  end

  if ( 0 < triangle_negative_num )

    i4mat_write ( element_orient_filename, triangle_order, ...
      triangle_num, triangle_node );

    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_ORIENT - Warning!\n' );
    fprintf ( 1, '  You have %d triangles with\n', triangle_negative_num );
    fprintf ( 1, '  negative area.\n' );
    fprintf ( 1, '\n' );
    fprintf ( 1, '  We have reoriented these triangles to have positive\n' );
    fprintf ( 1,'  area, and written the new triangle data to \n' );
    fprintf ( 1, '  the triangle file "%s".\n', element_orient_filename );

    i4mat_transpose_print_some ( triangle_order, triangle_num, ...
      triangle_node, 1, 1, triangle_order, 5, '  First 5 triangles:' );

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'TRIANGULATION_ORIENT:\n' );
    fprintf ( 1, '  None of your triangles had negative area.\n' );
    fprintf ( 1, '  Therefore, no new triangle file was written.\n' );

  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGULATION_ORIENT\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function column_num = file_column_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_COLUMN_COUNT counts the columns in the first line of a file.
%
%  Discussion:
%
%    The file is assumed to be a simple text file.
%
%    Most lines of the file are presumed to consist of COLUMN_NUM words,
%    separated by spaces.  There may also be some blank lines, and some
%    comment lines, which have a "#" in column 1.
%
%    The routine tries to find the first non-comment non-blank line and
%    counts the number of words in that line.
%
%    If all lines are blanks or comments, it goes back and tries to analyze
%    a comment line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the file.
%
%    Output, integer COLUMN_NUM, the number of columns in the file.
%
  FALSE = 0;
  TRUE = 1;
%
%  Open the file.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_COLUMN_COUNT - Error!' );
  end
%
%  Read one line, but skip blank lines and comment lines.
%  Use FGETL so we drop the newline character!
%
  got_one = FALSE;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( s_len_trim ( line ) == 0 )

    elseif ( line(1) == '#' )

    else
      got_one = TRUE;
      break;
    end

  end

  fclose ( input_unit );

  if ( got_one == FALSE )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_COLUMN_COUNT - Warning!\n' );
    fprintf ( 1, '  The file does not seem to contain any data.\n' );
    column_num = -1;
    return;
  end

  column_num = s_word_count ( line );

  return
end
function row_num = file_row_count ( input_file_name )

%*****************************************************************************80
%
%% FILE_ROW_COUNT counts the number of row records in a file.
%
%  Discussion:
%
%    Each input line is a "RECORD".
%
%    The records are divided into three groups:
%
%    * BLANK LINES (nothing but blanks)
%    * COMMENT LINES (begin with a '#')
%    * DATA RECORDS (anything else)
%
%    The value returned by the function is the number of data records.
%
%    By the way, if the MATLAB routine FGETS is used, instead of
%    FGETL, then the variable LINE will include line termination
%    characters, which means that a blank line would not actually
%    have zero characters.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILE_NAME, the name of the input file.
%
%    Output, integer ROW_NUM, the number of rows found.
%
  input_unit = fopen ( input_file_name );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FILE_ROW_COUNT - Error!\n' );
    fprintf ( 1, '  Could not open the file "%s".\n', input_file_name );
    error ( 'FILE_ROW_COUNT - Error!' );
  end

  blank_num = 0;
  comment_num = 0;
  row_num = 0;

  record_num = 0;

  while ( 1 )

    line = fgetl ( input_unit );

    if ( line == -1 )
      break;
    end

    record_num = record_num + 1;
    record_length = s_len_trim ( line );

    if ( record_length <= 0 )
      blank_num = blank_num + 1;
    elseif ( line(1) == '#' )
      comment_num = comment_num + 1;
    else
      row_num = row_num + 1;
    end

  end

  fclose ( input_unit );

  return
end
function table = i4mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% I4MAT_DATA_READ reads data from an I4MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns in the data.
%
%    Output, integer TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %d' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the input file.\n' );
    error ( 'I4MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'I4MAT_DATA_READ - Error!\n' );
      fprintf ( 1, '  End of input while reading data.\n' );
      error ( 'I4MAT_DATA_READ - Error!' );
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = i4mat_header_read ( input_filename )

%*****************************************************************************80
%
%% I4MAT_HEADER_READ reads the header from an I4MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function i4mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% I4MAT_TRANSPOSE_PRINT_SOME prints some of an I4MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, integer A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, an optional title.
%
  incx = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, '%s\n', title );

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d  ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );
    fprintf ( 1, '\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d  ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%7d  ', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function i4mat_write ( output_filename, m, n, table )

%*****************************************************************************80
%
%% I4MAT_WRITE writes an I4MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 August 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string OUTPUT_FILENAME, the output filename.
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, integer TABLE(M,N), the points.
%
%    Input, logical HEADER, is TRUE if the header is to be included.
%

%
%  Open the file.
%
  output_unit = fopen ( output_filename, 'wt' );

  if ( output_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4MAT_WRITE - Error!\n' );
    fprintf ( 1, '  Could not open the output file.\n' );
    error ( 'I4MAT_WRITE - Error!' );
  end
%
%  Write the data.
%
  for j = 1 : n
    for i = 1 : m
      fprintf ( output_unit, '  %12d', round ( table(i,j) ) );
    end
    fprintf ( output_unit, '\n' );
  end
%
%  Close the file.
%
  fclose ( output_unit );

  return
end
function element_node = mesh_base_one ( node_num, element_order, ...
  element_num, element_node )

%*****************************************************************************80
%
%% MESH_BASE_ONE ensures that the element definition is one-based.
%
%  Discussion:
%
%    The ELEMENT_NODE array contains nodes indices that form elements.
%    The convention for node indexing might start at 0 or at 1.
%
%    Since a MATLAB program will naturally assume a 1-based indexing, it is
%    necessary to check a given element definition and, if it is actually
%    0-based, to convert it.
%
%    This function attempts to detect 0-based node indexing and correct it.
%
%    Thanks to Feifei Xu for pointing out that I was subtracting 1 when I
%    should have been adding 1!  29 November 2012.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer ELEMENT_ORDER, the order of the elements.
%
%    Input, integer ELEMENT_NUM, the number of elements.
%
%    Input/output, integer ELEMENT_NODE(ELEMENT_ORDE,ELEMENT_NUM), the element
%    definitions.
%
  node_min = min ( min ( element_node(1:element_order,1:element_num) ) );
  node_max = max ( max ( element_node(1:element_order,1:element_num) ) );

  if ( node_min == 0 && node_max == node_num - 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE:\n' );
    fprintf ( 1, '  The element indexing appears to be 0-based!\n' );
    fprintf ( 1, '  This will be converted to 1-based.\n' );
    element_node(1:element_order,1:element_num) = ...
      element_node(1:element_order,1:element_num) + 1;
  elseif ( node_min == 1 && node_max == node_num )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE:\n' );
    fprintf ( 1, '  The element indexing appears to be 1-based!\n' );
    fprintf ( 1, '  No conversion is necessary.\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MESH_BASE_ONE - Warning!\n' );
    fprintf ( 1, '  The element indexing is not of a recognized type.\n' );
    fprintf ( 1, '  NODE_MIN = %d\n', node_min );
    fprintf ( 1, '  NODE_MAX = %d\n', node_max );
    fprintf ( 1, '  NODE_NUM = %d\n', node_num );
  end

  return
end
function table = r8mat_data_read ( input_filename, m, n )

%*****************************************************************************80
%
%% R8MAT_DATA_READ reads data from an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Input, integer M, N, the number of rows and columns of data.
%
%    Output, real TABLE(M,N), the point coordinates.
%
  table = zeros ( m, n );
%
%  Build up the format string for reading M real numbers.
%
  string = ' ';

  for i = 0 : m
    string = strcat ( string, ' %f' );
  end

  input_unit = fopen ( input_filename );

  if ( input_unit < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_DATA_READ - Error!\n' );
    fprintf ( 1, '  Could not open the file.\n' );
    error ( 'R8MAT_DATA_READ - Error!' );
  end

  i = 0;

  while ( i < n )

    line = fgets ( input_unit );

    if ( line == -1 )
      break;
    end

    if ( line(1) == '#' )

    elseif ( s_len_trim ( line ) == 0 )

    else

      [ x, count ] = sscanf ( line, string );

      if ( count == m )
        i = i + 1;
        table(1:m,i) = x(1:m);
      end

    end

  end

  fclose ( input_unit );

  return
end
function [ m, n ] = r8mat_header_read ( input_filename )

%*****************************************************************************80
%
%% R8MAT_HEADER_READ reads the header from an R8MAT file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string INPUT_FILENAME, the name of the input file.
%
%    Output, integer M, the spatial dimension.
%
%    Output, integer N, the number of points.
%
  m = file_column_count ( input_filename );

  if ( m <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data columns in\n' );
    fprintf ( 1, '  the file %s.\n', input_filename );
  end

  n = file_row_count ( input_filename );

  if ( n <= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8MAT_HEADER_READ - Fatal error!\n' );
    fprintf ( 1, '  There was some kind of I/O problem while trying\n' );
    fprintf ( 1, '  to count the number of data rows in\n' );
    fprintf ( 1, '  the file %s\n', input_filename );
  end

  return
end
function r8mat_transpose_print_some ( m, n, a, ilo, jlo, ihi, jhi, title )

%*****************************************************************************80
%
%% R8MAT_TRANSPOSE_PRINT_SOME prints some of an R8MAT, transposed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), an M by N matrix to be printed.
%
%    Input, integer ILO, JLO, the first row and column to print.
%
%    Input, integer IHI, JHI, the last row and column to print.
%
%    Input, string TITLE, an optional title.
%
  incx = 5;

  if ( 0 < s_len_trim ( title ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, '%s\n', title );
  end

  for i2lo = max ( ilo, 1 ) : incx : min ( ihi, m )

    i2hi = i2lo + incx - 1;
    i2hi = min ( i2hi, m );
    i2hi = min ( i2hi, ihi );

    inc = i2hi + 1 - i2lo;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Row: ' );
    for i = i2lo : i2hi
      fprintf ( 1, '%7d       ', i );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Col\n' );

    j2lo = max ( jlo, 1 );
    j2hi = min ( jhi, n );

    for j = j2lo : j2hi

      fprintf ( 1, '%5d ', j );
      for i2 = 1 : inc
        i = i2lo - 1 + i2;
        fprintf ( 1, '%12f', a(i,j) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end
function len = s_len_trim ( s )

%*****************************************************************************80
%
% S_LEN_TRIM returns the length of a character string to the last nonblank.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 June 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be measured.
%
%    Output, integer LEN, the length of the string up to the last nonblank.
%
  len = length ( s );

  while ( 0 < len )
    if ( s(len) ~= ' ' )
      return
    end
    len = len - 1;
  end

  return
end
function word_num = s_word_count ( s )

%*****************************************************************************80
%
%% S_WORD_COUNT counts the number of "words" in a string.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string S, the string to be examined.
%
%    Output, integer WORD_NUM, the number of "words" in the string.
%    Words are presumed to be separated by one or more blanks.
%
  FALSE = 0;
  TRUE = 1;

  word_num = 0;
  s_length = length ( s );

  if ( s_length <= 0 )
    return;
  end

  blank = TRUE;

  for i = 1 : s_length

    if ( s(i) == ' ' )
      blank = TRUE;
    elseif ( blank == TRUE )
      word_num = word_num + 1;
      blank = FALSE;
    end

  end

  return
end
function timestamp ( )

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

  return
end
function area = triangle_area_2d ( t )

%*****************************************************************************80
%
%% TRIANGLE_AREA_2D computes the area of a triangle in 2D.
%
%  Discussion:
%
%    If the triangle's vertices are given in counterclockwise order,
%    the area will be positive.  If the triangle's vertices are given
%    in clockwise order, the area will be negative!
%
%    An earlier version of this routine always returned the absolute
%    value of the computed area.  I am convinced now that that is
%    a less useful result!  For instance, by returning the signed
%    area of a triangle, it is possible to easily compute the area
%    of a nonconvex polygon as the sum of the (possibly negative)
%    areas of triangles formed by node 1 and successive pairs of vertices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real T(2,3), the triangle vertices.
%
%    Output, real AREA, the area of the triangle.
%
  area = 0.5 * ( ...
      t(1,1) * ( t(2,2) - t(2,3) ) ...
    + t(1,2) * ( t(2,3) - t(2,1) ) ...
    + t(1,3) * ( t(2,1) - t(2,2) ) );

  return
end


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal & Yosra Boukari & Houssem Haddar (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                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtIpbHelmholtz0.m                            |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal & Yosra Boukari               |
%|  ( # )  |                & Houssem Haddar                              |
%|  / 0 \  |   CREATION   : 14.03.2017                                    |
%| ( === ) |   LAST MODIF : 14.03.2018                                    |
%|  `---'  |   SYNOPSIS   : Completion data for spherical helmholtz       |
%|         |                scatering                                     |
%|         |   ref1 : A Convergent Data Completion Algorithm Using Surface|
%|         |   Integral Equation, Inverse Problems, IOP Publishing ...    |
%|         |   ref2: Poly cours Terasse p. 194, Calderon operators        |
%+========================================================================+

% Cleaning
clear all
close all
clc

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

% Wave number and frequency
k = 1
f = (k*340)/(2*pi)

% Data noise
noise = 0%1e-2

% Incident plane wave with normlized direction
X0         = [0 0 -1];
X0         = X0./norm(X0);
PW         = @(X) exp(1i*k*X*X0');
gradxPW{1} = @(X) 1i*k*X0(1) .* PW(X);
gradxPW{2} = @(X) 1i*k*X0(2) .* PW(X);
gradxPW{3} = @(X) 1i*k*X0(3) .* PW(X);

% Green kernel function --> G(x,y) = exp(ik|x-y|)/|x-y|
Gxy = @(X,Y) femGreenKernel(X,Y,'[exp(ikr)/r]',k);

% grady Green kernel function --> G(x,y) = grady[exp(ik|x-y|)/|x-y|]
dyGxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]1',k);
dyGxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]2',k);
dyGxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]3',k);

% Green kernel function --> G(x,y) = gradx[exp(ik|x-y|)/|x-y|]
dxGxy{1} = @(X,Y) femGreenKernel(X,Y,'gradx[exp(ikr)/r]1',k);
dxGxy{2} = @(X,Y) femGreenKernel(X,Y,'gradx[exp(ikr)/r]2',k);
dxGxy{3} = @(X,Y) femGreenKernel(X,Y,'gradx[exp(ikr)/r]3',k);

% Finite element
gss = 3;
typ = 'P1';
tol = 1e-3;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% SCATERING PROBLEM %%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fictive mesh for diffraction problem (sphere)
mesh = mshSphere(4e2,1);

% % Fictive mesh for diffraction problem (cube)
% mesh = mshCube(4e2,[1 1 1]);
% mesh = mesh.bnd;

% Verify wave number
stp = mesh.stp;
if k > 1/stp(2)
    error('completionHelmholtz.m : wave number k is too high for mesh')
end

% Quadrature
sigma = dom(mesh,gss);

% Finite element
u = fem(mesh,typ);

% Boundary element operator on fictive domain (Hypersingular)
H = 1/(4*pi) .* (k^2 * integral(sigma,sigma,ntimes(u),Gxy,ntimes(u)) ...
    - integral(sigma,sigma,nxgrad(u),Gxy,nxgrad(u)));
H = H + 1/(4*pi) .* (k^2 * regularize(sigma,sigma,ntimes(u),'[1/r]',ntimes(u)) ...
    - regularize(sigma,sigma,nxgrad(u),'[1/r]',nxgrad(u)));

% Solve neumann problem on fictive boundary : - [H] mu = - dnP0
RHS = - integral(sigma,ntimes(u),gradxPW);
mu  = (-H) \ RHS;

% Radiation on boundary :  p(x) =  - [D + I/2] mu
Id  = integral(sigma,u,u);
D   = 1/(4*pi) .* integral(sigma,sigma,u,dyGxy,ntimes(u));
D   = D + 1/(4*pi) .* regularize(sigma,sigma,u,'grady[1/r]',ntimes(u));
sol = - Id \ ( (D + 0.5*Id) * mu);

% Comparare to analytic solution
ref = sphereHelmholtz('dom','neu',1,k,1.0001*mesh.vtx);
errScatMesh = norm(ref - sol)/norm(ref)

% Graphical representation
figure
plot(mesh)
hold on
plot(mesh,abs(sol+PW(u.dof)))
xlabel('X');   ylabel('Y');   zlabel('Z');
title('Total field')
axis equal
colorbar

%%%%%%%%%%%%%%%%%%%%%%%%%%%% SCATERING RADIATION %%%%%%%%%%%%%%%%%%%%%%%%%%
% Interior boundary
meshInt = mshSphere(5e2,1.25);
meshInt = swap(meshInt);

% Exterior boundary
meshExt = mshSphere(1e3,1.5);

% Fusion des maillages
meshTot = union(meshExt,meshInt);

% Verify wave number
stp = mesh.stp;
if k > 1/stp(2)
    error('completionHelmholtz.m : wave number k is too high for mesh')
end

% Quadrature
sigmaTot = dom(meshTot,gss);

% Finite element
uTot = fem(meshTot,typ);

% Mass matrix
IdTot = integral(sigmaTot,uTot,uTot);

% Solution of the scatering problem : p(x) = - [D] mu
p = - 1/(4*pi) .* integral(sigmaTot,sigma,uTot,dyGxy,ntimes(u)) * mu;

% Extract Galerkin
p = IdTot \ p;

% Compare to analytical solution
ref = sphereHelmholtz('dom','neu',1,k,uTot.dof);
errScat = norm(ref-p)/norm(ref)

% Scatered speed : dnp(x) = - [H] mu
dnp = - 1/(4*pi) .* (k^2 * integral(sigmaTot,sigma,ntimes(uTot),Gxy,ntimes(u)) ...
    - integral(sigmaTot,sigma,nxgrad(uTot),Gxy,nxgrad(u))) * mu;

% Extract Galerkin
dnp = IdTot \ dnp;

% % Graphical representation
% figure
% plot(mesh)
% hold on
% plot(mesh,abs(p+PW(uTot.dof)))
% plotNrm(mesh,'r')
% xlabel('X');   ylabel('Y');   zlabel('Z');
% title('Interior radiation')
% axis equal
% alpha(0.5)
% colorbar

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% OPERATOR %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Single layer --> \int_Sx \int_Sy psi(x)' G(x,y) psi(y) dx dy
S = 1/(4*pi) .* integral(sigmaTot,sigmaTot,uTot,Gxy,uTot);
S = S + 1/(4*pi) .* regularize(sigmaTot,sigmaTot,uTot,'[1/r]',uTot);

% Double layer --> \int_Sx \int_Sy psi(x)' dny G(x,y) psi(y) dx dy
D = 1/(4*pi) .* integral(sigmaTot,sigmaTot,uTot,dyGxy,ntimes(uTot));
D = D + 1/(4*pi) .* regularize(sigmaTot,sigmaTot,uTot,'grady[1/r]',ntimes(uTot));

% Double layer --> \int_Sx \int_Sy psi(x)' dnx G(x,y) psi(y) dx dy
Dt = D.';

% Hypersingular --> k^2 * \int_Sx \int_Sy n.psi(x) G(x,y) n.psi(y) dx dy
%                   - \int_Sx \int_Sy nxgrad(psi(x)) G(x,y) nxgrad(psi(y)) dx dy
N = 1/(4*pi) .* (k^2 * integral(sigmaTot,sigmaTot,ntimes(uTot),Gxy,ntimes(uTot)) ...
    - integral(sigmaTot,sigmaTot,nxgrad(uTot),Gxy,nxgrad(uTot)));
N = N + 1/(4*pi) .* (k^2 * regularize(sigmaTot,sigmaTot,ntimes(uTot),'[1/r]',ntimes(uTot)) ...
    - regularize(sigmaTot,sigmaTot,nxgrad(uTot),'[1/r]',nxgrad(uTot)));

% Calderon operators
Z = sparse(length(uTot),length(uTot));
I = [IdTot Z ; Z IdTot];
H = [-D S ; -N Dt];

% Calderon operator for interior problem (projector)
A = H + 0.5*I;
errCalderon = norm(A*(I\A)-A,'fro')/norm(A,'fro')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% INVERSE PROBLEM %%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Indices des dof
N    = size(meshTot.vtx,1);
Iext = 1:size(meshExt.vtx,1);
Iint = Iext(end)+1:N;

% Excitation
f = p(Iext);
g = dnp(Iext);

% Solutions
pSol   = p(Iint);
dnpSol = dnp(Iint);

% Indice operateur
Iext = [Iext , N + Iext];
Iint = [Iint , N + Iint];

% Extraction des sous matrices
A11 = A(Iext,Iext);
A12 = A(Iext,Iint);
A21 = A(Iint,Iext);
A22 = A(Iint,Iint);

% Matrices identite
I11 = I(Iext,Iext);
I22 = I(Iint,Iint);

% Linear system
LHS = [A12 ; A22 - I22];

% Right hand side
RHS = [ I11 - A11 ; -A21] * [f;g];

% Solve linear system with gaussian noise
X = LHS \ (RHS .* (1 + noise*(1 + randn(size(RHS,1),1))));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% TICHONOV %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% data  = F1;
% datan = B * F1n;
% [U,S,V]=svd(A);
% global s2 delta rhs2
% a1 = length(fe{2});
% rhs=(U')*datan;
% rhsr =rhs(1:a1+a1);
% s2 = diag(S).^2;
% rhs2 = abs(rhsr).^2;
% delta=norm(datan-B*(data))/norm(datan);
% alpha0=delta*min(s2)/(1-delta);
% alpha1=delta*max(s2)/(1-delta);
%
% fmor = @(x)sum(((x ./(x+s2)).^2 - delta^2).*rhs2);
% alphaM = fzero(fmor,[alpha0 alpha1]);
%
% X2 = V*((diag(S)./(alphaM+s2)).*rhsr);
% errTichonov = norm(X-X2)./norm(X2)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ERROR ANALYSIS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compare external pression
sol = X(1:end/2);
ref = pSol;
errSolution = norm(ref-sol)/norm(ref)

% Graphical representation
figure(13)
plot(meshInt,abs(sol+PW(meshInt.vtx)))
xlabel('X');   ylabel('Y');   zlabel('Z');
title('Reconstructed total field')
axis equal
colorbar

% Graphical representation
figure(14)
plot(meshInt,abs(sol-ref)./abs(ref))
xlabel('X');   ylabel('Y');   zlabel('Z');
title('Relative error on reconstructed field')
axis equal
colorbar

% Compare normal derivative
sol = X(end/2+1:end);
ref = dnpSol;
errDerivative = norm(ref-sol)/norm(ref)

disp('~~> Michto gypsilab !')


function [f] = spm_fp_cmc_tfm(x,u,P,M)
% parameter equations for a neural mass model (canonical microcircuit)
% FORMAT [f] = spm_fp_cmc_tfm(x,u,P,M)
%
% x      - state vector
%   x(:,1) - voltage     (spiny stellate cells)
%   x(:,2) - conductance (spiny stellate cells)
%   x(:,3) - voltage     (superficial pyramidal cells)
%   x(:,4) - conductance (superficial pyramidal cells)
%   x(:,5) - voltage     (inhibitory interneurons)
%   x(:,6) - conductance (inhibitory interneurons)
%   x(:,7) - voltage     (deep pyramidal cells)
%   x(:,8) - conductance (deep pyramidal cells)
%
% f        - dP = h(x(t),u(t),P,M)
%
% Prior fixed parameter scaling
%
% G  = intrinsic rates
% D  = propagation delays (intrinsic, extrinsic)
% T  = synaptic time constants
% R  = slope of sigmoid activation function
%
%__________________________________________________________________________
% David O, Friston KJ (2003) A neural mass model for MEG/EEG: coupling and
% neuronal dynamics. NeuroImage 20: 1743-1755
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_fp_cmc_tfm.m 6234 2014-10-12 09:59:10Z karl $

% Neuronal states (deviations from baseline firing)
%--------------------------------------------------------------------------
%   x(:,1) - voltage     (spiny stellate cells)
%   x(:,2) - conductance (spiny stellate cells)
%   x(:,3) - voltage     (superficial pyramidal cells)
%   x(:,4) - conductance (superficial pyramidal cells)
%   x(:,5) - voltage     (inhibitory interneurons)
%   x(:,6) - conductance (inhibitory interneurons)
%   x(:,7) - voltage     (deep pyramidal cells)
%   x(:,8) - conductance (deep pyramidal cells)
%--------------------------------------------------------------------------
persistent iG nP Ca G
if isempty(iG)
    Ca  = zeros(size(P.G));
    G   = zeros(size(P.G));                    % parameter (deviates)
    iG  = spm_fieldindices(P,'G');
    nP  = spm_length(P);
end

% get dimensions and configure state variables
%--------------------------------------------------------------------------
f  = zeros(nP,1);                              % flow
x  = spm_unvec(x,M.x);                         % neuronal states
x  = x(:,1:2:end);                             % depolarisation

% neuronal populations with Voltage-dependent connectivity
%==========================================================================
%                  ss sp ii dp                 % neuronal populations
%--------------------------------------------------------------------------
a     = [1 8 2 1]*64;                          % potentiation rate
b     = [4 2 2 1]*4;                           % decay rate

NMDA  = @(x)1./(1 + exp(-x)) - 1/2;            % depolarisation CDF

% NMDA-like Voltage-dependent changes in (recurrent) synaptic efficacy
%--------------------------------------------------------------------------
A     = exp(P.E)*diag(a);
B     = exp(P.F)*diag(b);
dC    = (A.*NMDA(8*x) - Ca).*B;
dG    = Ca.*(2 - G)/2  - G.*B;
Ca    = Ca + dC*M.dt;
G     = G  + dG*M.dt;
f(iG) = G(:);


function [M,G]=triSurf2ImSpec(F,V,voxelSize)

% function [M,G,bwLabels]=triSurf2Im(F,V,voxelSize)
% -----------------------------------------------------------------------
% This function converts the input triangulated surface, specified by the
% faces F and the vertices V into an image based on the voxel size
% specified in the input voxelSize. If the latter is empty then the
% voxelSize is based on the maximum edge length. If required the surface is
% resampled to subvoxel resolution using the |subtri| function. Then
% surface vertices are simply mapped to an image coordinate system and
% since they are densely sampled with respect to the voxel size form an
% enclosing boundary of voxels. The exterior, boundary and interior are
% then formulated using the bwlabeln function. The exterior, boundary and
% intertior voxels are labelled using 0's, 1's and 2's in the output image
% M. The second output G is a structure containing the fields G.voxelSize
% and G.origin which form the geometric information for the image.
%
% Kevin Mattheus Moerman
% kevinmoerman@hotmail.com
% 28/08/2013
%------------------------------------------------------------------------

%%
%Checking edge lenghts of surface
TR = triangulation(F,V);
E=edges(TR);
Ed=0;
for q=1:1:size(V,2)
    Ed=Ed+(V(E(:,1),q)-V(E(:,2),q)).^2;
end
edgeLengths=sqrt(Ed);
maxEdgeLength=max(edgeLengths(:));

if isempty(voxelSize)
    voxelSize=maxEdgeLength;
end

%Resample surface if voxelsize is small with respect to edgelenghts
n=maxEdgeLength/voxelSize;
if n>(1-eps(1))
    n=floor(n);
    [~,V]=subtri(F,V,n);
end

%Determine surface set coordinate minima
minV=min(V,[],1);

%Determine shift so all coordinates are positive
imOrigin=-(minV-voxelSize);

%Shift points using origin
V=V+imOrigin(ones(size(V,1),1),:);

%Convert to image coordinates
V_IJK=V;
[V_IJK(:,1),V_IJK(:,2),V_IJK(:,3)]=cart2im(V(:,1),V(:,2),V(:,3),voxelSize*ones(1,3));

%Rounding image coordinates to snap to voxel
V_IJK=round(V_IJK);

%Determin image size
siz=max(V_IJK,[],1)+1;

%Get linear indices of points
indV=sub2ind(siz,V_IJK(:,1),V_IJK(:,2),V_IJK(:,3));

%Create surface boundary image
L=false(siz);
L(indV)=1;

%Create boundary, interior and exterior image
LL = bwlabeln(~L,6); %Get labels for ~L image which will segment interior, exterior and boundary
uniqueLabels=unique(LL(:));

labelsBoundary=LL(L); %The label numbers for the boundary

indExteriorVoxel=1; %First is outside since image is at least a voxel too big on all sides
labelExterior=LL(indExteriorVoxel); %The label number for the exterior

labelsInterior=uniqueLabels(~ismember(uniqueLabels,[labelsBoundary(:); labelExterior])); %Labels for the interior (possibly multiple)

M=zeros(size(L)); %The exterior is set to 0
M(L)=1; %The boundary is set to 1
M(ismember(LL,labelsInterior))=2; %Interior is set to 2

%Storing image geometry metrics
G.voxelSize=voxelSize;
G.origin=imOrigin;

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


function [hd_estimates]=hdols123(const,exo,betahat,k,n,p,D,m,T,X,Y,data_exo)

contributors = n + const + length(exo) + 1; %variables + constant + exogenous + initial conditions
hd_estimates=cell(contributors+2,n); %shocks+constant+initial values+exogenous+unexplained+to be explained by shocks only
hd_estimates2=cell(contributors+2,n); %contributors + to be explained by shocks only (unexplained part, other part)

identified=n;

%% compute the historical decomposition, if its not yet computed
%===============================================
%%%%% could take this part from previous computations if possible
Bfull=reshape(betahat,k,n);                    %get the Bfull matrix
B=Bfull(1:n*p,:);                           %drop the coefficients for all exogenous variables from the matrix
B_comp = [B'; eye(n*(p-1)) zeros(n*(p-1),n)];%put into companion form
EPS=Y-X*Bfull;                               %get reduced form residuals
ETA=(D\EPS');                              %get structural shocks

%% Compute historical decompositions
%===============================================

% Contribution of each shock
    aux_D = zeros(n*p,n); %auxilary D matrix, that is consistent with companion matrix
    aux_D(1:n,:) = D; %set the first entrys equal to Dinv
    Selec = [eye(n) zeros(n,(p-1)*n)]; %selection matrix
    HDestimates_store = zeros(p*n,T+1,n); %cell aray to store results
    HDestimates = zeros(n,T+1,n);
    for j=1:n % for each variable %%%%%
        ETA_comp = zeros(n,T+1); % structural shock matrix that is consistent with companion form
        if j <= identified % if j is an identified shock
        ETA_comp(j,2:end) = ETA(j,:); % fill in the entry for shock n, leave the first entry blank
        end
        for i = 2:T+1
            HDestimates_store(:,i,j)=aux_D*ETA_comp(:,i) + B_comp*HDestimates_store(:,i-1,j); %recursively sum over shock impulse at period i on variable j and the previous period
            HDestimates(:,i,j)=Selec*HDestimates_store(:,i,j); %select the entry corresponding to the current period
        end
    end

%% contribution of the initial values
    HDinitial_storage   = zeros(p*n,T+1);
    HDinitial_estimates = zeros(n, T+1);
    Xnoexo = X(:,1:n*p);
    HDinitial_storage(:,1) = Xnoexo(1,:)'; %set the initial values to the first row of X (n*P)+exo
    HDinitial_estimates(:,1) = Selec*HDinitial_storage(:,1); %select the initial values for the first n variables (i.e. the values at Y_{t-1}
    for i = 2:T+1 %loop over periods and compute the impact of the initial conditions recursively
        HDinitial_storage(:,i) = B_comp*HDinitial_storage(:,i-1); %compute the impact of those values (which in principle consist of past shocks)
        HDinitial_estimates(:,i) = Selec*HDinitial_storage(:,i);
    end

if const==1
%%  Contribution of the Constant
    HDconstant_storage = zeros(p*n,T+1);
    HDconstant_estimates = zeros(n, T+1);
    Coefficients = zeros(p*n,1);
        Coefficients(1:n,:) = Bfull(n*p+1,:);
        for i = 2:T+1 %loop over periods
            HDconstant_storage(:,i)=Coefficients+B_comp*HDconstant_storage(:,i-1);
            HDconstant_estimates(:,i)=Selec*HDconstant_storage(:,i);
        end
end
%% Contribution of exogenous variables
 if m > 1
    HDexo_storage = zeros(p*n,T+1);
    HDexo_estimates = zeros(n,T+1);
    Coefficients_exo = zeros(p*n,(m-1)*(1));
    data_exocut=data_exo(p+1:end,:); %cut initial conditions from exogenous
    Coefficients_exo(1:n,:) = Bfull(n*p+const+1:end,:)'; %get the corresponding coefficients
        for i = 2:T+1
            HDexo_storage(:,i)=Coefficients_exo*data_exocut(i-1,:)'+B_comp*HDexo_storage(:,i-1);
            HDexo_estimates(:,i)=Selec*HDexo_storage(:,i);
        end
 end

%% put these values into the corresponding cell for hd_estimates such that
% for variable x (hd_estimates(x,n+1)) = HDinitial_estimates(x,:)
%reorganize storage
        for jj=1:n %for variables
            for kk=1:T+1 %for periods
                for ii=1:n %for shock contributions
                    hd_estimates2{ii,jj}(1,kk)=HDestimates(jj,kk,ii);
                end
                hd_estimates2{n+1,jj}(1,kk)=HDinitial_estimates(jj,kk);
                if const==1
                hd_estimates2{n+2,jj}(1,kk)=HDconstant_estimates(jj,kk);
                end
                if m>1
                hd_estimates2{n+3,jj}(1,kk)=HDexo_estimates(jj,kk);
                end
            end
        end

 HDsum = zeros(T+1,n); %if we sum over all variables this should give Y
 for jj=1:n %loop over variables (columns)
 sumvariable = zeros(1,T+1);
 for kk=1:T+1 %loop over periods
     sumperiod=0;
  for ii=1:contributors %loop over contributors (rows)
      sumperiod=sumperiod+hd_estimates2{ii,jj}(1,kk);
  end
  sumvariable(1,kk)=sumperiod;
 end
 HDsum(:,jj)=sumvariable(1,:);  %%%%% this should be Y?
 end

 %% determine the unexplained part (if model is not fully identified) %%%%% does that mean in IRFt5???
 aux=zeros(1,n);
 unexplained = Y-HDsum(2:end,:);
 unexplained = [aux; unexplained];
for jj=1:n
    hd_estimates2{contributors+1,jj}=unexplained(:,jj)';
end

%% finally substract the sum of the contribution of the (residual?)
%exogenous, constant,initial conditions from Y to get the
%part that was left to be explained by the shocks (for plotting reasons)
 Exosum = zeros(T+1,n); %if we sum over all variables this should give Y
 for jj=1:n %loop over variables (columns)
 sumvariable = zeros(1,T+1);
 for kk=1:T+1 %loop over periods
     sumperiod=0;
  for ii=n+1:contributors %loop over contributors (rows)
      sumperiod = sumperiod+hd_estimates2{ii,jj}(1,kk);
  end
  sumvariable(1,kk)=sumperiod;
 end
 Exosum(:,jj)=sumvariable(1,:);
 end

 %% determine the part that was left to be explained by the shocks
 aux = zeros(1,n);
 tobeexplained = [aux; Y-Exosum(2:end,:)];
for jj=1:n
    hd_estimates2{contributors+2,jj}=tobeexplained(:,jj)';
end

%drop the initial entry for each cell in hd_estimates
for jj=1:n % over variables
    for ii=1:contributors+2 %all contributions
        hd_estimates{ii,jj}=hd_estimates2{ii,jj}(2:end);
    end
end


function gradfun = approxgradientFD(problem, options)
% Gradient approx. fnctn handle based on finite differences of the cost.
%
% function gradfun = approxgradientFD(problem)
% function gradfun = approxgradientFD(problem, options)
%
% Input:
%
% A Manopt problem structure (already containing the manifold and enough
% information to compute the cost) and an options structure (optional),
% containing one option:
%    options.stepsize (positive double; default: 2^-23).
%    options.subspacedim (positive integer; default: [], for M.dim()).
%
% If the cost cannot be computed on 'problem', a warning is issued.
%
% Output:
%
% Returns a function handle, encapsulating a generic finite difference
% approximation of the gradient of the problem cost. The finite difference
% is based on M.dim()+1 computations of the cost.
%
% The returned gradfun has this calling pattern:
%
%   function gradfd = gradfun(x)
%   function gradfd = gradfun(x, storedb)
%   function gradfd = gradfun(x, storedb, key)
%
% x is a point on the manifold problem.M, storedb is a StoreDB object,
% and key is the StoreDB key to point x.
%
% Usage:
%
% Typically, the user will set problem.M and other fields to define the
% cost (typically, problem.cost). Then, to use this generic purpose
% gradient approximation:
%
%   problem.approxgrad = approxgradientFD(problem, options);
%
% See also: steepestdescent conjugategradient

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Nov. 1, 2016.
% Contributors:
% Change log:

    % This gradient approximation is based on the cost:
    % check availability.
    if ~canGetCost(problem)
        warning('manopt:approxgradFD:nocost', ...
                'approxgradFD requires the cost to be computable.');
    end

    % Set local defaults here, and merge with user options, if any.
    localdefaults.stepsize = 2^-23;
    localdefaults.subspacedim = [];
    if ~exist('options', 'var') || isempty(options)
        options = struct();
    end
    options = mergeOptions(localdefaults, options);

    % % Finite-difference parameters
    % How far do we look?
    stepsize = options.stepsize;
    % Approximate the projection of the gradient on a random subspace of
    % what dimension? If [], uses full tangent space.
    subspacedim = options.subspacedim;

    % Build and return the function handle here. This extra construct via
    % funhandle makes it possible to make storedb and key optional.
    gradfun = @funhandle;
    function gradfd = funhandle(x, storedb, key)
        % Allow omission of the key, and even of storedb.
        if ~exist('key', 'var')
            if ~exist('storedb', 'var')
                storedb = StoreDB();
            end
            key = storedb.getNewKey();
        end
        gradfd = gradientFD(stepsize, subspacedim, problem, x, storedb, key);
    end

end

function gradfd = gradientFD(stepsize, subspacedim, problem, x, storedb, key)
% This function does the actual work.
%
% Original code: Nov. 1, 2016 (NB).

    % Evaluate the cost at the root point
    fx = getCost(problem, x, storedb, key);

    % Pick an orthonormal basis for the tangent space at x, or a subspace
    % thereof. The default is a full subspace. If a strict subspace is
    % picked, the returned vector approximates the orthogonal projection of
    % the gradient to that subspace.
    B = tangentorthobasis(problem.M, x, subspacedim);

    % Use finite differences to approximate the directional derivative
    % along each direction in the basis B.
    df = zeros(size(B));
    for k = 1 : numel(B)
        % Move in the B{k} direction
        xk = problem.M.retr(x, B{k}, stepsize);
        keyk = storedb.getNewKey();
        % Evaluate the cost there
        fxk = getCost(problem, xk, storedb, keyk);
        % Don't keep this point in cache
        storedb.remove(keyk);
        % Finite difference
        df(k) = (fxk - fx)/stepsize;
    end

    % Build the gradient approximation.
    gradfd = lincomb(problem.M, x, B, df);

end


% dftfilt3() - discrete complex wavelet filters
%
% Usage:
%   >> [wavelet,cycles,freqresol,timeresol] = dftfilt3( freqs, cycles, srate, varargin)
%
% Inputs:
%   freqs    - vector of frequencies of interest.
%   cycles   - cycles array. If cycles=0, then the Hanning tapered Short-term FFT is used.
%              If one value is given and cycles>0, all wavelets have
%              the same number of cycles. If two values are given, the
%              two values are used for the number of cycles at the lowest
%              frequency and at the highest frequency, with linear or
%              log-linear interpolation between these values for intermediate
%              frequencies
%   srate    - sampling rate (in Hz)
%
% Optional Inputs: Input these as 'key/value pairs.
%   'cycleinc' - ['linear'|'log'] increase mode if [min max] cycles is
%              provided in 'cycle' parameter. {default: 'linear'}
%   'winsize'  Use this option for Hanning tapered FFT or if you prefer to set the length of the
%              wavelets to be equal for all of them (e.g., to set the
%              length to 256 samples input: 'winsize',256). {default: [])
%              Note: the output 'wavelet' will be a matrix and it may be
%              incompatible with current versions of timefreq and newtimef.
%   'timesupport' The number of temporal standard deviation used for wavelet lengths {default: 7)
%
% Output:
%   wavelet - cell array or matrix of wavelet filters
%   timeresol - temporal resolution of Morlet wavelets.
%   freqresol - frequency resolution of Morlet wavelets.
%
% Note: The length of the window is always made odd.
%
% Authors: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 3/28/2003
%          Rey Ramirez, SCCN/INC/UCSD, La Jolla, 9/26/2006

% Copyright (C) 3/28/2003 Arnaud Delorme 8, SCCN/INC/UCSD, arno@salk.edu
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

%
% Revision 1.12 2006/09/25  rey r
% Almost complete rewriting of dftfilt2.m, changing both Morlet and Hanning
% DFT to be more in line with conventional implementations.
%
% Revision 1.11  2006/09/07 19:05:34  scott
% further clarified the Morlet/Hanning distinction -sm
%
% Revision 1.10  2006/09/07 18:55:15  scott
% clarified window types in help msg -sm
%
% Revision 1.9  2006/05/05 16:17:36  arno
% implementing cycle array
%
% Revision 1.8  2004/03/04 19:31:03  arno
% email
%
% Revision 1.7  2004/02/25 01:45:55  arno
% sinus test
%
% Revision 1.6  2004/02/15 22:23:08  arno
% implementing morlet wavelet
%
% Revision 1.5  2003/05/09 20:55:10  arno
% adding hanning function
%
% Revision 1.4  2003/04/29 16:02:54  arno
% header typos
%
% Revision 1.3  2003/04/29 01:09:16  arno
% debug imaginary part
%
% Revision 1.2  2003/04/28 23:01:13  arno
% *** empty log message ***
%
% Revision 1.1  2003/04/28 22:46:49  arno
% Initial revision
%

function [wavelet,cycles,freqresol,timeresol] = dftfilt3( freqs, cycles, srate, varargin);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Rey fixed all input parameter sorting.
if nargin < 3
    error(' A minimum of 3 arguments is required');
end;
numargin=length(varargin);
if rem(numargin,2)
    error('There is an uneven number key/value inputs. You are probably missing a keyword or its value.')
end
varargin(1:2:end)=lower(varargin(1:2:end));

% Setting default parameter values.
cycleinc='linear';
winsize=[];
timesupport=7;  % Setting default of 7 temporal standard deviations for wavelet's length.

for n=1:2:numargin
    keyword=varargin{n};
    if strcmpi('cycleinc',keyword)
        cycleinc=varargin{n+1};
    elseif strcmpi('winsize',keyword)
        winsize=varargin{n+1};
        if ~mod(winsize,2)
            winsize=winsize+1; % Always set to odd length wavelets and hanning windows;
        end
    elseif strcmpi('timesupport',keyword)
        timesupport=varargin{n+1};
    else
        error(['What is ' keyword '? The only legal keywords are: type, cycleinc, winsize, or timesupport.'])
    end
end
if isempty(winsize) & cycles==0
    error('If you are using a Hanning tapered FFT, please supply the winsize input-pair.')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% compute number of cycles at each frequency
% ------------------------------------------
type='morlet';
if length(cycles) == 1 & cycles(1)~=0
    cycles = cycles*ones(size(freqs));
elseif length(cycles) == 2
    if strcmpi(cycleinc, 'log') % cycleinc
         cycles = linspace(log(cycles(1)), log(cycles(2)), length(freqs));
         cycles = exp(cycles);
         %cycles=logspace(log10(cycles(1)),log10(cycles(2)),length(freqs)); %rey
    else
        cycles = linspace(cycles(1), cycles(2), length(freqs));
    end;
end;
if cycles==0
    type='sinus';
end

sp=1/srate; % Rey added this line (i.e., sampling period).
% compute wavelet
for index = 1:length(freqs)
    fk=freqs(index);
    if strcmpi(type, 'morlet') % Morlet.
        sigf=fk/cycles(index); % Computing time and frequency standard deviations, resolutions, and normalization constant.
        sigt=1./(2*pi*sigf);
        A=1./sqrt(sigt*sqrt(pi));
        timeresol(index)=2*sigt;
        freqresol(index)=2*sigf;
        if isempty(winsize) % bases will be a cell array.
            tneg=[-sp:-sp:-sigt*timesupport/2];
            tpos=[0:sp:sigt*timesupport/2];
            t=[fliplr(tneg) tpos];
            psi=A.*(exp(-(t.^2)./(2*(sigt^2))).*exp(2*i*pi*fk*t));
            wavelet{index}=psi;  % These are the wavelets with variable number of samples based on temporal standard deviations (sigt).
        else % bases will be a matrix.
            tneg=[-sp:-sp:-sp*winsize/2];
            tpos=[0:sp:sp*winsize/2];
            t=[fliplr(tneg) tpos];
            psi=A.*(exp(-(t.^2)./(2*(sigt^2))).*exp(2*i*pi*fk*t));
            wavelet(index,:)=psi; % These are the wavelets with the same length.
            % This is useful for doing time-frequency analysis as a matrix vector or matrix matrix multiplication.
        end
    elseif strcmpi(type, 'sinus') % Hanning
        tneg=[-sp:-sp:-sp*winsize/2];
        tpos=[0:sp:sp*winsize/2];
        t=[fliplr(tneg) tpos];
        win = exp(2*i*pi*fk*t);
        wavelet(index,:) = win .* hanning(winsize)';
        %wavelet{index} = win .* hanning(winsize)';
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    end;
end;

% symmetric hanning function
function w = hanning(n)
if ~rem(n,2)
    w = .5*(1 - cos(2*pi*(1:n/2)'/(n+1)));
    w = [w; w(end:-1:1)];
else
    w = .5*(1 - cos(2*pi*(1:(n+1)/2)'/(n+1)));
    w = [w; w(end-1:-1:1)];
end