import math
import time

from fischertechnik.controller.Motor import Motor
from lib.camera import *
from lib.controller import *
from lib.display import *


shot = None
v = None
ticks = None
factor_x = None
state = None
ts_ball = None
ts_goal_left = None
ts_goal_right = None
cmd = None
factor_y = None
goal_width = None
posx_ball = None
posx_goal_left = None
posx_goal_right = None
x_min = None
v_slow = None
posx_goal_both = None
posy_ball = None
posy_goal_left = None
posy_goal_right = None
x_max = None
v_var_ball_xpos = None
y_ball_far = None
v_fast = None
y_ball_touch = None
v_var_ball_ypos = None
y_ball_near = None
ts_last_print = None
search_dir_left = None
ts_deltas = None
def checkTimeoutsSetGoalBoth():
    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    if posx_goal_left != None and posx_goal_right != None:
        goal_width = posx_goal_right - posx_goal_left
        posx_goal_both = posx_goal_left + goal_width / 2
    else:
        goal_width = None
        posx_goal_both = None
    if (time.time() * 1000) - ts_ball > 500:
        posx_ball = None
        posy_ball = None
    if (time.time() * 1000) - ts_goal_left > 500:
        posx_goal_left = None
        posy_goal_left = None
    if (time.time() * 1000) - ts_goal_right > 500:
        posx_goal_right = None
        posy_goal_right = None



def stepOnlyGoal():
    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    if posx_goal_left != None and posx_goal_right != None:
        if posx_goal_both >= x_min and posx_goal_both <= x_max:
            if goal_width < 130:
                fwd(v_slow, 10)
            elif goal_width > 40:
                bwd(v_slow, 10)
            else:
                pass
        elif posx_goal_both < x_min:
            left(v_slow, 10)
        elif posx_goal_both > x_max:
            right(v_slow, 10)
        else:
            pass
    else:
        left(v_slow, 10)


def stepBall(shot):
    global v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    state = 'stepBall'
    if posx_ball < x_min:
        left(v_slow, round(math.fabs(posx_ball) * 0.07))
    elif posx_ball > x_max:
        right(v_slow, round(math.fabs(posx_ball) * 0.07))
    elif posx_ball >= x_min and posx_ball <= x_max:
        if posy_ball <= y_ball_far:
            fwd(v_fast, 10)
        elif posy_ball > y_ball_far and posy_ball <= y_ball_near:
            fwd(v_slow, 5)
        elif shot:
            fwd(v_slow, 5)
        elif posy_ball > y_ball_near and posy_ball <= y_ball_touch:
            turnSideLeft(v_fast, 20)
        elif posy_ball <= y_ball_touch:
            bwd(v_slow, 5)
        else:
            pass


def ball_callback(event):
    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    ts_ball = (time.time() * 1000)
    posx_ball = event.value.x
    posy_ball = event.value.y
    v_var_ball_xpos = round(min(max(math.fabs(posx_ball) * factor_x + v_slow, 0), 512))
    v_var_ball_ypos = round(min(max(math.fabs(v_var_ball_ypos) * factor_y + v_slow, 0), 512))
    if False:
        print('ball posx:{} posy:{} v_x:{} v_y:{}'.format(event.value.x, event.value.y, v_var_ball_xpos, v_var_ball_ypos))

ball_detector.add_detection_listener(ball_callback)


def ball_callback2(event):
    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    ts_goal_left = (time.time() * 1000)
    posx_goal_left = event.value.x
    posy_goal_left = event.value.y
    if False:
        print('goal left: posx:{} posy:{}'.format(event.value.x, event.value.y))

ball_detector_goal_left.add_detection_listener(ball_callback2)


def ball_callback23(event):
    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    ts_goal_right = (time.time() * 1000)
    posx_goal_right = event.value.x
    posy_goal_right = event.value.y
    if False:
        print('goal right: posx:{} posy:{}'.format(event.value.x, event.value.y))

ball_detector_goal_right.add_detection_listener(ball_callback23)


def callback(event):
    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    if TXT_M_I8_photo_transistor.is_dark():
        shotBall()
        time.sleep(5)


TXT_M_I8_photo_transistor.add_change_listener("dark", callback)


def shotBall():
    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    cmd = 'shotBall'
    TXT_M_S1_servomotor.set_position(int(400))
    TXT_M_M1_encodermotor.set_speed(int(512), Motor.CCW)
    TXT_M_M2_encodermotor.set_speed(int(512), Motor.CCW)
    TXT_M_M3_encodermotor.set_speed(int(512), Motor.CCW)
    TXT_M_M4_encodermotor.set_speed(int(512), Motor.CCW)
    TXT_M_M1_encodermotor.set_distance(int(50), TXT_M_M2_encodermotor, TXT_M_M3_encodermotor, TXT_M_M4_encodermotor)
    time.sleep(0.05)
    TXT_M_S1_servomotor.set_position(int(256))
    while True:
        if (not TXT_M_M1_encodermotor.is_running()):
            break
        time.sleep(0.010)
    TXT_M_M1_encodermotor.set_speed(int(512), Motor.CW)
    TXT_M_M2_encodermotor.set_speed(int(512), Motor.CW)
    TXT_M_M3_encodermotor.set_speed(int(512), Motor.CW)
    TXT_M_M4_encodermotor.set_speed(int(512), Motor.CW)
    TXT_M_M1_encodermotor.set_distance(int(20), TXT_M_M2_encodermotor, TXT_M_M3_encodermotor, TXT_M_M4_encodermotor)
    while True:
        if (not TXT_M_M1_encodermotor.is_running()):
            break
        time.sleep(0.010)
    soundGoal()


