close all
clear all
myData1=[ %subject 78, sheet1, path 2, 
%each row for one object, first two columns are original correct
%locations(x, y), columns 3 and 4 replaced locations (x, y)
0	-0.9	-0.09877862	-9.59E-05	;
 -1	0.1	-0.307525933	0.623188257	;
  1	0.1	0.720635533	0.18612583	;
 1	-1.9	0.987047017	-0.928970456	;
  -1 -1.9	-0.796069264 -0.301873654	;
 %estimated Position X Z in column AW, AY; estimated heading error in BB;
 %estimated position error in gama or column AS, in Lei's analysis she
 %called position error Oprime e.g. in sheet2
 %heading error Hprime
 %if we only use first row and one other row, we can get same position
 %error and heading error in sheet1 BB and AS
 %if we use all rows, we can get very similar result in sheet 2
];

myData=myData1
P=[-1.378879998, 1.157017697] % subject 78, sheet1, path 2, testing position
h=-50 % testing heading

output=bi_regressV2(myData(:,3),myData(:,4),myData(:,1),myData(:,2),P(:,1),P(:,2)) %using replaced object locations to predict the original locations, then use P to predict P'
hprime=h-output.regression(5) %column BC

O=[myData1(1,1), myData1(1,2)]
Pprime=output.dependent
[dis1,OPprime]=getBearingofAB(O,Pprime) %in the excel OPprime is the error rather than the OPprime, hprime is the heading error as well
[dis2,OP]=getBearingofAB(O,P)
HeadingError=-output.regression(5) 
PositionAngularError=(OPprime-OP)
PositionLengthError=dis1-dis2

%visualize(myData, P, h, Pprime, h', output)
visualize(myData, P, h, Pprime, hprime, output)

function [dist, dir]=getBearingofAB(A,B)

vec = A-B
dist = sqrt(sum(vec.^2)) % i.e. sqrt(x^2 + y^2)
dir = - atan(vec(2)/vec(1))*180/pi() % i.e. atan(vec(2) / vec(1)) %clockwise is positive now
end

function visualize(myData, P, h, Pprime, hprime, output)
 
            
        %save the predicted original O, X1-X4 so we can visualize the
        %distortation for each path
        alpha1=output.regression(3)
        alpha2=output.regression(4)
        beta1=output.regression(1)
        beta2=output.regression(2)
        A=myData(:,3)
        B=myData(:,4)
        Ap=alpha1+beta1*A-beta2*B
        Bp=alpha2+beta2*A+beta1*B

        %predictedXY=[Ap,Bp]

        correctX= myData(:,1)
        correctY= myData(:,2)
        responseX=A
        responseY=B
        predX=Ap
        predY=Bp
        PX=P(1,1)
        PY=P(1,2)
        PprimeX=Pprime(1,1)
        PprimeY=Pprime(1,2)
        
        p1=plot (correctX, correctY,'r*')
        hold on
        p2=plot (responseX, responseY,'k*')
        hold on
        p3=plot (predX, predY,'b*')
        hold on
        
        plot (correctX, correctY,'r')
        hold on
        plot (responseX, responseY,'k')
        hold on
        plot (predX, predY,'b')
        hold on
        
        OX=correctX(1)
        OY=correctY(1)
        text(OX+0.1,OY,'O', 'Color', 'r');
        hold on
        
        OprimeX=responseX(1)
        OprimeY=responseY(1)
        text(OprimeX+0.1,OprimeY,['O',char(39)], 'Color', 'k');
        hold on
        
%         OpredX=predX(1)
%         OpredY=predY(1)
%         text(OpredX+0.1,OpredY,'Opred', 'Color', 'b');
%         hold on
        
        
        %text(responseX(1)+0.1,responseY(1),['O',char(39)],'Color', 'k');%['O', char(39)] means O'
        plot (PX, PY, 'k+')
        text(PX+0.1,PY,'P', 'Color', 'k');
        hold on
        %plot h
        arrowLength=0.5;
        ArrowsizeS=1;
        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 (PprimeX, PprimeY,'b+')
        text(PprimeX+0.1, PprimeY,['P',char(39)], 'Color', 'b');
        %text(PprimeX+arrowLength*sind(hprime),PprimeY,+arrowLength*cosd(hprime),['h',char(39),'1'], 'Color', 'r');
        hold on
        
        quiver(PprimeX, PprimeY,arrowLength*sind(hprime),arrowLength*cosd(hprime),  0, 'LineWidth',1, 'MaxHeadSize',ArrowsizeS,'Color', 'b');
        text(PprimeX+arrowLength*sind(hprime),PprimeY+arrowLength*cosd(hprime),['h',char(39)], 'Color', 'b');
        hold on
        
        legend([p1 p2 p3],{'Original locations','Replaced locations', 'Predicted locations'})
        
        
        grid on
        pbaspect([1 1 1])
        axislim=5;
        xlim([-axislim axislim])
        ylim([-axislim axislim])  
end