import vision
import man.motion.HeadMoves as HeadMoves
from objects import RelRobotLocation

def gameSet(player):
    if player.firstFrame():
        player.brain.nav.stand() #Look at Navigator for controlling motions

    return player.stay()         #Return the next state to go into
                                 #Stay here, only go to playing when button is pressed

def gamePlaying(player):
    if player.firstFrame():   #This is so these things are only run once
        player.gainsOn()
        player.brain.nav.stand()  #Just in case
        player.brain.tracker.lookToAngleFixedPitch(0)  #Look straight ahead

    while (not player.brain.motion.calibrated()):  #If robot is not calibrated
        return player.stay()   #stay in this state

    return player.goNow('walkToBall')  #After calibrated, start walking to the ball

def walkToBall(player):
    ball = player.brain.ball #get ball info from Brain

    if(not ball.vis.on):  #if the ball is not in frame
        print "no ball"
        dest = RelRobotLocation(10, 0, 0)  #Create a destination directly in front of the robot
        player.brain.nav.goTo(dest)  #walk there, i.e. walk forward
        return player.stay()  #Keep walking to ball

    elif(ball.vis.dist < 30):  #if the ball is close
        print "saw the ball, stopping"
        return player.goNow('gameSet') #stop

    else:    #otherwise, the ball is visible but not too close
        print "see ball, going towards it"
        player.brain.nav.goTo(ball.loc)  #go to the ball's location
        return player.stay()   #keep going towards it