% created by LXK, 07-2021
%check \delta N uniqueness

clc
clear all

c_speed=3*10^8;
freq=1.57542*10^9;
lambda=c_speed/freq;

L=5;
W=1.8;
diag=sqrt(L^2+W^2);


S1=[1.5652450413949063e+07  -1.7820307704090886e+07   1.1926028269351177e+07];
S2=[6.6016569514994835e+06  -1.8673212455288142e+07   1.7822871502396956e+07];
S3=[1.3049909966452010e+07  -1.4146197210042311e+07  -1.8015218142967664e+07];
S4=[1.7247866225499839e+07  -2.0098200643532686e+07   3.6648442186015109e+06];
S5=[1.7603270965926457e+07   1.4269920753808914e+07   1.3541254719449662e+07 ];
S6=[2.3495593358080521e+07  -8.1879806646612510e+06  -8.9184970384220146e+06 ];
S7=[6.4630593703701170e+06  -4.0865205898711658e+06  -2.5433248510946646e+07 ];
S8=[1.8457439812680721e+07   1.9117327654520113e+07   1.0593823334453671e+05 ];

sat(1,:)=S1;
sat(2,:)=S2;
sat(3,:)=S3;
sat(4,:)=S4;


lat0=38.885982; %ENU origin, (0, 0, 0)
lont0=121.524142;
alt0=40;

% antenna coordinate in local ENU coordinate system
enu_Ant1=[0 0 0];
enu_Ant2=[5 0 0];
enu_Ant3=[5 1.8 0];
enu_Ant4=[0 1.8 0];

% convert the users' enu coordinates to ecef coordinates
u1_pos_true=lla2ecef([lat0 lont0 alt0]);
% u1_pos_true=[X1 Y1 Z1];
[X1 Y1 Z1]=enu2ecef(enu_Ant1(1), enu_Ant1(2), enu_Ant1(3), lat0, lont0, alt0, wgs84Ellipsoid,'degrees');
u1=[X1 Y1 Z1];

[X2 Y2 Z2]=enu2ecef(enu_Ant2(1), enu_Ant2(2), enu_Ant2(3), lat0, lont0, alt0, wgs84Ellipsoid,'degrees');
u2_pos_true=[X2 Y2 Z2];
[X3 Y3 Z3]=enu2ecef(enu_Ant3(1), enu_Ant3(2), enu_Ant3(3), lat0, lont0, alt0, wgs84Ellipsoid,'degrees');
u3_pos_true=[X3 Y3 Z3];
[X4 Y4 Z4]=enu2ecef(enu_Ant4(1), enu_Ant4(2), enu_Ant4(3), lat0, lont0, alt0, wgs84Ellipsoid,'degrees');
u4_pos_true=[X4 Y4 Z4];

z1=0; 
z2=0;
z3=0;
z4=0;
i_indx=0;

n_ant=4;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%compute the true  Delta N
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

clear ant sat_to_ant1

  for j=1:1:4  % satellite 1-4
       sat_to_ant1(j,1)=floor(norm(sat(j,:)-u1_pos_true)/lambda);
       sat_to_ant1(j,2)=floor(norm(sat(j,:)-u2_pos_true)/lambda);
       sat_to_ant1(j,3)=floor(norm(sat(j,:)-u3_pos_true)/lambda);
       sat_to_ant1(j,4)=floor(norm(sat(j,:)-u4_pos_true)/lambda);
    end 

    % compute \Delta N
    for j=1:1:4  % satellite 1-4
        ind=1;
        for k=1:1:n_ant % antenna 1-4
            for n=k+1:n_ant  %antenna 
            delta_true_N(j,ind)=sat_to_ant1(j,k)-sat_to_ant1(j,n);
            ind=ind+1;
            end 
        end 
    end
disp '-------- The true single differenced phase integer ambibuities at the true positions are below----'
delta_true_N

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% find all possible fitted geometry
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
step=1; % default 1
range=5; % default 10: to reduce the running time, can be changed to 5


