function [satPos, satClkBias] = calSat(eph, rcvr, oldPos);
%---------------------------------------------------------
% Calculate satellite position based on Ephemeris data and 
% receiver measurement data
% Ju-Yong Do,  Feb 20, 2004
%---------------------------------------------------------
% input  : Ephemeris data, receiver data
% output : satellite position in WGS format, clk bias
% 01/20/09: convert to SGMP format
%=========================================================
%=========================================================
mu          = 3.986005e14;          %WGS84 earth's gravitational const
oedot       = 7.2921151467e-5;      %WGS84 earth rotation rate
c           = 2.99792458e8;         %speed of light
F           = -4.442807633e-10;     %const for relavistic correction [sec/sqrt(m)]
accLmt      = 1e-8;                 %accuracy limit for eccentric anomaly

%=========================================================
%Read ephemerius data and receiver data
%=========================================================
%Allocate to individual variables
svid        = [eph(:).prn];         %satellite prn # (1-32)
toc         = [eph(:).toc];         %reference time of clk parameter[s]
toe         = [eph(:).toe];         %reference time of ephemeris parameter[s]
af0         = [eph(:).af0];         %clk correction coef [s]
af1         = [eph(:).af1];         %clk correction coef [s/s]
af2         = [eph(:).af2];         %clk correction coef [s/s/s]
ura         = [eph(:).ura];         %user range accuracy [m]
e           = [eph(:).e];           %eccentricity
sqrta       = [eph(:).sqrta];       %sqrt(semi-major axis)
dn          = [eph(:).dn];          %mean motion correction [rad/s]
m0          = [eph(:).m0];          %mean anomalyat ref time [rad]
w           = [eph(:).w];           %argument of perigee [rad]
omg0        = [eph(:).omg0];        %longitude of ascending node [rad]
i0          = [eph(:).i0];          %inclination angles at ref time [rad]
odot        = [eph(:).odot];        %rate of right ascension [rad/s]
idot        = [eph(:).idot];        %rate of right ascension [rad/s]
cus         = [eph(:).cus];         %argument of latitude correciton, sine [rad]
cuc         = [eph(:).cuc];         %argument of latitude correciton, cosine [rad]
cis         = [eph(:).cis];         %inclination correciton, sine [rad]
cic         = [eph(:).cic];         %inclination correciton, cosine [rad]
crs         = [eph(:).crs];         %radius correciton, sine [rad]
crc         = [eph(:).crc];         %radius correciton, cosine [rad]
iod         = [eph(:).iode];        %issue of data #
tgd         = [eph(:).tgd];         %group delay

rcvr_tow    = rcvr.tow;             %receiver time of week [s]
pr          = [rcvr.tx.prL1];       %L1 pseudorange [m]

%=========================================================
%correct satellite clock bias
%=========================================================
a       = sqrta.^2;                 %semi-major axis
n0      = sqrt(mu./a.^3);           %computed mean motion
n       = n0 + dn;                  %corrected mean motion
tTX     = rcvr_tow - pr./c;         %TX time
t       = tTX - toe;                %time from ephemeris ref epoch
% -302400<= t <= 302400
if t > 302400, t = t - 604800;
elseif t < -302400, t = t + 604800;
end

%=========================================================
%calculate clock bias terms (relativistic term, group delay, clock drift)

%relativistic correction term
m   = m0 + n.*t;                    %mean anomaly
E   = m;                            %initialize E to m
for iter=1:1e3
    d = m - (E-e.*sin(E));
    if abs(d) <accLmt, break, end; 
    E = E + d;                      %update E by difference
end
dtr     = F.*e.*sqrta.*sin(E);      %relativistic correction term

%clock drift
clkDrift= af0 + af1.*(tTX - toc) + af2.*(tTX - toc).^2;

%=========================================================
%overall satellite clock bias in sec and corrected tx time
satClkBias = clkDrift + dtr - tgd;  %clk bias [sec]
t          = t-satClkBias;          %time from ephemeris ref epoch
if t > 302400, t = t - 604800;
elseif t < -302400, t = t + 604800;
end


%=========================================================
%Derive coordinate from Ephemeris data
%=========================================================
%eccentric anomaly (m= E - e.*sin(E)) w.iteration       
m   = m0 + n.*t;                    %mean anomaly
E   = m;                            %initialize E to m
for iter=1:1e3
    d = m - (E-e.*sin(E));
    if abs(d) <accLmt, break, end; 
    E = E + d;                      %update E by difference
end

%true anomaly
nu      = atan2( sqrt(1-e.^2).*sin(E), (cos(E)-e) );
phi     = nu + w;                   %argument of latitude

%2nd harmonic perturbations correction
du      = cus.*sin(2*phi) + cuc.*cos(2*phi); %latitude
dr      = crs.*sin(2*phi) + crc.*cos(2*phi); %radius
di      = cis.*sin(2*phi) + cic.*cos(2*phi); %inclination

%correct latitude/radius/inclination
u       = phi + du;
r       = a.*(1-e.*cos(E)) + dr;
i       = i0 + idot.*t + di;

%positions in orbital plane
x_dash  = r.*cos(u);
y_dash  = r.*sin(u);

%corrected longitude of ascending node
omg     = omg0 + (odot-oedot).*t - oedot.*toe;

%earth-fixed coordinate
x       = (x_dash.*cos(omg) - y_dash.*cos(i).*sin(omg))';
y       = (x_dash.*sin(omg) + y_dash.*cos(i).*cos(omg))';
z       = (y_dash.*sin(i))';

%compensation for earth rotation
range      = (sum(([x y z] - ones(rcvr.numtx,1)*oldPos).^2,2)).^0.5; 
rotAng     = oedot.*range/c;
x          = x + y.*rotAng;
y          = y - x.*rotAng;


%=========================================================
% Return satellite coordinate, clk bias
%=========================================================
satPos      = [x y z];
satClkBias  = satClkBias'*c;%clock bias in meter

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% % Sky plot
% figure
% plot3(satPos(:,1)/6300e3 ,satPos(:,2)/6300e3,satPos(:,3)/6300e3,'rv'); grid on; hold on;
% % plot3(usrPos(1)/6300e3,usrPos(2)/6300e3,usrPos(3)/6300e3,'k*'); grid on;
% sphere;    axis([-5 5 -5 5 -5 5]);% square; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


return;

%=========================================================
%=========================================================
% x       = x.*cos(rotAng) + y.*sin(rotAng);
% y       = y.*cos(rotAng) - x.*sin(rotAng);
