classdef collectiveVectorCal
    %vectorCal=collectiveVectorCal
    methods
        function B=normalizeAngBMinimizeDisToA(obj,A,B)
            %A is positive angle (0, 180), B is (-180, 180), e.g. A=70, B=-125,
            %we should get B to make the linear angle distance is the same as the
            %smaller circular distance, B could be -125 or 360+(-125)=235.
            %abs(70-235)=165 < abs(70-(-125))=195, so we should pick 235
            Bc=B-360*B./abs(B);
            for i=1:length(A)
                if abs(A(i)-B(i))>abs(A(i)-Bc(i))
                    B(i)=Bc(i);
                end
            end
        end
        function v2=RotateVec(obj,beta,v1)
            %this is right handed one, 
              %RotateVec(-90,[0,1])
              %[1,0]
             R3 = [cosd(beta) -sind(beta); sind(beta) cosd(beta)];
             v2 = R3 * (v1'); 
        end
        function v2=RotateVecArray(obj,beta,v1)
            
            theta=beta;
            v_OTex=v1;
            Predict_v=[cosd(theta).*v_OTex(:,1)-sind(theta).*v_OTex(:,2), sind(theta).*v_OTex(:,1)+cosd(theta).*v_OTex(:,2)];
              %beta=EncodingAng_alpha(i)- alpha(i);
             %R3 = [cosd(beta) -sind(beta); sind(beta) cosd(beta)];
             v2 = Predict_v;
        end
        function v2=RotateVecArrayLeft(obj,beta,v1)
            %this one is left handed, %RotateVecArrayLeft(90,[0,1])
              %[1,0]
            v2=obj.RotateVecArray(-beta,v1);
        end
        function [dist, dir]=getBearingofAB(obj,A,B)
        %[d1,a1]=vectorCal.getBearingofAB([1,0],[0,0])
        %d1=1 a1=0
        %[d1,a1]=vectorCal.getBearingofAB([1,1],[0,0])
        %d1=1.414 a1=-45
            vec = A-B;
            dist = sqrt(sum(vec.^2,2)); % i.e. sqrt(x^2 + y^2)
%             for i=1:size(vec,1)
%                 dir(i,:) = - atan2(vec(i,2),vec(i,1))*180/pi(); % i.e. atan(vec(2) / vec(1)) %clockwise is positive now
%             end
            dir=-atan2(vec(:,2),vec(:,1))*180/pi();
        end
        function leftAngle=RightToLeftAngle(obj,rightAngle) %right handed system,[0, 90 180 270] left handed [90 0 270 180]
            leftAngle=-rightAngle+90;
            leftAngle=obj.ChangeAngleTo_0to360(leftAngle);
        end
        function output=ChangeAngleTo_0to360(obj,A)
            tempI=find(A<0);
            A(tempI)=A(tempI)+360;
            tempI=find(A>360);
            A(tempI)=A(tempI)-360;
            output=A;
        end
        function output=ChangeAngleTo_negative180to180(obj,A)
            tempI=find(A<-180);
            A(tempI)=A(tempI)+360;
            tempI=find(A>180);
            A(tempI)=A(tempI)-360;
            output=A;
        end
        function angle=getangle_vAtovB(obj,vA,vB) % Vb is the reference vector, clockwise is positive here
            v_PO=vA;
            v_TP=vB;
            angle = atan2d(v_TP(:,1).*v_PO(:,2)-v_TP(:,2).*v_PO(:,1),v_TP(:,1).*v_PO(:,1)+v_TP(:,2).*v_PO(:,2));
        end
        function output=WorldToLocal(XY, XYorigin,YAxis)
            %just get the localtoworld first as we always do in vizard, the first
            %row is the X positive (1,0) in the world
            %using left multiply; note that if using right multiply using column
            %rather than row; vizard using right multiply
            r1=[1*cos(YAxis), 1*-sin(YAxis), 0];
            r2=[1*sin(YAxis), 1*cos(YAxis), 0];
            r3=[XYorigin(1),XYorigin(2),1];

            LtoW=[r1;r2;r3]; 

            WtoL=inv(LtoW);

            output=[XY, 1]* WtoL;
            %second row is the Z positive (0,1) in the world
            %the last row is origin (0,0) in the world


        end 
        function  [vector] = calculate_vector(self,A,B)
          vector = B - A;
        end
    end
end