def soundGoal():
    global shot, v, ticks, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    cmd = 'soundGOAL'
    TXT_M.get_loudspeaker().play("26_Augenzwinkern.wav", False)
    display.set_attr("txt_switch_ball.checked", str(False).lower())
    while True:
        if (not (TXT_M.get_loudspeaker().is_playing())):
            break
        time.sleep(0.010)


def turnSideLeft(v, ticks):
    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    cmd = 'turnSideLeft'
    if ticks >= 1:
        TXT_M_M1_encodermotor.set_speed(int(v_slow), Motor.CCW)
        TXT_M_M2_encodermotor.set_speed(int(v_slow), Motor.CW)
        TXT_M_M1_encodermotor.set_distance(int(ticks), TXT_M_M2_encodermotor)
        TXT_M_M3_encodermotor.set_speed(int(v), Motor.CW)
        TXT_M_M4_encodermotor.set_speed(int(v), Motor.CCW)
        TXT_M_M3_encodermotor.set_distance(int(ticks), TXT_M_M4_encodermotor)
        while True:
            if (not TXT_M_M3_encodermotor.is_running()):
                break
            time.sleep(0.010)


def turnSideRight(v, ticks):
    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    cmd = 'turnSideRight'
    if ticks >= 1:
        TXT_M_M1_encodermotor.set_speed(int(v_slow), Motor.CW)
        TXT_M_M2_encodermotor.set_speed(int(v_slow), Motor.CCW)
        TXT_M_M1_encodermotor.set_distance(int(ticks), TXT_M_M2_encodermotor)
        TXT_M_M3_encodermotor.set_speed(int(v), Motor.CCW)
        TXT_M_M4_encodermotor.set_speed(int(v), Motor.CW)
        TXT_M_M3_encodermotor.set_distance(int(ticks), TXT_M_M4_encodermotor)
        while True:
            if (not TXT_M_M3_encodermotor.is_running()):
                break
            time.sleep(0.010)


def fwd(v, ticks):
    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    cmd = 'fwd'
    if ticks >= 1:
        TXT_M_M1_encodermotor.set_speed(int(v), Motor.CCW)
        TXT_M_M2_encodermotor.set_speed(int(v), Motor.CCW)
        TXT_M_M3_encodermotor.set_speed(int(v), Motor.CCW)
        TXT_M_M4_encodermotor.set_speed(int(v), Motor.CCW)
        TXT_M_M1_encodermotor.set_distance(int(ticks), TXT_M_M2_encodermotor, TXT_M_M3_encodermotor, TXT_M_M4_encodermotor)
        while True:
            if (not TXT_M_M1_encodermotor.is_running()):
                break
            time.sleep(0.010)


def bwd(v, ticks):
    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    cmd = 'bwd'
    if ticks >= 1:
        TXT_M_M1_encodermotor.set_speed(int(v), Motor.CW)
        TXT_M_M2_encodermotor.set_speed(int(v), Motor.CW)
        TXT_M_M3_encodermotor.set_speed(int(v), Motor.CW)
        TXT_M_M4_encodermotor.set_speed(int(v), Motor.CW)
        TXT_M_M1_encodermotor.set_distance(int(ticks), TXT_M_M2_encodermotor, TXT_M_M3_encodermotor, TXT_M_M4_encodermotor)
        while True:
            if (not TXT_M_M1_encodermotor.is_running()):
                break
            time.sleep(0.010)


def left(v, ticks):
    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    cmd = 'left ({}, {})'.format(v, ticks)
    if ticks >= 1:
        TXT_M_M1_encodermotor.set_speed(int(v), Motor.CCW)
        TXT_M_M2_encodermotor.set_speed(int(v), Motor.CW)
        TXT_M_M3_encodermotor.set_speed(int(v), Motor.CCW)
        TXT_M_M4_encodermotor.set_speed(int(v), Motor.CW)
        TXT_M_M1_encodermotor.set_distance(int(ticks), TXT_M_M2_encodermotor, TXT_M_M3_encodermotor, TXT_M_M4_encodermotor)
        while True:
            if (not TXT_M_M1_encodermotor.is_running()):
                break
            time.sleep(0.010)


