function [enm,dcbr]=tecdemo(t0,time,obs,iobs,nav,inav,posr,pos0,nmax)
% TECDEMO : TEC estimation demo.
%
%          Copyright (C) 2006 by T.TAKASU, All rights reserved.
%
% version: $Revision:$ $Date:$
% history: 2006/02/22 1.1 new

ne=nmax*nmax; nx=ne+size(posr,1); x=zeros(nx,1); HH=zeros(nx); Hy=zeros(nx,1);

for k=1:length(time)
    i=find(round(iobs(:,1))==time(k));
    
    if ~isempty(i)
        % geometry-free code (P1-P2)
        y=obs(i,3)-obs(i,4);
        
        % geometry-free code model
        [h,H]=gfmodel(t0,time(k),x,iobs(i,2),iobs(i,3),nav,inav,posr,pos0,nmax);
        
        % stack normal equations
        i=find(~isnan(y)&~isnan(h)); H=H(i,:);
        HH=HH+H'*H; Hy=Hy+H'*(y(i)-h(i));
    end
end
% solve normal equation
i=find(any(HH)); x(i)=x(i)+HH(i,i)\Hy(i);

enm=x(1:ne)'; dcbr=x(ne+1:end)';

% geometry-free code model -----------------------------------------------------
function [h,H]=gfmodel(t0,t,x,sats,rcvs,nav,inav,posr,pos0,nmax)
C=299792458; f1=1.57542E9; f2=1.2276E9; K=(1/f1^2-1/f2^2)*40.3E16;
ne=nmax*nmax; H=zeros(length(sats),ne+size(posr,1));
for n=1:31, [rs(:,n),tgd(n)]=satpos(t0,t,nav(inav==n,:)); end
dcbs=C*(tgd-mean(tgd(~isnan(tgd))))*(1-f1^2/f2^2);
for n=1:length(sats)
    azel=satdir(rs(:,sats(n)),posr(rcvs(n),:));
    [posp,z]=pppos(azel,posr(rcvs(n),:)*pi/180);
    [tec,dtde]=tecmodel(posp*180/pi,pos0,x(1:ne),nmax);
    h(n,1)=K/cos(z)*tec-x(ne+rcvs(n))+dcbs(sats(n));
    H(n,1:ne)=K/cos(z)*dtde; H(n,ne+rcvs(n))=-1;
end

% 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)*pi/180); cosp=cos(posr(1)*pi/180);
sinl=sin(posr(2)*pi/180); cosl=cos(posr(2)*pi/180);
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,tgd]=satpos(t0,t,nav)
if isempty(nav), rs=[nan;nan;nan]; tgd=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]);nav([11,23]);nav([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];
tgd=nav(32);

% 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];
