#!usr/bin/env python

import rospy
from tangible_master.msg import button_leds

def set_state(state_name, recieved_msg):
    """
    Handles the state of the tangible user interface based upon the input recieved via the buttons and current system
    status
    """
    pick_cloud_gathered = True
    basket_complete = True
    place_cloud_gathered = True



    if(state_name == "driving"):
        if(recieved_msg.button[2] == 1):
            #state_name = "gather_pick_cloud"
            state_name = "pick"

    #TODO add is pointcloud gathered flag
    if(state_name == "gather_pick_cloud"):
        if(pick_cloud_gathered):
            state_name == "pick"

    elif(state_name == "pick"):
        if(recieved_msg.button[5] == 1):
            state_name = "driving"
        elif(recieved_msg.button[0] == 1):
            state_name = "confirm_object"
            rospy.sleep(5)

    elif(state_name == "confirm_object"):
        if(recieved_msg.button[0] == 1):
            state_name = "confirm_pick"
            rospy.sleep(5)
        elif(recieved_msg.button[1] == 1):
            state_name = "driving"

    #Not sure how this is working with the grasp sucess detection code
    elif(state_name == "confirm_pick"):
        if(recieved_msg.button[0] == 1):
            state_name = "object_held"
            rospy.sleep(5)

        elif(recieved_msg.button[5] == 1):
            state_name = "driving"

    elif(state_name == "object_held"):
        if(recieved_msg.button[5] == 1):
            state_name = "driving"
        elif(recieved_msg.button[3] == 1):
            state_name = "gather_place_cloud"
        elif(recieved_msg.button[4] == 1):
            state_name = "basket"

    #TODO Need complete flag
    elif(state_name == "basket"):
        if(basket_complete):
            state_name = "driving"
    #TODO need cloudReady flag
    elif(state_name == "gather_place_cloud"):
        if(place_cloud_gathered):
            state_name = "place"

    elif(state_name == "place"):
        if(recieved_msg.button[0] == 1):
            state_name = "place_confirm"

    elif(state_name == "place_confirm"):
        if(recieved_msg.button[0] == 1):
            state_name = "placing"
        elif(recieved_msg.button[1] == 1):
            state_name = "place"

    #TODO Need placed confirm flag  ######and (placed_complete == True)
    elif((state_name == "placing")):
        state_name = "driving"

    print(str(recieved_msg) + '\n')
    print("current state name: " + state_name + '\n')
    return state_name





def compare_state(state_name, button_msg, scooter_instance):
    """
    Compares current state to desired LED state for the interface
    """
    #1 means solid light on, 2 means that it should be blinking, 0 is off
    #first led is thinking, second is ready for selection
    if(state_name == "driving"):
        button_msg.button = [0, 0, 2, 0, 0, 1]
        button_msg.leds = [0, 0, 0]

    elif(state_name == "gather_pick_cloud"):
        button_msg.button = [0, 0, 1, 0, 0, 0]
        button_msg.leds = [1, 0, 0]

    elif(state_name == "pick"):
        button_msg.button = [2, 2, 1, 0, 0, 0]
        button_msg.leds = [0, 1, 0]

    elif(state_name == "confirm_object"):
        button_msg.button = [2, 2, 1, 0, 0, 0]
        button_msg.leds = [0, 0, 0]

    elif(state_name == "confirm_pick"):
        button_msg.button = [2, 2, 0, 0, 0, 0]
        button_msg.leds = [0, 0, 0]


    elif(state_name == "object_held"):
        button_msg.button = [0, 0, 0, 2, 2, 1]
        button_msg.leds = [0, 0, 0]


    elif(state_name == "gather_place_cloud"):
        button_msg.button = [0, 0, 0, 1, 0, 0]
        button_msg.leds = [1, 0, 0]


    elif(state_name == "place"):
        button_msg.button = [2, 2, 0, 1, 0, 0]
        button_msg.leds = [0, 1, 0]


    elif(state_name == "place_confirm"):
        button_msg.button = [2, 2, 0, 1, 0, 0]
        button_msg.leds = [0, 0, 0]


    elif(state_name == "basket"):
        button_msg.button = [0, 0, 0, 0, 1, 0]
        button_msg.leds = [0, 0, 0]


    elif(state_name == "placing"):
        button_msg.button = [1, 0, 0, 1, 0, 0]
        button_msg.leds = [0, 0, 0]

    return button_msg

