%Orbit Kepler position velocity
% Richard Rieber
% November 9, 2006
% rrieber@gmail.com
% 
% Revision 8/21/07: Added H1 line for lookfor functionality
%
% function [R,V] = KeplerCOE(Ro,Vo,dT)
%
% Purpose: This function calculates position and velocity
%          at a given time based on an initial position
%          and velocity of an orbiting object.
%
% Inputs:  Ro - Initial position of length 3 (km)
%          Vo - Initial velocity of length 3 (km/s)
%          dT - A time step at which to calculate the new R and V vectors (sec)
%          U  - Gravitational constant of body being orbited (km^3/s^2).  Default is Earth
%               at 398600.4415 km^3/s^2.  OPTIONAL
%
% Outputs: R - Position at time dT of length 3 (km)
%          V - Velocity at time dT of length 3 (km/s)
%
% NOTE:  This function uses the subfunction CalcEA.m, randv.m, and elorb.m

function [R,V] = KeplerCOE(Ro,Vo,dT,U)

if nargin < 3 || nargin > 4
    error('Not enough inputs.  See help KeplerCOE')
elseif nargin == 3
    U = 398600.4415;  %km^3/s^2 Gravitational Constant of Earth
elseif length(Ro) ~= 3
    error('Position vector must be of length 3.  See help KeplerCOE')
elseif length(Vo) ~= 3
    error('Velocity vector must be of length 3.  See help KeplerCOE')
end


% Calculating kepler orbital elements at given position
[a,ecc,inc,O,w,nu] = elorb(Ro,Vo,mu); %[km, *, rad, rad, rad, rad]
% note: * = unitless

% Mean motion of orbit
n = (U/a^3)^.5; %rad/s

% Calculating the Eccentric anomaly
if ecc ~= 0
    Eo = atan2(sin(nu)*(1-ecc^2),ecc+cos(nu)); %rad
else
    Eo = nu; %rad
end

% Calculating the initial mean anomaly, the final mean
% anomaly, and final eccentric anomaly
if ecc < 1
    Mo = Eo-ecc*sin(Eo); %rad
    M = Mo + n*dT; %rad
    E = CalcEA(M,ecc,10^-14); %rad
end

% Calculating the final true anomaly
if ecc ~= 0
    nu2 = atan2(sin(E)*(1-ecc^2)^.5,cos(E)-ecc); %rad
else
    nu2 = M; %rad
end

% Calculating the position and velocity based on the 
% orbital elements
[R,V] = randv(a,ecc,inc,O,w,nu2); %[km, km/s]