def right(v, ticks):
    global shot, factor_x, state, ts_ball, ts_goal_left, ts_goal_right, cmd, factor_y, goal_width, posx_ball, posx_goal_left, posx_goal_right, x_min, v_slow, posx_goal_both, posy_ball, posy_goal_left, posy_goal_right, x_max, v_var_ball_xpos, y_ball_far, v_fast, y_ball_touch, v_var_ball_ypos, y_ball_near, ts_last_print, search_dir_left, ts_deltas
    cmd = 'right ({}, {})'.format(v, ticks)
    if ticks >= 1:
        TXT_M_M1_encodermotor.set_speed(int(v), Motor.CW)
        TXT_M_M2_encodermotor.set_speed(int(v), Motor.CCW)
        TXT_M_M3_encodermotor.set_speed(int(v), Motor.CW)
        TXT_M_M4_encodermotor.set_speed(int(v), Motor.CCW)
        TXT_M_M1_encodermotor.set_distance(int(ticks), TXT_M_M2_encodermotor, TXT_M_M3_encodermotor, TXT_M_M4_encodermotor)
        while True:
            if (not TXT_M_M1_encodermotor.is_running()):
                break
            time.sleep(0.010)


factor_x = 0.2
factor_y = 3.5
x_min = -10
x_max = 10
y_ball_far = -5
y_ball_near = 45
y_ball_touch = 74
v_slow = 250
v_fast = 512
v_var_ball_xpos = 512
v_var_ball_ypos = 512
posx_goal_left = None
posy_goal_left = None
posx_goal_right = None
posy_goal_right = None
goal_width = 0
posx_goal_both = 0
posx_ball = None
posy_ball = None
ts_ball = (time.time() * 1000)
ts_goal_left = (time.time() * 1000)
ts_goal_right = (time.time() * 1000)
ts_last_print = (time.time() * 1000)
search_dir_left = True
state = 'NONE'
cmd = 'NONE'
while True:
    if display.get_attr("txt_switch_ball.checked"):
        display.set_attr("txt_switch_goal.checked", str(False).lower())
        state = 'NONE'
        cmd = 'NONE'
        if posx_ball != None and posy_ball != None:
            if posx_goal_left != None and posx_goal_right != None:
                state = 'BALL_GOAL_BOTH'
                if posy_goal_left - posy_ball > -15 and posy_goal_right - posy_ball > -15:
                    soundGoal()
                else:
                    if posx_ball > posx_goal_left and posx_ball < posx_goal_right:
                        stepBall(True)
                    elif posx_ball < posx_goal_both:
                        turnSideLeft(v_slow, 20)
                    elif posx_ball > posx_goal_both:
                        turnSideRight(v_slow, 20)
            elif posx_goal_left != None and posx_goal_right == None:
                state = 'BALL_GOAL_LEFT'
                right(v_slow, 20)
                turnSideLeft(v_slow, 20)
            elif posx_goal_left == None and posx_goal_right != None:
                state = 'BALL_GOAL_RIGHT'
                left(v_slow, 20)
                turnSideRight(v_slow, 20)
            else:
                state = 'BALL_NO_GOAL'
                stepBall(False)
        else:
            if posx_goal_left != None and posx_goal_right != None:
                state = 'NO_BALL_GOAL_BOTH'
                left(v_slow, 10)
            elif posx_goal_left != None and posx_goal_right == None:
                state = 'NO_BALL_GOAL_LEFT'
                right(v_slow, 10)
            elif posx_goal_left == None and posx_goal_right != None:
                state = 'NO_BALL_GOAL_RIGHT'
                left(v_slow, 10)
            else:
                state = 'NONE'
                left(v_slow, 10)
    elif display.get_attr("txt_switch_goal.checked"):
        display.set_attr("txt_switch_ball.checked", str(False).lower())
        stepOnlyGoal()
    checkTimeoutsSetGoalBoth()
    ts_deltas = [(time.time() * 1000) - ts_ball, (time.time() * 1000) - ts_goal_left, (time.time() * 1000) - ts_goal_right]
    if (time.time() * 1000) - ts_last_print > 200:
        ts_last_print = (time.time() * 1000)
        display.set_attr("txt_status_indicator_ball.active", str(posx_ball != None).lower())
        display.set_attr("txt_status_indicator_goal_left.active", str(posx_goal_left != None).lower())
        display.set_attr("txt_status_indicator_goal_right.active", str(posx_goal_right != None).lower())
        display.set_attr("txt_label_ball.text", str('x:{} y:{}'.format(posx_ball, posy_ball)))
        display.set_attr("txt_label_goal_left.text", str('x:{} y:{}'.format(posx_goal_left, posy_goal_left)))
        display.set_attr("txt_label_goal_right.text", str('x:{} y:{}'.format(posx_goal_right, posy_goal_right)))
        display.set_attr("txt_label_goal_both.text", str('x:{}, width:{}'.format(posx_goal_both, goal_width)))
        print('{:.1f} {:.1f} {:.1f} {}:{}  GOAL left: {} {}, right:{} {}, p:{}, w:{} BALL_pos:{},{} v_x:{} v_y:{}'.format(ts_deltas[0], ts_deltas[1], ts_deltas[2], state, cmd, posx_goal_left, posy_goal_left, posx_goal_right, posy_goal_right, posx_goal_both, goal_width, posx_ball, posy_ball, v_var_ball_xpos, v_var_ball_ypos))
    if posx_ball != None and posx_goal_left != None and posx_goal_right != None:
        time.sleep(min(ts_deltas) / 1000)
    else:
        time.sleep(0.01)