"""
Geometry functions
"""
from typing import Tuple
import math

def min_radius_contraction_span_pos(
        d_open: float,
        d_closed: float,
        theta: float,
    ) -> Tuple[float, float]:
    """
    Calculates the position of the two ends of an actuator, whose fully opened
    length is `d_open`, closed length is `d_closed`, and whose motion spans a
    range `theta` (in radians). Returns (r, phi): If one end of the actuator is
    held at `(r, 0)`, then the other end will trace an arc `r` away from the
    origin with span `theta`

    Let `P` (resp. `P'`) be the position of the front of the actuator when its
    fully open (resp. closed), `Q` be the position of the back of the actuator,
    we note that `OP = OP' = OQ`.
    """
    assert d_open > d_closed
    assert 0 < theta < math.pi

    pq2 = d_open * d_open
    p_q2 = d_closed * d_closed
    # angle of PQP'
    psi = 0.5 * theta
    # |P-P'|, via the triangle PQP'
    pp_2 = pq2 + p_q2 - 2 * d_open * d_closed * math.cos(psi)
    r2 = pp_2 / (2 - 2 * math.cos(theta))
    # Law of cosines on POQ:
    phi = math.acos(1 - pq2 / 2 / r2)
    return math.sqrt(r2), phi

def min_tangent_contraction_span_pos(
        d_open: float,
        d_closed: float,
        theta: float,
    ) -> Tuple[float, float, float]:
    """
    Returns `(r, phi, r')` where `r` is the distance of the arm to origin, `r'`
    is the distance of the base to origin, and `phi` the angle in the open
    state.
    """
    assert d_open > d_closed
    assert 0 < theta < math.pi
    # Angle of OPQ = OPP'
    pp_ = d_open - d_closed
    pq = d_open
    p_q = d_closed

    a = (math.pi - theta) / 2
    # Law of sines on POP'
    r = math.sin(a) / math.sin(theta) * pp_
    # Law of cosine on OPQ
    oq = math.sqrt(r * r + pq * pq - 2 * r * pq * math.cos(a))
    # Law of sines on OP'Q. Not using OPQ for numerical reasons since the angle
    # `phi` could be very close to `pi/2`
    phi_ = math.asin(math.sin(a) / oq * p_q)
    phi = phi_ + theta
    assert theta <= phi < math.pi
    return r, phi, oq