#!/usr/bin/python
# Post 1010 Ring Collector
import os, sys
import ctypes
KIPR=ctypes.CDLL("/usr/lib/libkipr.so")

## constants
slow_speed = 100
fast_speed = 250

base_servo = 1
top_servo = 3
botguy_servo = 0

LS = 5

## built in wrappers

# return gyroscope val
def _gyro_val():
    return(KIPR.gyro_z())

# suspend for n miliseconds
def msleep(time):
    KIPR.msleep(time)

# drive roomba
def _drive_direct(left_power, right_power):
    KIPR.create_drive_direct(left_power, right_power)

# all off
def stop():
    _drive_direct(0,0)


## top level functions for use

# function for using servo
def servo_control(servo_name, end_pos, rate=1):
    pos = KIPR.get_servo_position(servo_name)
    print(servo_name, pos, end_pos)
    if pos > end_pos:
        for i in range(pos, end_pos, -rate):
            KIPR.set_servo_position(servo_name, i)
            KIPR.msleep(rate)
    else:
        for i in range(pos, end_pos, rate):
            KIPR.set_servo_position(servo_name, i)
            KIPR.msleep(rate)

# drives forward for n miliseconds
def drive(left_power, right_power, duration):
    _drive_direct(left_power, right_power)
    msleep(duration)
    stop()  

# main routines
def wfl():
    START = KIPR.analog(LS)
    while(KIPR.analog(LS) > START/2):
        msleep(50)

def back_align():
    drive(-100, -100, 900)

def horizontal_claw():
    servo_control(base_servo, 200)
        
def vertical_claw():
    servo_control(base_servo, 1350)

def vertical_claw_half():
    servo_control(base_servo, 900)
        
def claw_open():
    servo_control(top_servo, 0)
    
def claw_close():
    servo_control(top_servo, 470)
        
def lower_botguy_hook():
    servo_control(botguy_servo, 0) 
        
def raise_botguy_hook():
    servo_control(botguy_servo, 2047) 

# move #1
def drive_towards_rings():
    drive(56, 62, 3300)    

# move #2 drop ring base
def left_rotate_servo():
    drive(200, -200, 1800)	
    servo_control(base_servo, 200)
    drive(-200, 200, 850)	
    servo_control(base_servo, 200)
        
# move #3 position to place rings
def drive_after_rings():
    drive(100, 100, 4000)
    KIPR.msleep(500)
    drive(-100, 100, 1800)	
    KIPR.msleep(500)
    drive(-100, -100, 2000)
    KIPR.msleep(500)                
    drive(100, 100, 1100)	
    KIPR.msleep(500)
        
# move #4 load rings
def rotate_to_cylinder():
    drive(100, -100, 3100)
        
# move #5 go after Botguy
def drive_backward_to_middlepipe():
    drive(-150, -150, 800)
    drive(150, -150, 400)
    drive(-150, -150, 2800)
    drive(-150, 150, 400)
    claw_close()
    KIPR.msleep(500)
    raise_botguy_hook()  
    drive(-150, -150, 4000)
    drive(100, 100, 500)
        
# move #6
def rotate_to_botguy():
    drive(200, -200, 800)
        
def pause(time,test):
    _drive_direct(0, 0)
    if test==1:
        print "Press Button"  
        while(KIPR.push_button() == 0):
            KIPR.msleep(10) 
    msleep(time)

def main():

    # Roomba Setup ==============================================================

    loop = 0
    test = 0
    print "Press A to continue"
    print "Press B to test"
    while(loop == 0):
        if(KIPR.a_button() == 1):
            test = 0
            print "Continuing"
            loop = 1
        elif(KIPR.b_button() == 1):
            test = 1
            print "Testing"
            loop = 1        
        
    print("Turn on Roomba")
    KIPR.create_connect()
    print("Roomba Connected!")
        
    horizontal_claw()
    claw_close()
    lower_botguy_hook()
    KIPR.enable_servos()
    print("Servos Enabled")
    horizontal_claw()
    claw_close()
    lower_botguy_hook()
    
    # Wait for light start
    print("Wait for Start Light")
    wfl()

    KIPR.shut_down_in(119)

    # Back align with wall in start box
    back_align()

    # Move claw into operating position
    vertical_claw()
    claw_open()

    # Set Roomba to Claw Position
    drive_towards_rings()

    # Reduce extraneous momentum 
    pause(500,test)

    # Tighten claw around rings
    claw_close()
        
    pause(500,test)

    # #2 Rotate Claw and Rotate Roomba to Remove Rings  
    left_rotate_servo()

    pause(500,test)
    
    # #3 Align Roomba to Rotation Position
    drive_after_rings()
        
    pause(500,test)        
    
    # #4 Rotate Roomba to Insert Rings into Cylinder
    rotate_to_cylinder()
        
    pause(500,test)

    # # Release rings        
    claw_open()
    vertical_claw_half()
    
    # # Wait for other bot for 80 secs
    pause(80000,test)
    
    # #5 Drive to middle pipe
    drive_backward_to_middlepipe()
    rotate_to_botguy()
                
    msleep(1000)
        
    # # Clean up
    KIPR.ao()
    KIPR.create_stop()
    KIPR.disable_servos()
    KIPR.create_disconnect()
    print("Game Over")
    msleep(2000)

if __name__== "__main__":
    sys.stdout = os.fdopen(sys.stdout.fileno(),"w",0)
    main();