i=0;
for x1=-range:step:range
    for y1=-range:step:range
        i=i+1;
        A1(i,:)=enu_Ant1+[x1 y1 z1];
        A2(i,:)=enu_Ant2+[x1 y1 z1];
        A3(i,:)=enu_Ant3+[x1 y1 z1];
        A4(i,:)=enu_Ant4+[x1 y1 z1];
    end
end
total_N=(floor(2*range/step)+1)^2;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% consiering 4 antennas, take too long time
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if n_ant==4
    for i1=1:total_N  % 4 antennas, antenna 1
        for i2=1:total_N  % 4 antennas, antenna 2
            for i3=1:total_N % 4 antennas, antenna 3
                for i4=1:total_N  % 4 antennas, antenna 4
                    d1=norm(A1(i1,:)-A2(i2,:));
                    d2=norm(A1(i1,:)-A3(i3,:));
                    d3=norm(A1(i1,:)-A4(i4,:));
                    d4=norm(A2(i2,:)-A3(i3,:));
                    d5=norm(A2(i2,:)-A4(i4,:));
                    d6=norm(A3(i3,:)-A4(i4,:));
                    center_cor1=(A1(i1,:)+A3(i3,:))/2;
                    center_cor2=(A2(i2,:)+A4(i4,:))/2;
                    d7=norm(center_cor1-center_cor2); %should be 0
                    %d8=norm(center_cor2); %should be 0
                    d1_diff=abs(d1-L);
                    d2_diff=abs(d2-diag);
                    d3_diff=abs(d3-W);
                    d4_diff=abs(d4-W);
                    d5_diff=abs(d5-diag);
                    d6_diff=abs(d6-L);
                    sum_d=d1_diff+d2_diff+d3_diff+d4_diff+d5_diff+d6_diff+d7; %+d8;
                    if sum_d<0.000000000000000000000001 % should be =0
                        i_indx=i_indx+1;
                        A_record(i_indx,:)=[A1(i1,:) A2(i2,:) A3(i3,:) A4(i4,:)];
                    end
                 end
            end
        end
    end

    disp '-------- The searched points where the gemotry formed by antennas meets the specified shape are below----'
    A_record

    %save('antena_possible_pos.txt', 'A_record', '-ascii')

    % delta N: differened ambiguity
    % satellite to antenna 1 and 2


    % with the fitted geometry of antennas positioning, compute the Delta N
    [mm nn]=size(A_record);

    for r=1:1:mm

        [X1 Y1 Z1]=enu2ecef(A_record(r,1), A_record(r,2), A_record(r,3), lat0, lont0, alt0, wgs84Ellipsoid,'degrees');
        ant(1,:)=[X1 Y1 Z1];
        [X2 Y2 Z2]=enu2ecef(A_record(r,4), A_record(r,5), A_record(r,6), lat0, lont0, alt0, wgs84Ellipsoid,'degrees');
        ant(2,:)=[X2 Y2 Z2];
        [X3 Y3 Z3]=enu2ecef(A_record(r,7), A_record(r,8), A_record(r,9), lat0, lont0, alt0, wgs84Ellipsoid,'degrees');
        ant(3,:)=[X3 Y3 Z3];
        [X4 Y4 Z4]=enu2ecef(A_record(r,10), A_record(r,11), A_record(r,12), lat0, lont0, alt0, wgs84Ellipsoid,'degrees');
        ant(4,:)=[X4 Y4 Z4];

       for j=1:1:4  % satellite 1-4
            for k=1:1:n_ant % antenna 1-4
                sat_to_ant1(j,k)=floor(norm(sat(j,:)-ant(k,:))/lambda);
            end 
       end 

        % compute \Delta N
        for j=1:1:4  % satellite 1-4
            ind=1;
            for k=1:1:n_ant % antenna 1-4
                for n=k+1:n_ant  %antenna 
                delta_N(j,ind)=sat_to_ant1(j,k)-sat_to_ant1(j,n);
                ind=ind+1;
                end 
            end 
        end

    disp '-------- The singled differenced phase integers yieled by those antennas meeting the geometry shape----'
    delta_N
    delta_N_diff=norm(delta_true_N-delta_N);

    if delta_N_diff==0
        disp '-------------- a fitted geometry found--see the above delta_N-----------^^^^^^^^'
        pos_index=r
        coordinate_true_point=[enu_Ant1 enu_Ant2 enu_Ant3 enu_Ant4]
        coordinate_searched_point=A_record(pos_index,:)
        distance=norm(enu_Ant1-A_record(pos_index,1:3))+ norm(enu_Ant2-A_record(pos_index,4:6))+norm(enu_Ant3-A_record(pos_index,7:9))+norm(enu_Ant4-A_record(pos_index,10:12));
        fprintf('>>>>>>>>The searched point with the identical group has the distance to the true point: %f\n', distance);
        %disp '-------------- a fitted geometry found--see the above delta_N-----------^^^^^^^^'
    end
    end

