function [r] = Sun(JD,AUkm)
%Converts relative position of sun from JD in either AU or km
%Vallado Pg 266

T_UT1 = (JD-2451545)/36525;
T_TDB = T_UT1;
L_M = 280.460 + 36000.7700*T_TDB;
M = 357.52772333 + 35999.05344*T_TDB;
L_ec = L_M + 1.914666471 * sind(M) + 0.019994643 * sind(2*M);
r_s = 1.000140612 - .016708617*cosd(M) - 0.000139589 * cosd(2*M);
e = 23.439291 - 0.0130042 * T_TDB;

AU = 149597870.7;

if AUkm == 1
    p = AU.*[r_s*cosd(L_ec);r_s*cosd(e)*sind(L_ec);r_s*sind(e)*sind(L_ec)];
else
    p = [r_s*cosd(L_ec);r_s*cosd(e)*sind(L_ec);r_s*sind(e)*sind(L_ec)];
end

[yyyy, mm, dd, hh, min, ss] = invjday(JD);
JD_new = cal2mjd(yyyy,mm,dd,hh,min,ss+37+32.184);
T_JD = (JD_new-2451545)/36525;

r = MoD(p,T_JD);

end

function ToD2MoD = MoD(r,T_UT1)
% Converts ToD to MoD

a = 2306.2181 * T_UT1/3600 + 0.30188 * T_UT1^2 + 0.017998 * T_UT1^3;
b = 2004.3109 * T_UT1/3600 - 0.42665 * T_UT1^2 - 0.041833 * T_UT1^3;
c = 2306.2181 * T_UT1/3600 + 1.09468 * T_UT1^2 + 0.018203 * T_UT1^3;

% GCRF = rot(a,3) * rot(-b,2) * rot(c,3) * r;
GCRF = rot(a,3) * rot(-b,2) * rot(c,3) * r;
B = rot(0.0146/3600,3) * rot(0.16617/3600,2) * rot(-0.0068192/3600,1);

ToD2MoD = B\GCRF;
end

function [mat] = rot(phi,num)
%Gives rotation matrix given angle and number
%Must be in radians

phi = deg2rad(phi);
mat = zeros(3);

    if num == 1
        mat = [1 0 0;0 cos(phi) sin(phi);0 -sin(phi) cos(phi)];
    elseif num == 2
        mat = [cos(phi) 0 -sin(phi);0 1 0;sin(phi) 0 cos(phi)];
    elseif num == 3
        mat = [cos(phi) sin(phi) 0;-sin(phi) cos(phi) 0;0 0 1];
    end

end

function [JD,MJD] = cal2mjd(yyyy,mm,dd,hh,min,ss)
%Converts calendar time to MJD

cal = 1721013.5;
cal1 = cal - 2400000.5;

JD = 367*yyyy-floor((7*(yyyy+floor((mm+9)/12)))/4)+floor(275*mm/9)+dd+cal+...
    (hh+(min+ss/60)/60)/24;

MJD = 367*yyyy-floor((7*(yyyy+floor((mm+9)/12)))/4)+floor(275*mm/9)+dd+cal1+...
    (hh+(min+ss/60)/60)/24;

end

function [year, month, day, hr, min, sec] = invjday(jd)
z = fix(jd + .5);
fday = jd + .5 - z;
if (fday < 0)
   fday = fday + 1;
   z = z - 1;
end
if (z < 2299161)
   a = z;
else
   alpha = floor((z - 1867216.25) / 36524.25);
   a = z + 1 + alpha - floor(alpha / 4);
end
b = a + 1524;
c = fix((b - 122.1) / 365.25);
d = fix(365.25 * c);
e = fix((b - d) / 30.6001);
day = b - d - fix(30.6001 * e) + fday;
if (e < 14)
   month = e - 1;
else
   month = e - 13;
end
if (month > 2)
   year = c - 4716;
else
   year = c - 4715;
end
hr = abs(day-floor(day))*24;
min = abs(hr-floor(hr))*60;
sec = abs(min-floor(min))*60;
day = floor(day);
hr = floor(hr);
min = floor(min);

end