function [xyzM,dtsvS] = GpsEph2Xyz(gpsEph,gpsTime)
%[xyzM,dtsvS] = GpsEph2Xyz(gpsEph,gpsTime)
% Calculate sv coordinates, in ECEF frame, at ttx = gpsTime
%
% Inputs:
% gpsEph: vector of GPS ephemeris structures, as defined in ReadRinexNav.m
%
% gpsTime = [gpsWeek, ttxSec]: GPS time at time of transmission, ttx
%   ttx = trx - PR/c - dtsvS, where trx is time of reception (receiver clock),
%     dtsvS is the satellite clock error (seconds), can be computed in advance 
%     using Eph2Dtsv.m or iterating this function: gps time = sat time - dtsvS
% gpsWeek, ttxSec must be vectors of length(gpsEph), 
%
% outputs:
%   xyzM = [i,j,k] matrix of coordinates of satellites (ecef meters)
%   dtsvS = vector of satellite clock error (seconds)
% The row dimension of xyzM, dtsvS = length(gpsEph)
%
% xyzM = sat positions at time ttxSec, in terms of ecef coords at the same time
% Use FlightTimeCorrection.m to get xyzM & v in ecef coords at time of reception
%
% See  IS-GPS-200 for details of data

%Author: Frank van Diggelen
%Open Source code for processing Android GNSS Measurements

xyzM=[]; dtsvS=[]; 

[bOk,gpsEph,gpsWeek,ttxSec] = CheckGpsEphInputs(gpsEph,gpsTime);
if ~bOk
    return
end
p=length(gpsEph);
%Now we are done checking and manipulating the inputs
% the time vectors: gpsWeek, ttxSec are the same length as gpsEph
%-------------------------------------------------------------------------------

%set fitIntervalSeconds
fitIntervalHours = [gpsEph.Fit_interval]';
%Rinex says "Zero if not known", so adjust for zeros
fitIntervalHours(fitIntervalHours == 0) = 2;
fitIntervalSeconds = fitIntervalHours*3600; 

%Extract variables from gpsEph, into column vectors
%orbit variable names follow RINEX 2.1 Table A4
%clock variables af0, af1, af2 follow IS GPS 200
TGD     = [gpsEph.TGD]';
Toc     = [gpsEph.Toc]';
af2     = [gpsEph.af2]';
af1     = [gpsEph.af1]';
af0     = [gpsEph.af0]';
Crs     = [gpsEph.Crs]';
Delta_n = [gpsEph.Delta_n]';
M0      = [gpsEph.M0]';
Cuc     = [gpsEph.Cuc]';
e       = [gpsEph.e]';
Cus     = [gpsEph.Cus]';
Asqrt   = [gpsEph.Asqrt]';
Toe     = [gpsEph.Toe]';
Cic     = [gpsEph.Cic]';
OMEGA   = [gpsEph.OMEGA]';
Cis     = [gpsEph.Cis]';
i0      = [gpsEph.i0]';
Crc     = [gpsEph.Crc]';
omega   = [gpsEph.omega]';
OMEGA_DOT=[gpsEph.OMEGA_DOT]';  
IDOT    = [gpsEph.IDOT]';
ephGpsWeek = [gpsEph.GPS_Week]';
    
%Calculate dependent variables -------------------------------------------------

%Time since time of applicability accounting for weeks and therefore rollovers
%subtract weeks first, to avoid precision errors:
tk =(gpsWeek-ephGpsWeek)*GpsConstants.WEEKSEC + (ttxSec-Toe);

I = find(abs(tk)>fitIntervalSeconds);
if ~isempty(I)
    numTimes = length(I);
    fprintf(sprintf('WARNING in GpsEph2Xyz.m, %d times outside fit interval.',...
        numTimes));
end

A = Asqrt.^2;    %semi-major axis of orbit
n0=sqrt(GpsConstants.mu./(A.^3));    %Computed mean motion (rad/sec)
n=n0+Delta_n;      %Corrected Mean Motion
h = sqrt(A.*(1-e.^2).*GpsConstants.mu); 
Mk=M0+n.*tk;     %Mean Anomaly
Ek=Kepler(Mk,e);  %Solve Kepler's equation for eccentric anomaly

%Calculate satellite clock bias (See ICD-GPS-200 20.3.3.3.3.1)
%subtract weeks first, to avoid precision errors:
dt =(gpsWeek-ephGpsWeek)*GpsConstants.WEEKSEC + (ttxSec-Toc);

%Calculate satellite clock bias
sin_Ek=sin(Ek);
cos_Ek=cos(Ek);
dtsvS = af0 + af1.*dt + af2.*(dt.^2)  + ...
    GpsConstants.FREL.*e.*Asqrt.*sin_Ek -TGD;

%true anomaly:
vk=atan2(sqrt(1-e.^2).*sin_Ek./(1-e.*cos_Ek),(cos_Ek-e)./(1-e.*cos_Ek));
Phik=vk + omega;   %Argument of latitude          

sin_2Phik=sin(2*Phik);
cos_2Phik=cos(2*Phik);
% The next three terms are the second harmonic perturbations
duk = Cus.*sin_2Phik + Cuc.*cos_2Phik;   %Argument of latitude correction
drk = Crc.*cos_2Phik + Crs.*sin_2Phik;   %Radius Correction
dik = Cic.*cos_2Phik + Cis.*sin_2Phik;   %Correction to Inclination

uk = Phik + duk;  %Corrected argument of latitude
rk = A.*((1-e.^2)./(1+e.*cos(vk))) + drk; %Corrected radius
ik = i0 + IDOT.*tk + dik; %Corrected inclination

sin_uk=sin(uk);
cos_uk=cos(uk);
xkp = rk.*cos_uk; % Position in orbital plane
ykp = rk.*sin_uk; % Position in orbital plane

% Wk = corrected longitude of ascending node, 
Wk  = OMEGA + (OMEGA_DOT - GpsConstants.WE).*tk - GpsConstants.WE*[gpsEph.Toe]';

%for dtflight, see FlightTimeCorrection.m
sin_Wk = sin(Wk);
cos_Wk = cos(Wk);

xyzM=zeros(p,3);
% The matrix xyzM contains the ECEF coordinates of position
sin_ik=sin(ik);
cos_ik=cos(ik);
xyzM(:,1)=xkp.*cos_Wk-ykp.*cos_ik.*sin_Wk;
xyzM(:,2)=xkp.*sin_Wk+ykp.*cos_ik.*cos_Wk;
xyzM(:,3)=ykp.*sin_ik;                                       

end %end of function GpsEph2Xyz
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright 2016 Google Inc.
% 
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
% 
%     http://www.apache.org/licenses/LICENSE-2.0
% 
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.