from micropython import const
import math

PI2 = math.pi / 2
PI4 = math.pi / 4

COXA = const(12)
FEMUR = const(20)
TIBIA = const(30)
BASE = const(21)
HOME = (COXA + FEMUR) / math.sqrt(2)


def _solve_triangle(a, b, c):
    a, b, c = abs(a), abs(b), abs(c)
    if a + b < c or a + c < b or b + c < a:
        raise ValueError()
    cos = (a ** 2 + b ** 2 - c ** 2) / (2 * a * b)
    return math.acos(cos)


def _norm(a, b):
    return math.sqrt(a ** 2 + b ** 2)


def ik(x, y, z):
    f = _norm(x, y) - COXA
    d = _norm(f, z)
    if d > FEMUR + TIBIA:
        raise ValueError()
    hip = PI4 - math.atan2(y, x)
    knee = _solve_triangle(FEMUR, d, TIBIA) - math.atan2(-z, f)
    ankle = PI2 - _solve_triangle(FEMUR, TIBIA, d)
    return hip, knee, ankle