"""
    Geometry 2D line helper
"""

import math


def distance(planar1, planar2):
    """Distance of two planar points"""
    x = planar1[0] - planar2[0]
    y = planar1[1] - planar2[1]
    return math.hypot(x, y)


def pointAtPolyLineDist(pts, dist):
    """Return point on polyline (planar) in given distance from start"""
    if len(pts) == 0:
        return None
    if len(pts) == 1:
        return pts[0]

    prev = pts[0]
    distFromStart = 0
    for p in pts[1:]:
        distFromStart += distance( prev, p )
        if dist < distFromStart:
            break
        prev = p
    if distance( prev, p ) < 0.000001:
        return p
    # resize (p - prev) to dist-distFromStart
    assert dist < distFromStart, (dist, distFromStart)
    frac = (distFromStart - dist)/distance(prev, p)
    return p[0] + frac*(prev[0]-p[0]), p[1] + frac*(prev[1]-p[1])



class Line:
    """
       2D line with basic distance and snap operations
    """

    def __init__(self, start, end):
        """Define line with two points"""
        self.start = start
        self.end = end
        self.angle = math.atan2( end[1] - start[1], end[0] - start[0] )
        self._a = -math.sin( self.angle )
        self._b = math.cos( self.angle )
        self._c = -self._a * start[0] - self._b * start[1]
        self._perpC = self._b * end[0] - self._a * end[1] # perpendicular line in end position
        self.length = distance(start, end)

    def signedDistance(self, pos):
        """Distance from line with orientation - positive on left side"""
        return self._a * pos[0] + self._b * pos[1] + self._c

    def distanceToFinishLine(self, pos):
        """Distance to the end of line along the line"""
        # returns negative value when end was already crossed
        return - self._b * pos[0] + self._a * pos[1] + self._perpC;

    def finished(self, pos):
        return self.distanceToFinishLine(pos) <= 0

    def snap(self, pos):
        """Return nearest position on (infinite) line"""
        dist = self.distanceToFinishLine( pos )
        return self.end[0] - self._b * dist, self.end[1] + self._a * dist

    def nearest( self, pos ):
        """return abs distance and type 0=start, 1=end, -1=inside of line"""
        # the same as snap, but restricted to finite line
        dist = self.distanceToFinishLine(pos)
        if dist <= 0:
            return self.end, distance(pos, self.end), 1
        if dist >= self.length:
            return self.start, distance(pos, self.start), 0
        return self.snap(pos), math.fabs(self.signedDistance(pos)), -1

# vim: expandtab sw=4 ts=4
