"""
Utility functions for cadquery objects
"""
import math
import functools
import cadquery as Cq
from nhf import Role
from typing import Union, Tuple, cast

# Bug fixes
def _subloc(self, name: str) -> Tuple[Cq.Location, str]:
    """
       Calculate relative location of an object in a subassembly.

       Returns the relative positions as well as the name of the top assembly.
       """

    rv = Cq.Location()
    obj = self.objects[name]
    name_out = name

    if obj not in self.children and obj is not self:
        locs = []
        while not obj.parent is self:
            locs.append(obj.loc)
            obj = cast(Cq.Assembly, obj.parent)
            name_out = obj.name

        rv = functools.reduce(lambda l1, l2: l2 * l1, locs)

    return (rv, name_out)
Cq.Assembly._subloc = _subloc

### Vector arithmetic

def location_sub(self: Cq.Location, rhs: Cq.Location) -> Cq.Vector:
    (x1, y1, z1), _ = self.toTuple()
    (x2, y2, z2), _ = rhs.toTuple()
    return Cq.Vector(x1 - x2, y1 - y2, z1 - z2)
Cq.Location.__sub__ = location_sub

def from2d(x: float, y: float, rotate: float=0.0) -> Cq.Location:
    return Cq.Location((x, y, 0), (0, 0, 1), rotate)
Cq.Location.from2d = from2d

def is2d(self: Cq.Location) -> bool:
    (_, _, z), (rx, ry, _) = self.toTuple()
    return z == 0 and rx == 0 and ry == 0
Cq.Location.is2d = is2d

def to2d(self: Cq.Location) -> Tuple[Tuple[float, float], float]:
    """
    Returns position and angle
    """
    (x, y, z), (rx, ry, rz) = self.toTuple()
    assert z == 0
    assert rx == 0
    assert ry == 0
    return (x, y), rz
Cq.Location.to2d = to2d

### Tags

def tagPoint(self, tag: str):
    """
    Adds a vertex that can be used in `Point` constraints.
    """
    vertex = Cq.Vertex.makeVertex(0, 0, 0)
    self.eachpoint(vertex.moved, useLocalCoordinates=True).tag(tag)

Cq.Workplane.tagPoint = tagPoint

def tagPlane(self, tag: str,
             direction: Union[str, Cq.Vector, Tuple[float, float, float]] = '+Z'):
    """
    Adds a phantom `Cq.Edge` in the given location which can be referenced in a
    `Axis`, `Point`, or `Plane` constraint.
    """
    if isinstance(direction, str):
        x, y, z = 0, 0, 0
        assert len(direction) == 2
        sign, axis = direction
        if axis in ('z', 'Z'):
            z = 1
        elif axis in ('y', 'Y'):
            y = 1
        elif axis in ('x', 'X'):
            x = 1
        else:
            assert False, "Axis must be one of x,y,z"
        if sign == '+':
            sign = 1
        elif sign == '-':
            sign = -1
        else:
            assert False, "Sign must be one of +/-"
        v = Cq.Vector(x, y, z) * sign
    else:
        v = Cq.Vector(direction)
    edge = Cq.Edge.makeLine(v * (-1), v)
    return self.eachpoint(edge.located, useLocalCoordinates=True).tag(tag)

Cq.Workplane.tagPlane = tagPlane

def make_sphere(r: float = 2) -> Cq.Solid:
    """
    Makes a full sphere. The default function makes a hemisphere
    """
    return Cq.Solid.makeSphere(r, angleDegrees1=-90)
def make_arrow(size: float = 2) -> Cq.Workplane:
    cone = Cq.Solid.makeCone(
        radius1 = size,
        radius2 = 0,
        height=size)
    result = (
        Cq.Workplane("XY")
        .cylinder(radius=size / 2, height=size, centered=(True, True, False))
        .union(cone.located(Cq.Location((0, 0, size))))
    )
    result.faces("<Z").tag("dir_rev")
    return result

def to_marker_name(tag: str) -> str:
    return tag.replace("?", "__T").replace("/", "__Z") + "_marker"

COLOR_MARKER = Cq.Color(0, 1, 1, 1)

def mark_point(self: Cq.Assembly,
               tag: str,
               size: float = 2,
               color: Cq.Color = COLOR_MARKER) -> Cq.Assembly:
    """
    Adds a marker to make a point visible
    """
    name = to_marker_name(tag)
    return (
        self
        .add(make_sphere(size), name=name, color=color)
        .constrain(tag, name, "Point")
    )

Cq.Assembly.markPoint = mark_point

def mark_plane(self: Cq.Assembly,
               tag: str,
               size: float = 2,
               color: Cq.Color = COLOR_MARKER) -> Cq.Assembly:
    """
    Adds a marker to make a plane visible
    """
    name = to_marker_name(tag)
    return (
        self
        .add(make_arrow(size), name=name, color=color)
        .constrain(tag, f"{name}?dir_rev", "Plane", param=180)
    )

Cq.Assembly.markPlane = mark_plane

def get_abs_location(self: Cq.Assembly,
                     tag: str) -> Cq.Location:
    name, shape = self._query(tag)
    loc_self = shape.location()
    loc_parent, _ = self._subloc(name)
    loc = loc_parent * loc_self
    return loc

Cq.Assembly.get_abs_location = get_abs_location