%%%%in real application, please change the builiding1,  builiding2,
%%%%builiding3,  builiding4,  builiding5 using the correct coordinates of
%%%%them
%please also use change the PointingDirection using participants' pointing
%responses to these five buildings
%Please also change the initialPosition using the correct position of
%participants

clear all 
close all

global building1 
global building2 
global building3 
global building4   
global building5 

building1 = [272, 0, 350]; %correct position of buildings 
building2 = [120, 0, -360];
building3 = [-200, 0, -300];
building4 = [-40, 0, -20];  
building5 = [100, 0, 350];


global PointingDirection % response pointing directions to five buildings
%the angles are specified egocentrically i.e. the response pointing direction relative to the correct
%testing direction of the participant, for exmaple, relative to an allocentric reference direction, the allocentric correct facing
%direction is 50 degree and the allocentric pointing direction is 51
%degree, then PointingDirection is 1

%in real applicaiton, add the real response pointing directions
%below are some hypothetical response pointing directions based on correct
%position P=[0,0], and correct heading H=30 plus some normal noise with
%a sigma, when sigma is 0, search should be perfect.

%we can use the following P and H to create some hypothetical correct
%pointing direction. Then add some noises to create the hypothetical
%response pointing direction.
P=[76.6,164.3]
H=50
%create some hypothetical correct pointing directions
bearingA = getbearingA (P,building1,H) 
bearingB = getbearingA (P,building2,H) 
bearingC = getbearingA (P,building3,H)
bearingD = getbearingA (P,building4,H)
bearingE = getbearingA (P,building5,H)
sigma=0;
%sigma=10;
PointingDirection=[
bearingA+sigma*randn() %building 1
bearingB+sigma*randn() %building 2
bearingC+sigma*randn() %building 3
bearingD+sigma*randn() %building 4
bearingE+sigma*randn() ]%building 5] 

% PointingDirection=[
% 4 %building 1
% 309 %building 2
% 172 %building 3
% 150 %building 4
% 91 ]%building 5] 

initialPosition = [0,0]; %the initial position for search. 
%We may use the correct testing position of participants as the initial
%searching location

% for any possible estimated heading, defined allocentrically, we search
% for the least square of pointing errors across the five buildings.
for i = 1:360 
    i;
    [x,fval] = fminsearch (@(x)myfun1(x,i),initialPosition); % search for the position leading to the least square of errors. The search funcition is enclosed in myfun1.m
    estpos_x_index(i) = x(1); % store X coordinate of the 360 positions
    estpos_y_index(i) = x(2); % Y coordinate of the 360 positions
    error1(i) = fval; % store all 360 least square error
end

%Across all 360 headings, we find the minimum of the least square of
%pointing errors
[min1,index1] = min(error1);
 
Hprime = index1  %esitmated heading
Pprime= [estpos_x_index(index1),estpos_y_index(index1)] % the estimated position


%visualization below
figure



%plot P
plot (P(1),P(2),'.k')
hold on
text(P(1),P(2),'P', 'Color','b');
%plot h
arrowLength=100;
ArrowsizeS=5;
quiver(P(1),P(2),arrowLength*sind(H),arrowLength*cosd(H), 'LineWidth',1, 'MaxHeadSize',ArrowsizeS, 'Color', 'k')
text(P(1)+arrowLength*sind(H),P(2)+arrowLength*cosd(H),'H','Color', 'k');
hold on

%plot Pprime
plot ([Pprime(1), building1(1), building2(1), building3(1), building4(1),building5(1)],[Pprime(2), building1(3), building2(3), building3(3), building4(3),building5(3)],'.k')
hold on
text(Pprime(1),Pprime(2),['P', char(39)],'Color', 'b');
text(building1(1),building1(3),'B1','Color', 'k');
text(building2(1),building2(3),'B2','Color', 'k');
text(building3(1),building3(3),'B3','Color', 'k');
text(building4(1),building4(3),'B4','Color', 'k');
text(building5(1),building5(3),'B5','Color', 'k');

%plot hprime
arrowLength=100;
ArrowsizeS=5;
quiver(Pprime(1),Pprime(2),arrowLength*sind(Hprime),arrowLength*cosd(Hprime), 'LineWidth',1, 'MaxHeadSize',ArrowsizeS, 'Color', 'b')
text(Pprime(1)+arrowLength*sind(Hprime),Pprime(2)+arrowLength*cosd(Hprime),['H', char(39)],'Color', 'b');
hold on

grid on
pbaspect([1 1 1])
axislim=600;
xlim([-axislim axislim])
ylim([-axislim axislim])