elseif n_ant==3   %%%%%%%%%%%%%%%%%%%%% three antennas to reduce the running time

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % consiering 3 antennas
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    for i1=1:total_N  % 4 antennas, antenna 1
        for i2=1:total_N  % 4 antennas, antenna 2
            for i3=1:total_N % 4 antennas, antenna 3
                    d1=norm(A1(i1,:)-A2(i2,:));
                    d2=norm(A1(i1,:)-A3(i3,:));
                    d3=norm(A2(i2,:)-A3(i3,:));
                    d1_diff=abs(d1-L);
                    d2_diff=abs(d2-diag);
                    d3_diff=abs(d3-W);
                    sum_d=d1_diff+d2_diff+d3_diff; %+d8;
                    if sum_d<0.000000000000000000000001 % should be =0
                        i_indx=i_indx+1;
                        A_record(i_indx,:)=[A1(i1,:) A2(i2,:) A3(i3,:)];
                    end

            end
        end
    end

    disp '-------- The searched points where the gemotry formed by antennas meets the specified shape are below----'
    A_record

    %save('antena_possible_pos.txt', 'A_record', '-ascii')

    % delta N: differened ambiguity
    % satellite to antenna 1 and 2
    [mm nn]=size(A_record);

    % determine the Delta N with 
    for r=1:1:mm

        [X1 Y1 Z1]=enu2ecef(A_record(r,1), A_record(r,2), A_record(r,3), lat0, lont0, alt0, wgs84Ellipsoid,'degrees');
        ant(1,:)=[X1 Y1 Z1];
        [X2 Y2 Z2]=enu2ecef(A_record(r,4), A_record(r,5), A_record(r,6), lat0, lont0, alt0, wgs84Ellipsoid,'degrees');
        ant(2,:)=[X2 Y2 Z2];
        [X3 Y3 Z3]=enu2ecef(A_record(r,7), A_record(r,8), A_record(r,9), lat0, lont0, alt0, wgs84Ellipsoid,'degrees');
        ant(3,:)=[X3 Y3 Z3];

       for j=1:1:4  % satellite 1-4
            for k=1:1:n_ant % antenna 1-3
                sat_to_ant1(j,k)=floor(norm(sat(j,:)-ant(k,:))/lambda);
            end 
       end 

        % compute \Delta N
        for j=1:1:4  % satellite 1-4
            ind=1;
            for k=1:1:n_ant % antenna 1-3
                for n=k+1:n_ant  %antenna 
                delta_N(j,ind)=sat_to_ant1(j,k)-sat_to_ant1(j,n);
                ind=ind+1;
                end 
            end 
        end

    delta_N

    delta_N_diff=norm(delta_true_N-delta_N);

    if delta_N_diff==0
        disp '-------------- a fitted geometry found--see the above delta_N-----------^^^^^^^^'
        pos_index=r
        coordinate_true_point=[enu_Ant1 enu_Ant2 enu_Ant3]
        coordinate_searched_point=A_record(pos_index,:)
        distance=norm(enu_Ant1-A_record(pos_index,1:3))+ norm(enu_Ant2-A_record(pos_index,4:6))+norm(enu_Ant3-A_record(pos_index,7:9));
        fprintf('>>>>>>>>The searched point with the identical group has the distance to the true point: %f\n', distance);

    end
    end

end