from __future__ import division

from bisect import bisect
from collections import namedtuple
from math import sqrt, hypot

# a planner computes a motion profile for a list of (x, y) points
class Planner(object):
    def __init__(self, acceleration, max_velocity, corner_factor):
        self.acceleration = acceleration
        self.max_velocity = max_velocity
        self.corner_factor = corner_factor

    def plan(self, points):
        return constant_acceleration_plan(
            points, self.acceleration, self.max_velocity, self.corner_factor)

    def plan_all(self, paths):
        return [self.plan(path) for path in paths]

# a plan is a motion profile generated by the planner
class Plan(object):
    def __init__(self, blocks):
        self.blocks = blocks
        self.ts = [] # start time of each block
        self.ss = [] # start distance of each block
        t = 0
        s = 0
        for b in blocks:
            self.ts.append(t)
            self.ss.append(s)
            t += b.t
            s += b.s
        self.t = t # total time
        self.s = s # total duration

    def instant(self, t):
        t = max(0, min(self.t, t)) # clamp t
        i = bisect(self.ts, t) - 1 # find block for t
        return self.blocks[i].instant(t - self.ts[i], self.ts[i], self.ss[i])

# a block is a constant acceleration for a duration of time
class Block(object):
    def __init__(self, a, t, vi, p1, p2):
        self.a = a
        self.t = t
        self.vi = vi
        self.p1 = p1
        self.p2 = p2
        self.s = p1.distance(p2)

    def instant(self, t, dt=0, ds=0):
        t = max(0, min(self.t, t)) # clamp t
        a = self.a
        v = self.vi + self.a * t
        s = self.vi * t + self.a * t * t / 2
        s = max(0, min(self.s, s)) # clamp s
        p = self.p1.lerps(self.p2, s)
        return Instant(t + dt, p, s + ds, v, a)

# an instant gives position, velocity, etc. at a single point in time
Instant = namedtuple('Instant', ['t', 'p', 's', 'v', 'a'])

# a = acceleration
# v = velocity
# s = distance
# t = time
# i = initial
# f = final

# vf = vi + a * t
# s = (vf + vi) / 2 * t
# s = vi * t + a * t * t / 2
# vf * vf = vi * vi + 2 * a * s

EPS = 1e-9

_Point = namedtuple('Point', ['x', 'y'])

class Point(_Point):
    def length(self):
        return hypot(self.x, self.y)

    def normalize(self):
        d = self.length()
        if d == 0:
            return Point(0, 0)
        return Point(self.x / d, self.y / d)

    def distance(self, other):
        return hypot(self.x - other.x, self.y - other.y)

    def distance_squared(self, other):
        return (self.x - other.x) ** 2 + (self.y - other.y) ** 2

    def add(self, other):
        return Point(self.x + other.x, self.y + other.y)

    def sub(self, other):
        return Point(self.x - other.x, self.y - other.y)

    def mul(self, factor):
        return Point(self.x * factor, self.y * factor)

    def dot(self, other):
        return self.x * other.x + self.y * other.y

    def lerps(self, other, s):
        v = other.sub(self).normalize()
        return self.add(v.mul(s))

    def segment_distance(self, v, w):
        p = self
        l2 = v.distance_squared(w)
        if l2 == 0:
            return p.distance(v)
        t = ((p.x - v.x) * (w.x - v.x) + (p.y - v.y) * (w.y - v.y)) / l2
        t = max(0, min(1, t))
        x = v.x + t * (w.x - v.x)
        y = v.y + t * (w.y - v.y)
        q = Point(x, y)
        return p.distance(q)

Triangle = namedtuple('Triangle',
    ['s1', 's2', 't1', 't2', 'vmax', 'p1', 'p2', 'p3'])

def triangle(s, vi, vf, a, p1, p3):
    # compute a triangular profile: accelerating, decelerating
    s1 = (2 * a * s + vf * vf - vi * vi) / (4 * a)
    s2 = s - s1
    vmax = (vi * vi + 2 * a * s1) ** 0.5
    t1 = (vmax - vi) / a
    t2 = (vf - vmax) / -a
    p2 = p1.lerps(p3, s1)
    return Triangle(s1, s2, t1, t2, vmax, p1, p2, p3)

Trapezoid = namedtuple('Trapezoid',
    ['s1', 's2', 's3', 't1', 't2', 't3', 'p1', 'p2', 'p3', 'p4'])

def trapezoid(s, vi, vmax, vf, a, p1, p4):
    # compute a trapezoidal profile: accelerating, cruising, decelerating
    t1 = (vmax - vi) / a
    s1 = (vmax + vi) / 2 * t1
    t3 = (vf - vmax) / -a
    s3 = (vf + vmax) / 2 * t3
    s2 = s - s1 - s3
    t2 = s2 / vmax
    p2 = p1.lerps(p4, s1)
    p3 = p1.lerps(p4, s - s3)
    return Trapezoid(s1, s2, s3, t1, t2, t3, p1, p2, p3, p4)

