%this script describes how to calucate the errors in homing, position, and
%heading estimates from participants' replaced locations of O, X1 to X4.
%We only use concrete values of  X1 and X2 for demonstrations
%%get homing error
% homingError=b_POprime-b_PO; % homing error = b_PO' - b_PO
% %get positione error
% positionError1=(b_OX1-b_PO)-(b_OprimeX1prime-b_POprime);%position error = (b_OX1– b_PO) – (b_O’X1’– b_PO’)
% %headinError
% headingError1= positionError1-homingError;

close all;
clear all;
ArrowsizeB=1;
ArrowsizeS=0.3;
%the locations of the five objects
O=[0 0 0]; % origin
X1=[1 1 0]; % target 1
X2=[1 -1 0]; % target2
X3=[-1 -1 0]; %target3
X4=[-1 1 0]; %target4
Leg1direction=0; % define the first leg as the bearing 0 (reference direction, e.g. north) in the environment 
turnAngle=45; % the turning angle between the two legs 
Leg1=1.8; % the length of the first leg
Leg2=1.8; % the length of the second leg
T=[Leg1*sind(Leg1direction) Leg1*cosd(Leg1direction) 0]; % the coordinate of the turning point
P=T+[Leg2*sind(turnAngle) Leg2*cosd(turnAngle) 0]; % the testing position P
h=Leg1direction+turnAngle; % the testing heading h

%a model participant replaced the targets at OPrime, X1Prime, X2Prime, X3Prime,
%X4Prime
Oprime=[2 1.5 0]; % origin

%v = [-1 -12];
theta = 30;
R = [cosd(theta) -sind(theta) 0; sind(theta) cosd(theta) 0; 0 0 0];

noise=[0.5 0.5 0];
X1prime=X1*R+Oprime; % target 1
X2prime=X2*R+Oprime+noise; % target2
X3prime=X3*R+Oprime; %target3
X4prime=X4*R+Oprime; %target4

figure

%plot the path
plot ([O(1), T(1), P(1)],[O(2), T(2), P(2)],'k')
hold on
%text(O(1),O(2),'O','Color', 'k');
text(T(1),T(2),'T','Color', 'k');
text(P(1)+0.1,P(2),'P','Color', 'k');
%plot h
arrowLength=0.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=',num2str(h,4), char(176)]);
hold on

% the correct locations of the five objects
plot ([O(1), X1(1), X2(1), X3(1), X4(1)],[O(2), X1(2), X2(2), X3(2), X4(2)], '.k')
text(O(1)+0.1,O(2),'O','Color', 'k');
text(X1(1),X1(2),'X1','Color', 'k');
text(X2(1),X2(2),'X2','Color', 'k');
text(X3(1),X3(2),'X3','Color', 'k');
text(X4(1),X4(2),'X4','Color', 'k');
hold on
% the replaced locations of the five objects
plot ([Oprime(1), X1prime(1), X2prime(1), X3prime(1), X4prime(1)],[Oprime(2), X1prime(2), X2prime(2), X3prime(2), X4prime(2)], '.r')
text(Oprime(1)+0.1,Oprime(2),['O', char(39)],'Color', 'r');%['O', char(39)] means O'
text(X1prime(1),X1prime(2),['X1', char(39)], 'Color', 'r'); %['X1', char(39)] means X1'
text(X2prime(1),X2prime(2),['X2', char(39)],'Color', 'r');
text(X3prime(1),X3prime(2),['X3', char(39)], 'Color', 'r');
text(X4prime(1),X4prime(2),['X4', char(39)],'Color', 'r');

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


