import numpy as np

def demo_track(gen, t, init):
    next = gen.straight(init, 3.5*t)
    next = gen.left_turn(next,t,np.pi/2)
    next = gen.straight(next, 5*t)
    next = gen.left_turn(next,t,np.pi/2)
    next = gen.straight(next, 1*t)
    next = gen.left_turn(next,t,np.pi/2)
    next = gen.straight(next, 2*t)
    next = gen.right_turn(next,t,np.pi/2)
    next = gen.straight(next, 1*t)
    next = gen.right_turn(next,t,np.pi/2)
    next = gen.straight(next, 2*t)
    next = gen.left_turn(next,t,np.pi/2)
    next = gen.straight(next, 2*t)
    next = gen.left_turn(next,t,np.pi/2)
    next = gen.straight(next, 5*t)
    next = gen.left_turn(next,t,np.pi/2)
    next = gen.straight(next, 4.5*t)
    return gen

def hard_track(gen, t, init):
    next = gen.straight(init,6*t)
    next = gen.left_turn(next, 3*t, np.pi/2)
    next = gen.right_turn(next,8*t, np.pi/2)
    next = gen.right_turn(next, 6*t, np.pi/2)
    next = gen.right_turn(next, 4*t, np.pi/2)
    next = gen.right_turn(next, 2*t, np.pi)
    next = gen.left_turn(next, 1*t, np.pi)
    next = gen.straight(next, 1*t)
    next = gen.right_turn(next, 1*t, np.pi/2)
    next = gen.left_turn(next, 1*t, np.pi/2)
    next = gen.left_turn(next, 3*t, np.pi/2)
    next = gen.left_turn(next, 8*t, np.pi/2)
    next = gen.right_turn(next, 2*t, np.pi)
    next = gen.straight(next, 5*t)
    next = gen.right_turn(next, 4*t, np.pi/2)
    next = gen.left_turn(next, 1.5*t, np.pi)
    next = gen.straight(next, 2*t)
    next = gen.right_turn(next, 2*t, 2.5*np.pi)
    next = gen.straight(next, 7*t)
    next = gen.right_turn(next, 3*t, np.pi)
    return gen

def long_track(gen, t, init):
    next = gen.straight(init, 3*t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, 7*t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, 3*t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, 2*t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 3*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 7*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 5*t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 4*t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 5*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, 3*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 3*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 3*t)
    return gen

def lucerne_track(gen, t, init):
    next = gen.straight(init, 5*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 6*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 4*t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, 2*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 2*t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, 4*t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, 5*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 7*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 8*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 5*t)
    return gen

def bern_track(gen, t, init):
    next = gen.straight(init, 3*t)
    next = gen.left_turn(next, 3*t, 3*np.pi/4)
    next = gen.straight(next, 3*t)
    next = gen.right_turn(next, t, np.pi/4)
    next = gen.straight(next, 3*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 2*t)
    next = gen.left_turn(next, 2*t, np.pi/3)
    next = gen.straight(next, 3*t)
    next = gen.right_turn(next, 2*t, np.pi/3)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 2*t)
    next = gen.left_turn(next, t/2, 2*np.pi/3)
    next = gen.straight(next, 3*t)
    next = gen.right_turn(next, 4.333*t, np.pi/6)
    next = gen.straight(next, 1.7*t+0.032)
    return gen

def test_track(gen, t, init):
    next = gen.straight(init, 3*t)
    next = gen.left_turn(next, 3*t, 3*np.pi/4)
    next = gen.straight(next, 3*t)
    next = gen.right_turn(next, t, np.pi/4)
    next = gen.straight(next, 3*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 2*t)
    next = gen.left_turn(next, 2*t, np.pi/3)
    next = gen.straight(next, 3*t)
    next = gen.right_turn(next, 2*t, np.pi/3)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 2*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 0.5*t)
    next = gen.left_turn(next, 4.333*t, np.pi/5)
    next = gen.straight(next, 1.7*t+0.01)
    next = gen.right_turn(next, 4.333*t, np.pi/5)
    return gen

def test_track2(gen, t, init):
    next = gen.straight(init, 2*t)
    next = gen.straight(next, 3*t)
    next = gen.right_turn(next, t, np.pi/4)
    next = gen.straight(next, 3*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 4*t)
    next = gen.left_turn(next, 2*t, np.pi/2)
    next = gen.straight(next, 3*t)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/4)
    next = gen.straight(next, t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, t)
    next = gen.right_turn(next, t, np.pi/2)
    next = gen.straight(next, 6*t)
    next = gen.left_turn(next, t, np.pi/2)
    next = gen.straight(next, 2*t)
    next = gen.left_turn(next, 3*t, np.pi/2)
    next = gen.straight(next, 5.37*t)
    next = gen.left_turn(next, 3*t, np.pi/2)
    return gen

def infinity_track(gen, t, init):
    init = [0,0,-np.pi/4]
    next = gen.straight(init,2*np.sqrt(2)*t)
    next = gen.left_turn(next,np.sqrt(2)*t,3*np.pi/2)
    next = gen.straight(next,2*np.sqrt(2)*t)
    next = gen.right_turn(next,np.sqrt(2)*t,3*np.pi/2)
    return gen

def snail_track(gen, t, init):
    track_name = "SNAIL_TRACK"
    next = gen.left_turn(init,t,np.pi)
    for i in range(6):
        next = gen.left_turn(next,(i+2)*t,np.pi)
    next = gen.left_turn(next,4*t,np.pi)
    return gen
