import rone, sys, math, velocity, leds

FTL_TV = 100
FTL_RV = math.pi*1000/2

STATE_STOP = 0
STATE_STRAIGHT = 1
STATE_TURN = 2

TIME_STRAIGHT = 2000
TIME_ROTATE = 1000

LED_BRIGHTNESS = 40


def velocity_test():
    # initialize the velocity controller
    velocity.init(0.22, 40, 0.3, 0.1)
    leds.init()

    state = STATE_STRAIGHT
    rotations = 0
    time_motion_end = sys.time() + TIME_STRAIGHT

    while True:
        if state == STATE_STOP:
            velocity.set_tvrv(0, 0)
            leds.set_pattern('r', 'circle', LED_BRIGHTNESS)
        elif state == STATE_STRAIGHT:
            velocity.set_tvrv(FTL_TV, 0)
            leds.set_pattern('g', 'blink_slow', LED_BRIGHTNESS)
            # figure out the next thing to do
            if sys.time() > (time_motion_end):
                rotations += 1
                if rotations < 4:
                    time_motion_end += TIME_ROTATE
                    state = STATE_TURN
                else:
                    state = STATE_STOP
        elif state == STATE_TURN:
            velocity.set_tvrv(0, FTL_RV)
            leds.set_pattern('b', 'blink_fast', LED_BRIGHTNESS)
            # figure out the next thing to do
            if sys.time() > (time_motion_end):
                time_motion_end += TIME_STRAIGHT
                state = STATE_STRAIGHT
    
        # call the velocity controller update function
        velocity.update()
        leds.update()


velocity_test()