clear all;
%(25.73N,79.30W)  Target answer
samplerate = 100;
Rearth = 6378.137e3; %meters
Rsat = Rearth +850e3;

c = 3e8;

Vsat = 7.42e3; %meters/second
Vx = Vsat*sin(9*pi/180); %meters/second
Vy = Vsat*cos(9*pi/180); %meters/second

load DopplerData.txt;

time = DopplerData(1,:);
Fr = DopplerData(2,:);

smalltime = time(1:samplerate:length(time));
smallFr = Fr(1:samplerate:length(Fr));

diffFr = diff(smallFr);
difftime = diff(smalltime);

plot(smalltime(1:(length(smalltime)-1)),diffFr./difftime);

fc = (Fr(1)+Fr(length(Fr)))/2;


%I divide by 10, round and multiply by 10 to ensure fc is a multiple of 10
zerodop_time1 = time(find(Fr == ceil(fc/10)*10));  %seconds
zerodop_time2 = time(find(Fr == floor(fc/10)*10));
zerodop_time = sum([zerodop_time1,zerodop_time2])/(length(zerodop_time1)+length(zerodop_time2));

lambda = c/fc;  %meters

s = zerodop_time*Vy;

latitude = s/Rsat*180/pi;
 
thetaR = acos((fc-Fr)*lambda/Vy);
 
tt = time(451:9450);
vt = (Fr(451:9450) - fc) * lambda;
 
long = -77;
D = 5;
i = 0;
while D > 1E-6
    i = i + 1;
    x = plotpath3d(latitude,long,tt,vt);
    if x > 0
        long = long + D;
    else
        long = long - D;
    end
    D = D/2;
end
long

