from math import atan2, pi, cos, sin
from time import sleep
from random import gauss
from robosim import *
init()

horizon = 50
alpha = 0.1
beta = 0.01


def todeg(angle):
    return (360.0 * angle) / (2 * pi)

def torad(angle):
    return angle / 360.0 * (2 * pi)

def cosdeg(angle):
    return cos(torad(angle))

def sindeg(angle):
    return sin(torad(angle))

def get_prox():
    a = orientation()
    L = [a + x for x in [40,20,0,-20,-40]]
    tel = telemetre_coords_list(*position(), angle_list = L, show_rays=False)
    res = [min(horizon, x) for x in tel]

    return res

def cap(x, y):
    x0, y0 = position()
    angle = todeg(atan2(y - y0, x - x0))
    return angle

def aller_a(x, y):
    prox = get_prox()
    directions = [40, 20, 0, -20, -40]
    angle = 0
    for i in range(5):
        angle += prox[i] * directions[i]
    angle = angle * beta
    angle = (1 - alpha) * angle + alpha * (cap(x,y) - orientation())
    if angle > 0:
        td(angle)
    else:
        tg(-angle)
    av(5)

def reset():
    set_position(100,100)
    setheading(0)

def distance(a, b):
    (x0, y0) = a
    (x, y) = b
    return sqrt((x - x0)**2 + (y - y0)**2)

def test_evitement():
    reset()
    game.fps = 200
    for i in range(200):
        if distance(position(), (460, 460)) < 10:
            break
        aller_a(460, 460)

def f(x, y):
    setheading(cap(x, y))
    for i in range(200):
        aller_a(x, y)
        d =  distance((x, y), position())
        if d < 10:
            break
    return i + d

def g(L):
    global alpha
    global beta
    reset()
    alpha = L[0] / 5.0
    beta = L[1] / 50.0
    return f(460, 460)


def optim(f, N, niter):
    T0 = N * [0.5]
    perf = f(T0)
    for i in range(niter):
        T1 = [abs(x + gauss(0., 1.0)) for x in T0]
        perf1 = f(T1)
        if perf1 < perf:
            perf = perf1
            T0 = T1
            print T0
    return T0


def test_optim():
    game.fps = 0
    print optim(g, 2, 100)

def cap_doux():
    reset()
    game.fps = 10
    angle = 0
    x, y = 460, 460
    alpha = 0.05
    for i in range(100):
        angle = alpha * cap(x, y) + (1 - alpha) * angle
        setheading(angle)
        av(5)

cap_doux()
test_evitement()
test_optim()