% Make the latitude, longitude the center of the coordinate system.

%function ret=gps_locate(latitude_steps, longitude_steps, u_steps)

% Convert the coordinate system to Atlanta's coodrinates. Convert all three
% satellites to the new coordinates. In these new coordinates, z is
% pointing to the sky, x is pointing downwards and y is pointing west. 

function ret=hw8_brute_force(u)

global sat_pos;
global transformation_inverse;
global Atlanta_coords;
global PR;
global transformation_matrix;

sat_pos

x_steps=100;
y_steps=100;
z_steps=100;

x_start=-1.353266294249846e+004-1000;
y_start=2.833541766487528e+003-1000;
z_start=-11.78063648846000-5;

x_end=-1.353266294249846e+004+1000;
y_end=2.833541766487528e+003+1000;
z_end=-11.78063648846000+5;

sat_pos_transformed=transformation_matrix*sat_pos;
atl_pos_transformed=transformation_matrix*Atlanta_coords'

for i=1:3
    sat_pos_transformed(:,i)=sat_pos_transformed(:,i)-atl_pos_transformed;
end

sat_pos_transformed


% Large value.
min_error=1e20;

% for k=1:100
%     u=10^(u/10);
%     current_error=0;
%     for i=1:3
%         current_error=current_error+norm(sat_pos_transformed(i,:));
%     end
% end

%u1=norm(sat_pos_transformed(:,1))-PR(1);
%u2=norm(sat_pos_transformed(:,2))-PR(2);
%u3=norm(sat_pos_transformed(:,3))-PR(3);

ranges=PR+u;


min_error=1e20;

xx=1;
yy=1;
zz=1;

global error_vector;

for x=x_start:(x_end-x_start)/x_steps:x_end
    for y=y_start:(y_end-y_start)/y_steps:y_end
        for z=z_start:(z_end-z_start)/z_steps:z_end
            % compute error function
            current_error=0;
            for i=1:3
                %current_error=current_error+sum((sat_pos_transformed(:,i)-[x y z]').^2);
                error_temp=norm(sat_pos_transformed(:,i)-[x y z]')-ranges(i);
                current_error=current_error+abs(error_temp);
            end
            %error_vector(xx,yy,zz)=current_error;
            %current_error=abs(current_error - sum((PR + [u u u]).^2));
                if(current_error<min_error)
                    min_error=current_error;
                    location(1)=x;
                    location(2)=y;
                    location(3)=z;
            end         
            zz=zz+1;
        end
        yy=yy+1;
    end
    xx=xx+1;
end
location
ret=min_error
%min_error

location_normal=transformation_inverse*(location'+atl_pos_transformed);
[th, phi, rr]=cart2sph(location_normal(1), location_normal(2), location_normal(3));
% So far best is: 2.150939564583503e+007
% avg is 2.150698698977393e+007
% fminse 2.150821966717453e+007
% avg after feedback is 2.150698701907389e+007
% 2.150828632475963e+007 giving 0.6 error but height is negative.
phi*180/pi
th*180/pi
