"""
Geometry functions
"""
from typing import Tuple, Optional
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

def contraction_span_pos_from_radius(
        d_open: float,
        d_closed: float,
        theta: float,
        r: Optional[float] = None,
        smaller: bool = True,
    ) -> Tuple[float, float, float]:
    """
    Returns `(r, phi, r')`

    Set `smaller` to false to use the other solution, which has a larger
    profile.
    """
    if r is None:
        return min_tangent_contraction_span_pos(
            d_open=d_open,
            d_closed=d_closed,
            theta=theta)
    assert 0 < theta < math.pi
    assert d_open > d_closed
    assert r > 0
    # Law of cosines
    pp_ = r * math.sqrt(2 * (1 - math.cos(theta)))
    d = d_open - d_closed
    assert pp_ > d, f"Triangle inequality is violated. This joint is impossible: {pp_}, {d}"
    assert d_open + d_closed > pp_, f"The span is too great to cover with this stroke length: {pp_}"
    # Angle of PP'Q, via a numerically stable acos
    beta = math.acos(
        - d / pp_ * (1 + d / (2 * d_closed))
        + pp_ / (2 * d_closed))
    # Two solutions based on angle complementarity
    if smaller:
        contra_phi = beta - (math.pi - theta) / 2
    else:
        # technically there's a 2pi in front
        contra_phi = -(math.pi - theta) / 2 - beta
    # Law of cosines, calculates `r'`
    r_ = math.sqrt(
        r * r + d_closed * d_closed - 2 * r * d_closed * math.cos(contra_phi)
    )
    # sin phi_ / P'Q = sin contra_phi / r'
    phi_ = math.asin(math.sin(contra_phi) / r_ * d_closed)
    assert phi_ > 0, f"Actuator would need to traverse pass its minimal point, {math.degrees(phi_)}"
    assert 0 <= theta + phi_ <= math.pi
    return r, theta + phi_, r_