#!/usr/bin/python
from time import sleep

from iot_car import Iot_car
from ikt_car_sensorik import Value_collector

car = Iot_car(testmode=False)
vc = Value_collector()

def is_state_save(vc):
    return True
    distance_front = vc.ultrasonic_front.distance
    distance_back = vc.ultrasonic_back.distance
    if distance_front > 10 and distance_back > 10:
        return True
    return False

def stop(vc, car):
    print 'danger'
    car.set_angle(0)
    car.accelerate(0)
    vc.stop()
    exit(0)

def website_stop():
    car.set_angle(0)
    car.accelerate(0)
    vc.stop()

def park_lite():

    car.set_angle(0)
    car.accelerate(0)
    try:
        bearing_start = vc.compass.bearing
        # steeling right
        car.set_angle(45)
        sleep(0.2)
        print
        'Backward'
        car.accelerate(-5)
        while is_state_save(vc) and not (vc.compass.bearing <= bearing_start - 45):
            sleep(0.1)
        if not is_state_save(vc):
            stop(vc, car)

        print
        'stop'
        car.accelerate(0)
        sleep(0.1)
        car.set_angle(-45)
        sleep(0.1)
        print
        'Backward'
        car.accelerate(-5)
        while is_state_save(vc) and not (vc.compass.bearing >= bearing_start):
            sleep(0.1)
        if not is_state_save(vc):
            stop(vc, car)

        print
        'stop'
        car.accelerate(0)
        print
        'end'

        car.accelerate(0)
        car.set_angle(0)

    except KeyboardInterrupt:
        car.accelerate(0)
        car.set_angle(0)
        vc.stop()
        exit(0)

    car.accelerate(0)
    car.set_angle(0)
    vc.stop()
    exit(0)



def park():
    car = Iot_car(testmode=False)
    vc = Value_collector()
    car.accelerate(0)
    car.set_angle(0)

    #angle_offset = -10

    print 'Wait for Values'
    sleep(10)
    print 'Dist Front: ' + str(vc.ultrasonic_front.distance)
    print 'Dist Back: ' + str(vc.ultrasonic_back.distance)
    speed = 4

    car.set_angle(0)
    car.accelerate(0)
    try:
        # drive forward an find gab
        print 'Forward'
        car.accelerate(0)
        while is_state_save(vc) and not vc.infrared.passed_gab:
            print vc.infrared.distance
            print vc.infrared.passed_gab

            sleep(0.1)
        if not is_state_save(vc):
            stop(vc, car)

        print 'stop'
        car.accelerate(0)
        bearing_start = vc.compass.bearing
        # steeling right
        car.set_angle(45)
        print 'Backward'
        car.accelerate(-5)
        while is_state_save(vc) and not (vc.compass.bearing <= bearing_start - 45):
            sleep(0.1)
        if not is_state_save(vc):
            stop(vc, car)

        print 'stop'
        car.accelerate(0)

        car.set_angle(-45)
        print 'Backward'
        car.accelerate(-5)
        while is_state_save(vc) and not (vc.compass.bearing >= bearing_start):
            sleep(0.1)
        if not is_state_save(vc):
            stop(vc, car)

        print 'stop'
        car.accelerate(0)
        print 'end'

        car.accelerate(0)
        car.set_angle(0)

    except KeyboardInterrupt:
        car.accelerate(0)
        car.set_angle(0)
        vc.stop()
        exit(0)

    car.accelerate(0)
    car.set_angle(0)
    vc.stop()
    exit(0)

if __name__ == "__main__":
    park()