def corner_velocity(s1, s2, vmax, a, delta):
    # compute a maximum velocity at the corner of two segments
    # https://onehossshay.wordpress.com/2011/09/24/improving_grbl_cornering_algorithm/
    cosine = -s1.vector.dot(s2.vector)
    if abs(cosine - 1) < EPS:
        return 0
    sine = sqrt((1 - cosine) / 2)
    if abs(sine - 1) < EPS:
        return vmax
    v = sqrt((a * delta * sine) / (1 - sine))
    return min(v, vmax)

class Segment(object):
    # a segment is a line segment between two points, which will be broken
    # up into blocks by the planner
    def __init__(self, p1, p2):
        self.p1 = p1
        self.p2 = p2
        self.length = p1.distance(p2)
        self.vector = p2.sub(p1).normalize()
        self.max_entry_velocity = 0
        self.entry_velocity = 0
        self.blocks = []

class Throttler(object):
    def __init__(self, points, vmax, dt, threshold):
        self.points = points
        self.vmax = vmax
        self.dt = dt
        self.threshold = threshold
        self.distances = []
        prev = points[0]
        d = 0
        for point in points:
            d += prev.distance(point)
            self.distances.append(d)
            prev = point
    def lookup(self, d):
        return bisect(self.distances, d) - 1
    def is_feasible(self, i0, v):
        d = v * self.dt
        x0 = self.distances[i0]
        x1 = x0 + d
        i1 = self.lookup(x1)
        if i0 == i1:
            return True
        p0 = self.points[i0]
        p10 = self.points[i1]
        try:
            p11 = self.points[i1+1]
        except IndexError:
            p11 = p10
        s = x1 - self.distances[i1]
        p1 = p10.lerps(p11, s)
        i = i0 + 1
        while i <= i1:
            p = self.points[i]
            if p.segment_distance(p0, p1) > self.threshold:
                return False
            i += 1
        return True
    def compute_max_velocity(self, index):
        if self.is_feasible(index, self.vmax):
            return self.vmax
        lo = 0
        hi = self.vmax
        for _ in range(16):
            v = (lo + hi) / 2
            if self.is_feasible(index, v):
                lo = v
            else:
                hi = v
        v = lo
        return v
    def compute_max_velocities(self):
        return [self.compute_max_velocity(i) for i in range(len(self.points))]

def constant_acceleration_plan(points, a, vmax, cf):
    # make sure points are Point objects
    points = [Point(x, y) for x, y in points]

    # the throttler reduces speeds based on the discrete timeslicing nature of
    # the device
    # TODO: expose parameters
    throttler = Throttler(points, vmax, 0.02, 0.001)
    max_velocities = throttler.compute_max_velocities()

    # create segments for each consecutive pair of points
    segments = [Segment(p1, p2) for p1, p2 in zip(points, points[1:])]

    # compute a max_entry_velocity for each segment
    # based on the angle formed by the two segments at the vertex
    for v, s1, s2 in zip(max_velocities, segments, segments[1:]):
        s1.max_entry_velocity = min(s1.max_entry_velocity, v)
        s2.max_entry_velocity = corner_velocity(s1, s2, vmax, a, cf)

    # add a dummy segment at the end to force a final velocity of zero
    segments.append(Segment(points[-1], points[-1]))

    # loop over segments
    i = 0
    while i < len(segments) - 1:
        # pull out some variables
        segment = segments[i]
        next_segment = segments[i + 1]
        s = segment.length
        vi = segment.entry_velocity
        vexit = next_segment.max_entry_velocity
        p1 = segment.p1
        p2 = segment.p2

        # determine which profile to use for this segment
        m = triangle(s, vi, vexit, a, p1, p2)
        if m.s1 < -EPS:
            # too fast! update max_entry_velocity and backtrack
            segment.max_entry_velocity = sqrt(vexit * vexit + 2 * a * s)
            i -= 1
        elif m.s2 < 0:
            # accelerate
            vf = sqrt(vi * vi + 2 * a * s)
            t = (vf - vi) / a
            segment.blocks = [
                Block(a, t, vi, p1, p2),
            ]
            next_segment.entry_velocity = vf
            i += 1
        elif m.vmax > vmax:
            # accelerate, cruise, decelerate
            z = trapezoid(s, vi, vmax, vexit, a, p1, p2)
            segment.blocks = [
                Block(a, z.t1, vi, z.p1, z.p2),
                Block(0, z.t2, vmax, z.p2, z.p3),
                Block(-a, z.t3, vmax, z.p3, z.p4),
            ]
            next_segment.entry_velocity = vexit
            i += 1
        else:
            # accelerate, decelerate
            segment.blocks = [
                Block(a, m.t1, vi, m.p1, m.p2),
                Block(-a, m.t2, m.vmax, m.p2, m.p3),
            ]
            next_segment.entry_velocity = vexit
            i += 1

    # concatenate all of the blocks
    blocks = []
    for segment in segments:
        blocks.extend(segment.blocks)

    # filter out zero-duration blocks and return
    blocks = [b for b in blocks if b.t > EPS]
    return Plan(blocks)