%%%%calcuate position error (b_OP’ – b_OP) and heading error (h'-h) based
%%%%on object1 (X1) and O 
%homing error = b_PO’ – b_PO = b_O'P - b_OP
%position error = b_OP’ – b_OP = (b_OX1– b_PO) – (b_O’X1’– b_PO’)
%heading error = h'-h= position error - homing error
%all bearings are specified in terms of b_OT which is 0
v_OT=T-O; % get reference vector which has the bearing 0 (b_OT).
v_PO=O-P; % the vector from P to O, should be opposite to the vector from O to P or bearings of them differ 180 degree
%v_OP=-v_PO;
v_POprime=Oprime-P;% the vector from P to O'
v_OX1=X1-O; % the vector from O to X1
v_OX2=X2-O; % the vector from O to X2
v_OX3=X3-O; % the vector from O to X3
v_OX4=X4-O; % the vector from O to X1
v_OprimeX1prime=X1prime-Oprime; % the vector from O' to X' 
v_OprimeX2prime=X2prime-Oprime; % the vector from O' to X' 
v_OprimeX3prime=X3prime-Oprime; % the vector from O' to X' 
v_OprimeX4prime=X4prime-Oprime; % the vector from O' to X' 
%u=v_OT; % u always the reference direction, i.e. bearing of it is 0


b_OT=atan2d(norm(cross(v_OT,v_OT)),dot(v_OT,v_OT)); % should be 0

cross1=cross(v_OT,v_PO); % correct homing bearing or direction
sign=-cross1(3)/abs(cross1(3)); % correct homing bearing or direction
b_PO=atan2d(sign*norm(cross(v_OT,v_PO)),dot(v_OT,v_PO)); % correct homing bearing or direction

cross1=cross(v_OT,v_POprime);
sign=-cross1(3)/abs(cross1(3));
b_POprime=atan2d(sign*norm(cross(v_OT,v_POprime)),dot(v_OT,v_POprime)); % response homing bearing or direction

cross1=cross(v_OT,v_OX1);
sign=-cross1(3)/abs(cross1(3));
b_OX1=atan2d(sign*norm(cross(v_OT,v_OX1)),dot(v_OT,v_OX1)); % correct bearing from O to target X1


cross1=cross(v_OT,v_OX2);
sign=-cross1(3)/abs(cross1(3));
b_OX2=atan2d(sign*norm(cross(v_OT,v_OX2)),dot(v_OT,v_OX2)); % correct bearing from O to target X1

cross1=cross(v_OT,v_OX3);
sign=-cross1(3)/abs(cross1(3));
b_OX3=atan2d(sign*norm(cross(v_OT,v_OX3)),dot(v_OT,v_OX3)); % correct bearing from O to target X1

cross1=cross(v_OT,v_OX4);
sign=-cross1(3)/abs(cross1(3));
b_OX4=atan2d(sign*norm(cross(v_OT,v_OX4)),dot(v_OT,v_OX4)); % correct bearing from O to target X1

cross1=cross(v_OT,v_OprimeX1prime);
sign=-cross1(3)/abs(cross1(3));
b_OprimeX1prime=atan2d(sign*norm(cross(v_OT,v_OprimeX1prime)),dot(v_OT,v_OprimeX1prime)); % the bearing from replaced location of O to replaced location of X1

cross1=cross(v_OT,v_OprimeX2prime);
sign=-cross1(3)/abs(cross1(3));
b_OprimeX2prime=atan2d(sign*norm(cross(v_OT,v_OprimeX2prime)),dot(v_OT,v_OprimeX2prime)); % the bearing from replaced location of O to replaced location of X2

cross1=cross(v_OT,v_OprimeX3prime);
sign=-cross1(3)/abs(cross1(3));
b_OprimeX3prime=atan2d(sign*norm(cross(v_OT,v_OprimeX3prime)),dot(v_OT,v_OprimeX3prime)); % the bearing from replaced location of O to replaced location of X3

cross1=cross(v_OT,v_OprimeX4prime);
sign=-cross1(3)/abs(cross1(3));
b_OprimeX4prime=atan2d(sign*norm(cross(v_OT,v_OprimeX4prime)),dot(v_OT,v_OprimeX4prime)); % the bearing from replaced location of O to replaced location of X2


% plot all bearings
%quiver(P(1),P(2),O(1)-P(1),O(2)-P(2), 'LineWidth',1, 'MaxHeadSize',10)
shiftLeft=0.5
text((0.5*O(1)+0.5*T(1))-shiftLeft,(0.5*O(2)+0.5*T(2)),['b_OT',char(39),'=',num2str(b_OT,4), char(176)] ,'Interpreter','none');
hold on

%plot b_PO

%quiver(P(1),P(2),O(1)-P(1),O(2)-P(2), 0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS,'Color', 'k');
quiver(P(1),P(2),O(1)-P(1),O(2)-P(2), 0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS,'Color', 'k');
text((0.7*P(1)+0.3*O(1))-shiftLeft,(0.7*P(2)+0.3*O(2)),['b_PO=',num2str(b_PO,4),char(176), ], 'Interpreter','none');
hold on
%plot b_PO'
quiver(P(1),P(2),Oprime(1)-P(1),Oprime(2)-P(2),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS,'Color', 'r');
text((0.5*P(1)+0.5*Oprime(1)),(0.5*P(2)+0.5*Oprime(2)),['b_PO',char(39),'=',num2str(b_POprime,4), char(176)] ,'Interpreter','none');
hold on
%plot b_OX1
quiver(O(1),O(2),X1(1)-O(1),X1(2)-O(2), 0,  'LineWidth',1, 'MaxHeadSize',ArrowsizeS,'Color', 'k');
text((0.5*O(1)+0.5*X1(1))-shiftLeft,(0.5*O(2)+0.5*X1(2)),['b_OX1=',num2str(b_OX1,4), char(176)] ,'Interpreter','none');
hold on


%plot b_OX2
quiver(O(1),O(2),X2(1)-O(1),X2(2)-O(2),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS, 'Color', 'k');
text((0.5*O(1)+0.5*X2(1))-shiftLeft,(0.5*O(2)+0.5*X2(2)),['b_OX2=',num2str(b_OX2,4), char(176)] ,'Interpreter','none');
hold on

%plot b_OX3
%quiver(O(1),O(2),X3(1)-O(1),X3(2)-O(2),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS, 'Color', 'k');
%text((0.5*O(1)+0.5*X3(1))-shiftLeft,(0.5*O(2)+0.5*X3(2)),['b_OX3=',num2str(b_OX3,4), char(176)] ,'Interpreter','none');
hold on

%plot b_OX4
%quiver(O(1),O(2),X4(1)-O(1),X4(2)-O(2),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS, 'Color', 'k');
%text((0.5*O(1)+0.5*X4(1)-shiftLeft),(0.5*O(2)+0.5*X4(2)),['b_OX4=',num2str(b_OX4,4), char(176)] ,'Interpreter','none');
hold on


%plot b_O'X1'
quiver(Oprime(1),Oprime(2),X1prime(1)-Oprime(1),X1prime(2)-Oprime(2),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS, 'Color', 'r');
text((0.5*Oprime(1)+0.5*X1prime(1))-shiftLeft,(0.5*Oprime(2)+0.5*X1prime(2)),['b_O',char(39),'X1',char(39),'=',num2str(b_OprimeX1prime,4), char(176)] ,'Interpreter','none','Color', 'r' );
hold on

%plot b_O'X2'
quiver(Oprime(1),Oprime(2),X2prime(1)-Oprime(1),X2prime(2)-Oprime(2),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS, 'Color', 'r');
text((0.5*Oprime(1)+0.5*X2prime(1))-shiftLeft,(0.5*Oprime(2)+0.5*X2prime(2)),['b_O',char(39),'X2',char(39),'=',num2str(b_OprimeX2prime,4), char(176)] ,'Interpreter','none','Color', 'r' );
hold on


%get homing error
homingError=b_POprime-b_PO; % homing error = b_PO' - b_PO
%get positione error
positionError1=(b_OX1-b_PO)-(b_OprimeX1prime-b_POprime);%position error = b_OP’ – b_OP = (b_OX1– b_PO) – (b_O’X1’– b_PO’)
%headinError
headingError1= positionError1-homingError;

%plot b_OP' and h', the length of v_OP' is scaled to be the length of v_PO'
b_OP=b_PO-180;
b_OPprime1=positionError1+b_OP; %b_OP'=position error + b_OP

%length_OPprime1/length_OX1 = length_OprimeP/length_OprimeX1prime
%or norm(v_OPprime1)/norm(v_OX1)=norm(v_POprime)/norm(v_OprimeX1prime)
%
norm_v_OPprime1=norm(v_POprime)/norm(v_OprimeX1prime)*norm(v_OX1)


Pprime1=[O(1)+norm_v_OPprime1*sind(b_OPprime1) O(2)+norm_v_OPprime1*cosd(b_OPprime1)];
quiver(O(1),O(2),norm_v_OPprime1*sind(b_OPprime1),norm_v_OPprime1*cosd(b_OPprime1),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS,'Color', 'b');
hold on
%text((0.5*Oprime(1)+0.5*X1prime(1))-shiftLeft,(0.5*Oprime(2)+0.5*X1prime(2)),['b_O',char(39),'X1',char(39),'=',num2str(b_OprimeX1prime,4), char(176)] ,'Interpreter','none');
%text(P(1),P(2),'P');
text(Pprime1(1)+0.1,Pprime1(2),['P',char(39),'1'],'Color', 'b');
%text(Pprime(1),Pprime(2),'P');
hold on
hprime1=h+headingError1;%h'=h+ heading error
quiver(Pprime1(1),Pprime1(2),arrowLength*sind(hprime1),arrowLength*cosd(hprime1),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS,'Color', 'b');
text(Pprime1(1)+arrowLength*sind(hprime1),Pprime1(2)+arrowLength*cosd(hprime1),['h',char(39),'1'], 'Color', 'b');
hold on

%plot h to show the angle between h'1 and h
quiver(Pprime1(1),Pprime1(2),arrowLength*sind(h),arrowLength*cosd(h),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS,'Color', 'k');
text(Pprime1(1)+arrowLength*sind(h),Pprime1(2)+arrowLength*cosd(h),'h', 'Color', 'k');
hold on


%do the same thing based on X2, X3, X4
%get positione error
positionError2=(b_OX2-b_PO)-(b_OprimeX2prime-b_POprime);%position error = b_OP’ – b_OP = (b_OX1– b_PO) – (b_O’X1’– b_PO’)
%headinError
headingError2= positionError2-homingError;

positionError3=(b_OX3-b_PO)-(b_OprimeX3prime-b_POprime);%position error = b_OP’ – b_OP = (b_OX1– b_PO) – (b_O’X1’– b_PO’)
%headinError
headingError3= positionError3-homingError;

positionError4=(b_OX4-b_PO)-(b_OprimeX4prime-b_POprime);%position error = b_OP’ – b_OP = (b_OX1– b_PO) – (b_O’X1’– b_PO’)
%headinError
headingError4= positionError4-homingError;

%CosTheta = dot(u,v)/(norm(u)*norm(v));

%ThetaInDegrees = acosd(CosTheta);

%b_PO=atan((O(2)-P(2))/(O(1)-P(1)));
b_OPprime2=positionError2+b_OP;

norm_v_OPprime2=norm(v_POprime)/norm(v_OprimeX2prime)*norm(v_OX2)


Pprime2=[O(1)+norm_v_OPprime2*sind(b_OPprime2) O(2)+norm_v_OPprime2*cosd(b_OPprime2)];
quiver(O(1),O(2),norm_v_OPprime2*sind(b_OPprime2),norm_v_OPprime2*cosd(b_OPprime2),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS,'Color', 'b');
hold on
%text((0.5*Oprime(1)+0.5*X1prime(1))-shiftLeft,(0.5*Oprime(2)+0.5*X1prime(2)),['b_O',char(39),'X1',char(39),'=',num2str(b_OprimeX1prime,4), char(176)] ,'Interpreter','none');
%text(P(1),P(2),'P');
text(Pprime2(1)+0.1,Pprime2(2),['P',char(39),'2'],'Color', 'b');
%text(Pprime(1),Pprime(2),'P');
hold on
hprime2=h+headingError2;%h'=h+ heading error
quiver(Pprime2(1),Pprime2(2),arrowLength*sind(hprime2),arrowLength*cosd(hprime2),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS,'Color', 'b');
text(Pprime2(1)+arrowLength*sind(hprime2),Pprime2(2)+arrowLength*cosd(hprime2),['h',char(39),'2'], 'Color', 'b');
hold on

%plot h to show the angle between h'2 and h
quiver(Pprime2(1),Pprime2(2),arrowLength*sind(h),arrowLength*cosd(h),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS,'Color', 'k');
text(Pprime2(1)+arrowLength*sind(h),Pprime2(2)+arrowLength*cosd(h),'h', 'Color', 'k');
hold on


textPosition=[-3.8 -1.3]
text(textPosition(1), textPosition(2),[char(8736),'OPO’ = b_PO',char(39),' – b_PO =','(', num2str(b_POprime,4),')','-', '(', num2str(b_PO,4),')', '=', num2str(homingError,4), char(176)] ,'Interpreter','none');
hold on

text(textPosition(1), textPosition(2)-0.3,[char(8736),'POP’1= (b_OX1– b_PO) – (b_O’X’1– b_PO’)=','((', num2str(b_OX1,4),')','-','(',num2str(b_PO,4),'))','-', '((', num2str(b_OprimeX1prime,4),')','-','(',num2str(b_POprime,4),'))', '=', num2str(positionError1,4), char(176)] ,'Interpreter','none');
hold on
text(textPosition(1), textPosition(2)-1.2,[char(8736),'POP’2 = (b_OX2– b_PO) – (b_O’X2’– b_PO’)=','((', num2str(b_OX2,4),')','-','(',num2str(b_PO,4),'))','-', '((', num2str(b_OprimeX2prime,4),')','-','(',num2str(b_POprime,4),'))', '=', num2str(positionError2,4), char(176)] ,'Interpreter','none');
hold on


text(textPosition(1), textPosition(2)-0.5,[char(951),'1=',char(8736),'POP1',char(39),'-',char(8736),'OPO',char(39)','= (', num2str(positionError1,4),')','-','(',num2str(homingError,4),')', '=', num2str(headingError1,4), char(176)] ,'Interpreter','none');
hold on
text(textPosition(1), textPosition(2)-1.4,[char(951),'2=',char(8736),'POP2',char(39),'-',char(8736),'OPO',char(39)','= (', num2str(positionError2,4),')','-','(',num2str(homingError,4),')', '=', num2str(headingError2,4), char(176)] ,'Interpreter','none');
hold on

% text(textPosition(1), textPosition(2)-0.7,['b_OP',char(39),'1 = position error1+b_OP =','(', num2str(positionError1,4),')','+','(',num2str(b_OP,4),')', '=', num2str(b_OPprime1,4), char(176)] ,'Interpreter','none');
% hold on
% text(textPosition(1), textPosition(2)-1.6,['b_OP',char(39),'2 = position error2+b_OP =','(', num2str(positionError2,4),')','+','(',num2str(b_OP,4),')', '=', num2str(b_OPprime2,4), char(176)] ,'Interpreter','none');
% hold on
% 
% text(textPosition(1), textPosition(2)-0.9,['h',char(39),'1 = heading error1+h =','(', num2str(headingError1,4),')','+','(',num2str(h,4),')', '=', num2str(hprime1,4), char(176)] ,'Interpreter','none');
% hold on
% text(textPosition(1), textPosition(2)-1.8,['h',char(39),'2 = heading error2+h =','(', num2str(headingError2,4),')','+','(',num2str(h,4),')', '=', num2str(hprime2,4), char(176)] ,'Interpreter','none');
% hold on