function [enm,dcb,Q]=tecdemo(t0,time,obs,iobs,nav,inav,posr,model,nmax,pos0)
% TECDEMO : TEC estimation demo.
%
% Estimate tec (total electron content) by least-sqaure adjustment using GPS
% dual-frequency code observables. Ionosphere is approximated as the single-
% layer model at the height of 350km with local or global tec model.
% Satellite/receiver dcbs (differential code biases) are estimated as well
% as tec model coefficients.
%
%          Copyright (C) 2006 by T.TAKASU, All rights reserved.
%
% argin  : t0    = date number by datenum.m (days)
%          time  = time vector relative to t0 (sec)
%          obs,iobs = observation data/indexes (see readrnx.m)
%          nav,inav = navigation messages/indexes (see readrnx.m)
%          posr  = station positions (m) [lat1,lon1;lat2,lon2,...] (rad)
%          model = tec model (0:local,1:global) (see tecmodel.m)
%          nmax  = max degree of tec model
%          pos0  = origin of local tec model [lat0,lon0] (rad)
%
% argout : enm   = seiralized tec model coefficients (see tecmodel.m)
%          dcb   = satellite/receiver dcbs (m)
%                  dcb(1:31)   : satellite dcbs
%                  dcb(32:end) : receiver dcbs
%          Q     = varience/covarience matrix
%
% version: $Revision: 3 $ $Date: 06/02/24 18:15 $
% history: 2006/02/22 1.1 new

ne=(nmax+1)*(nmax+1); nx=ne+31+size(posr,1);
x=zeros(nx,1); HH=zeros(nx); Hy=zeros(nx,1); tr=(time(1)+time(end))/2;

for k=1:length(time)
    i=find(round(iobs(:,1))==time(k)&iobs(:,2)>0);
    
    if ~isempty(i)
        % geometry-free code (PG=P1-P2)
        y=obs(i,3)-obs(i,4);
        
        % geometry-free code model
        [h,H,W]=gfmodel(t0,time(k),iobs(i,2),iobs(i,3),nav,inav,x(1:ne)',...
                        x(ne+1:end)',posr,model,pos0+[0,pi*(time(k)-tr)/43200]);
        
        % stack normal equations
        i=find(~isnan(y)&~isnan(h)); H=H(i,:); W=W(i,i);
        HH=HH+H'*W*H; Hy=Hy+H'*W*(y(i)-h(i));
    end
    disp(sprintf('t=%5d : n=%d',time(k),sum(~isnan(i))))
end
% add constraint of dcbr(1)==0
H=zeros(1,nx); H(ne+31+1)=1; sig=1E-3; HH=HH+H'*H/sig^2;

% solve normal equation
i=find(any(HH)); x(i)=x(i)+HH(i,i)\Hy(i); x(all(~HH))=nan;

enm=x(1:ne)'; dcb=x(ne+1:end)'; Q=zeros(nx); Q(i,i)=inv(HH(i,i));

% geometry-free code model -----------------------------------------------------
function [h,H,W]=gfmodel(t0,time,sats,rcvs,nav,inav,enm,dcb,posr,model,pos0)
f1=1.57542E9; f2=1.2276E9; K=(1/f1^2-1/f2^2)*40.3E16;
ne=length(enm); H=zeros(length(sats),ne+length(dcb));
for n=1:31, rs(:,n)=satpos(t0,time,nav(inav==n,:)); end
for n=1:length(sats)
    azel=satdir(rs(:,sats(n)),posr(rcvs(n),:));
    [posp,z]=pppos(azel,posr(rcvs(n),:));
    [tec,dtde]=tecmodel(time,posp,enm,model,pos0);
    h(n,1)=K/cos(z)*tec+dcb(sats(n))+dcb(31+rcvs(n));
    H(n,[1:ne,ne+sats(n),ne+31+rcvs(n)])=[K/cos(z)*dtde,1,1];
    sig(n)=0.3/sin(azel(2));
end
W=diag(1./sig.^2);

% ionospheric pierce point position --------------------------------------------
function [posp,z]=pppos(azel,posr)
RE=6370000; H=350000;
zr=pi/2-azel(2); z=asin(RE*sin(zr)/(RE+H));
posp(1)=asin(cos(zr-z)*sin(posr(1))+sin(zr-z)*cos(posr(1))*cos(azel(1)));
posp(2)=posr(2)+asin(sin(zr-z)*sin(azel(1)/cos(posp(1))));

% satellite azimuth/elevation angle --------------------------------------------
function azel=satdir(rs,posr)
RE=6370000;
sinp=sin(posr(1)); cosp=cos(posr(1)); sinl=sin(posr(2)); cosl=cos(posr(2));
E=[-sinl,cosl,0;-sinp*cosl,-sinp*sinl,cosp;cosp*cosl,cosp*sinl,sinp];
rs=E*(rs-RE*[cosp*cosl;cosp*sinl;sinp]);
azel=[atan2(rs(1),rs(2)),asin(rs(3)/norm(rs))];

% navigation messeage to satellite position ------------------------------------
function rs=satpos(t0,t,nav)
if isempty(nav), rs=[nan;nan;nan]; return, end
C=299792458; MU=3.986005E14; OMGE=7.292115167E-5;
tk=(t0-datenum(1980,1,6)-nav(:,28)*7)*86400+t-nav(:,18);
[tt,i]=min(abs(tk)); tk=tk(i); nav=nav(i,:);
A=nav(17)^2; e=nav(15); n=sqrt(MU/A^3)+nav(12); M=nav(13)+n*tk;
E=M; Ek=0; while abs(E-Ek)>1E-12, Ek=E; E=M+e*sin(Ek); end % solve kepler's eq.
p=atan2(sqrt(1-e^2)*sin(E),cos(E)-e)+nav(24);
r=[p;A*(1-e*cos(E));nav(22)+nav(26)*tk]+[nav([16,14;11,23;21,19])]*[sin(2*p);cos(2*p)];
OMG=nav(20)+(nav(25)-OMGE)*tk-OMGE*nav(18);
rs=r(2)*Rz(-OMG)*Rx(-r(3))*[cos(r(1));sin(r(1));0];

% coordinate rotation matrix ---------------------------------------------------
function R=Rx(t), R=[1,0,0;0,cos(t),sin(t);0,-sin(t),cos(t)];
function R=Rz(t), R=[cos(t),sin(t),0;-sin(t),cos(t),0;0,0,1];
