classdef SphereContactPoint < handle 

  properties (Access = private)
    sphere 
    anchorPoint = []
    polarPoint = []
  end

  methods 
    function obj = SphereContactPoint(sphere)
      obj.sphere = sphere;
    end

    function set.sphere(obj, value)
      assert(isequal(class(value), "Sphere"), "Must be a Sphere instance");
      obj.sphere = value;
    end

    function set.anchorPoint(obj, value)
      assert(isnumeric(value) & isequal(size(value), [1 3]),...
        "Must be a 1x3 numeric array")
      assert(not(isequal(value, obj.sphere.center)),...
        "Anchor point can't be superposed with sphere center");
      obj.anchorPoint = value;
    end

    function set.polarPoint(obj, value)
      assert(isnumeric(value) & isequal(size(value), [1 3]),...
        "Must be a 1x3 numeric array")
      assert(not(Vector(obj.sphere.center, value).norm <= obj.sphere.radius),...
        "Polar point must be outside the sphere");
      obj.polarPoint = value;
    end

    function setSphere(obj, sphere)
      obj.sphere = sphere;
    end
    
    function setAnchorPoint(obj, point)
      obj.anchorPoint = point;
    end
    
    function setPolarPoint(obj, point)
      obj.polarPoint = point;
    end

    function output = getContactPoint(obj)
      assert(not(isempty(obj.anchorPoint)), "Anchor point missing");
      assert(not(isempty(obj.polarPoint)), "Polar point missing");
      assert(obj.sphereIsBetweenAnchorAndPolarPoints,...
        "There is no contact point");

      poleToCenter = Vector(obj.polarPoint, obj.sphere.center);
      anchorToCenter = Vector(obj.anchorPoint, obj.sphere.center);
      sliceNormal = poleToCenter.cross(anchorToCenter);

      poleToCenterPerpendicular = poleToCenter.cross(sliceNormal);
      % angle at vertex sphere.center between contact point and polar point
      contactPointsAngle = acos(obj.sphere.radius/poleToCenter.norm);

      firstContactPoint = obj.sphere.center...
        + obj.sphere.radius*sin(contactPointsAngle)*poleToCenterPerpendicular.direction...
        + obj.sphere.radius*cos(contactPointsAngle)*(-poleToCenter.direction);

      secondContactPoint = obj.sphere.center...
        + obj.sphere.radius*sin(contactPointsAngle)*(-poleToCenterPerpendicular.direction)...
        + obj.sphere.radius*cos(contactPointsAngle)*(-poleToCenter.direction);

      output = obj.getContactPointClosestToAnchor(...
        firstContactPoint, secondContactPoint);
    end

    function output = getContactPointClosestToAnchor(obj,...
        firstContactPoint, secondContactPoint)
      anchorToFirstContactPoint = Vector(obj.anchorPoint, firstContactPoint);
      anchorToSecondContactPoint = Vector(obj.anchorPoint, secondContactPoint);

      if anchorToFirstContactPoint.norm < anchorToSecondContactPoint.norm
        output = firstContactPoint;
      else
        output = secondContactPoint;
      end
    end

    function output = sphereIsBetweenAnchorAndPolarPoints(obj)
      poleToCenter = Vector(obj.polarPoint, obj.sphere.center);
      polarApertureAngle = abs(asin(obj.sphere.radius / poleToCenter.norm));
      poleToAnchor = Vector(obj.polarPoint, obj.anchorPoint);
      sphereCenterToAnchorAngle = angle(poleToCenter, poleToAnchor);

      poleToContactPointDistance =...
        sqrt(poleToCenter.norm^2 - obj.sphere.radius^2);

      output = (sphereCenterToAnchorAngle < polarApertureAngle)...
        & (poleToContactPointDistance < poleToAnchor.norm);
    end
  end

end