%Test compare pseudorange
clear all
%%

%--- Include folders with functions ---------------------------------------
addpath ..\include             % The software receiver functions
addpath ..\geoFunctions        % Position calculation related functions
addpath ..

load trackingResults

navSolutions = postNavigation(trackResults, settings);
% plot(navSolutions.latitude,navSolutions.longitude,'o');
pseudorange=navSolutions.channel.rawP;
save pseudorange pseudorange
PRNsa=[];
for idxprn=1:length(trackResults)
    PRNsa=[PRNsa,trackResults(idxprn).PRN];
end;
%%
fid=fopen('C:\Users\Administrator\Google Drive\PhD\works\EstimatingCN0\GNSS_SIM\GenSatInfor\pseudorange.bin','rb');
NumMS=100000;
InitCodePhase=zeros(NumMS,6);
for ii=1:NumMS
    InitCodePhase(ii,:)=fread(fid,6,'double');
end;
InitCodePhase=InitCodePhase(6000:end,:);
startOffset=68.802;
speedOfLight=299792458;

PRNs=sort(PRNsa);
%PRNs=[8 15 17 26 27 28];
%PRNsa=[17 15 26 27 28];

%Re-arrange the actual pseudorange measurement
load pseudorange;
psa=[];
pseudorange=pseudorange';
for prn=1:length(PRNs)
    psa=[psa,pseudorange(:,find(PRNsa==PRNs(prn)))];
end;
step=500;
for idx=1:150
    tmpps=InitCodePhase((idx-1)*step+1,:)/speedOfLight*1000;
    minimum         = floor(min(tmpps));
    ps(idx,:)=(tmpps-minimum+startOffset)*speedOfLight/1000;
end;
%ps=ps(:,2:end);
figure;hold on;
plot(ps(:,1),'r--','LineWidth',2); hold on;
plot(psa(:,1),'b--');hold on;
plot(ps(:,2),'r-o','LineWidth',2); hold on;
plot(psa(:,2),'b-o');hold on;
plot(ps(:,3),'r-.','LineWidth',2); hold on;
plot(psa(:,3),'b-.');hold on;
plot(ps(:,4),'r','LineWidth',2); hold on;
plot(psa(:,4),'b');hold on;
plot(ps(:,5),'r-*','LineWidth',2); hold on;
plot(psa(:,5),'b-*');

figure;
sizea=min(size(psa,1),size(ps,1));
ps=ps(1:sizea,:);
psa=psa(1:sizea,:);
plot(ps(:,1)-psa(:,1),'r--','LineWidth',2); hold on;
plot(ps(:,2)-psa(:,2),'b-o','LineWidth',2); hold on;
plot(ps(:,3)-psa(:,3),'m-.','LineWidth',2); hold on;
plot(ps(:,4)-psa(:,4),'r','LineWidth',2); hold on;
plot(ps(:,5)-psa(:,5),'r-*','LineWidth',2); hold on;