
function f = HeadingPosition2Dpointing20231126(x,resp,Targets,method,boundary)
%resp is participants pointing directions, x is to be estimated posi_x posi_y and
%heading, Targets are the correct location of targests
%method could be 'leastsquarePointingError', '1-cossimilarity', 'meanabsolutePointingError'
       LargeConst=5*180^2;
       penalty=0;
       if size(Targets,2)==2
           Targets=[Targets(:,1),zeros(size(Targets,1),1),Targets(:,2)];
       end
        A=Targets(1,:);
        B=Targets(2,:);
        C=Targets(3,:);
        D=Targets(4,:);
        E=Targets(5,:);
        
        heading =x(3);
        
            
        p = [x(1),x(2)];
        
        %test if p is the same as any target
        %onwhichTarget=onTarget([p(1),0,p(2)],Targets)
        
        for j=1: length ( Targets(:,1) )
             target=Targets(j,:);
             dis=sqrt((p(1)-target(1))^2+(p(2)-target(3))^2);
            if dis<1
                 error(j) = LargeConst/dis*10;
                 show="###############################";
            else    
                bearing(j) = getbearingA (p, target,heading); %refer to the function of getbearingA
                v=angletoV(bearing(j));
                vresp=angletoV(resp(j));
 
                switch method
                    case 'leastsquarePointingError'
                        cost=(convertangleto180(bearing(j) - resp(j)))^2;
                    case '1-cossimilarity'
                        CosSimilarity=sum(v.* vresp,2)./sqrt(sum(v.^2,2).*sum(vresp.^2,2));
                        cost=1-mean(CosSimilarity); 
                    case 'meanabsolutePointingError'
                        cost=abs(convertangleto180(bearing(j) - resp(j)));
                    otherwise
                         print "wrong method";
                end
                error(j)=cost;
            end
        end 
        
        f = mean(error);
        
        %let's set a boundary and add penalty outside boundary
          XMax=boundary(2);%max([A(1),B(1),C(1),D(1),E(1)])*2;
          XMin=boundary(1);%min([A(1),B(1),C(1),D(1),E(1)]);
          YMax=boundary(4);%max([A(3),B(3),C(3),D(3),E(3)]);
          YMin=boundary(3);%min([A(3),B(3),C(3),D(3),E(3)]);

        % XMax=max([A(1),B(1),C(1),D(1),E(1)]);
        % XMin=min([A(1),B(1),C(1),D(1),E(1)]);
        % YMax=max([A(3),B(3),C(3),D(3),E(3)]);
        % YMin=min([A(3),B(3),C(3),D(3),E(3)]);
  
        
        if heading > 360 || heading<-360 || p(1)<XMin ||p(1)>XMax || p(2)<YMin ||p(2)>YMax
            penalty=LargeConst;
        end     

%         error=[errorA,errorB, errorC, errorD, errorE]; 
%        if(sum(isnan(error))>0)
%            a='isnan(error)##########################'
%        end
%        error(isnan(error)) = LargeConst;
         if sum(p==[A(1),A(3)])==2 || sum(p==[B(1),B(3)])==2 ||sum(p==[C(1),C(3)])==2||sum(p==[D(1),D(3)])==2||sum(p==[E(1),E(3)])==2
             penalty=LargeConst;
         end

         f = f + penalty;
end    
       


function close=isTooClose(p,target)
    close=0;    
    dis=sqrt((p(1)-target(1))^2+(p(3)-target(3))^2);
    if dis<1
        close=1;
    end 
end

function v=angletoV(ang)
    v=[cos(ang*pi/180),sin(ang*pi/180)];
end
    

    
    

            
        
        