classdef CoordinateSystem < handle
% Useful to define, use, and test a coordinate system.
%
% Can be tested on orthogonality, right or left handedness.
%
% Can be used to express global points in local coordinates
% and vice versa.
% Can be used to project points on planes spanned by the
% CoordinateSystem axes.
  
  properties
    origin = [];
  end


  properties (Hidden = true)
    xAxis = [];
    yAxis = [];
    zAxis = [];

    rotationMatrix = [];
  end



  methods

    function obj = CoordinateSystem()
    end

    function output = isEmpty(obj)
      output = isempty(obj.origin) ||...
               isempty(obj.xAxis) ||...
               isempty(obj.yAxis) ||...
               isempty(obj.zAxis);
    end

    function set.xAxis(obj,value)
      obj.xAxis = value;
      obj.rotationMatrix(:,1) = value'/norm(value);
    end

    function set.yAxis(obj,value)
      obj.yAxis = value;
      obj.rotationMatrix(:,2) = value'/norm(value);
    end

    function set.zAxis(obj,value)
      obj.zAxis = value;
      obj.rotationMatrix(:,3) = value'/norm(value);
    end

    function output = getRotationMatrix(obj)
      assert(obj.isOrthogonal,"CoordinateSystem is not orthogonal. Can't get its rotation matrix");
      output = obj.rotationMatrix;
    end

    function localPoints = express(obj,globalPoints)
      % Give the coordinates of global cartesian points in present coordinate system
      assert(not(obj.isEmpty),'Coordinate System has not been initialized yet.');
      assert(size(globalPoints,2) == 3,'Points have to be a Nx3 double array.')
      localPoints = (globalPoints-obj.origin) * obj.rotationMatrix;
    end

    function globalPoints = getGlobalCoordinatesOfLocalPoints(obj,localPoints)
        % Give the coordinates of local points in global cartesian coordinate system
        assert(not(obj.isEmpty),'Coordinate System has not been initialized yet.');
        assert(size(localPoints,2) == 3,'Points have to be a Nx3 double array.')
        globalPoints = (localPoints * obj.rotationMatrix') + obj.origin;
    end

    function projected = projectOnXYPlane(obj,points)
      assert(not(obj.isEmpty),'Coordinate System has not been initialized yet.');
      projectionPlane = Plane;
      projectionPlane.fit([obj.origin;(obj.origin + obj.xAxis);(obj.origin + obj.yAxis)]);
      projected = projectionPlane.projectOnPlane(points);
    end

    function projected = projectOnYZPlane(obj,points)
      assert(not(obj.isEmpty),'Coordinate System has not been initialized yet.');
      projectionPlane = Plane;
      projectionPlane.fit([obj.origin;(obj.origin + obj.yAxis);(obj.origin + obj.zAxis)]);
      projected = projectionPlane.projectOnPlane(points);
    end

    function projected = projectOnZXPlane(obj,points)
      assert(not(obj.isEmpty),'Coordinate System has not been initialized yet.');
      projectionPlane = Plane;
      projectionPlane.fit([obj.origin;(obj.origin + obj.zAxis);(obj.origin + obj.xAxis)]);
      projected = projectionPlane.projectOnPlane(points);
    end

    function output = isOrthogonal(obj)
      % The axes comparison can't be done looking for strict equality due to
      % rounded values. Therefore the values must be evaluated with a given
      % tolerance
      assert(not(obj.isEmpty),'Coordinate System has not been initialized yet.');
      minAngle = (pi/2)-(10^-6); % Arbitrarily defined as a deviation of
                                 % a millionth of a radian
      if dot(obj.xAxis,obj.yAxis) > norm(obj.xAxis)*norm(obj.yAxis)*cos(minAngle)
        output = false;
        return
      end
      if dot(obj.xAxis,obj.zAxis) > norm(obj.xAxis)*norm(obj.zAxis)*cos(minAngle)
        output = false;
        return
      end
      if dot(obj.zAxis,obj.yAxis) > norm(obj.zAxis)*norm(obj.yAxis)*cos(minAngle)
        output = false;
        return
      end
      output = true;
    end

    function output = isRightHanded(obj)
      % The comparison between a theoretical right-handed axis and the actual
      % z Axis can't be done looking for strict vector equality due to rounded
      % values. Therefore the norm of the sum of the two vectors is evaluated.
      assert(not(obj.isEmpty),'Coordinate System has not been initialized yet.');
      if not(obj.isOrthogonal)
        output = false;
        return
      end
      rightHandedAxis = cross(obj.xAxis,obj.yAxis);
      if norm(rightHandedAxis+obj.zAxis) < norm(rightHandedAxis)
        output = false;
        return
      end
      output = true;
    end

    function output = isLeftHanded(obj)
      assert(not(obj.isEmpty),'Coordinate System has not been initialized yet.');
      if not(obj.isOrthogonal)
        output = false;
        return
      end
      if obj.isRightHanded
        output = false;
        return
      end
      output = true;
    end

    function output = plot(obj,axesColor,center)
      axisLength = 50;
      if (center)
        origin = [0 0 0];
      else
        origin = obj.origin;
      end

      X = [origin; origin + obj.xAxis*axisLength];
      Y = [origin; origin + obj.yAxis*axisLength];
      Z = [origin; origin + obj.zAxis*axisLength];

      plotHandle(1) = plot3(X(:,1),X(:,2),X(:,3),'Color',axesColor,'LineWidth', 3);
      hold on
      % set the point a bit further to plot axis name
      X(2,:) = X(2,:) + obj.xAxis*2;
      plotHandle(2) = text(X(2,1),X(2,2),X(2,3),'X');

      plotHandle(3) = plot3(Y(:,1),Y(:,2),Y(:,3),'Color',axesColor,'LineWidth', 3);
      % set the point a bit further to plot axis name
      Y(2,:) = Y(2,:) + obj.yAxis*2;
      plotHandle(4) = text(Y(2,1),Y(2,2),Y(2,3),'Y');

      plotHandle(5) = plot3(Z(:,1),Z(:,2),Z(:,3),'Color',axesColor,'LineWidth', 3);
      % set the point a bit further to plot axis name
      Z(2,:) = Z(2,:) + obj.zAxis*2;
      plotHandle(6) = text(Z(2,1),Z(2,2),Z(2,3),'Z');
      
      
      output = plotHandle;
    end

  end



end