########################################
# TEST FOR BUTTONS
########################################
from machine import Pin
from time import sleep  
import time
from Button import Button
import network
import socket
import WFConnect

# WiFi server setup
WIFI_SSID = "WiFiCar1"
WIFI_PWD = "WiFiCar1"
WC_SERVER = '192.168.4.1'
WC_PORT = 23

CMD_FWD="F"
CMD_BACK="B"
CMD_LEFT="L"
CMD_RIGHT="R"
CMD_STOP="S"
CMD_FIRE="X"
CMD_END ="\n"

# Button names - assuming 5 different colour buttons
BTN_BLUE="BLUE"
BTN_YELLOW="YELLOW"
BTN_RED="RED"
BTN_BLACK="BLACK"
BTN_GREEN="GREEN"

# Create all of our buttons
buttons = {
    BTN_BLUE : Button(BTN_BLUE, 5),     # create input pin on GPIO5 = d1
    BTN_YELLOW: Button(BTN_YELLOW, 4),  # create input pin on GPIO4 = d2
    BTN_RED : Button(BTN_RED, 14),      # create input pin on GPIO14 = d5
    BTN_BLACK : Button(BTN_BLACK, 12),  # create input pin on GPIO12 = d6
    BTN_GREEN : Button(BTN_GREEN, 13)   # create input pin on GPIO13 = d7
}

#    CMD_LASER : Button(CMD_LASER, 13) # create input pin on GPIO13 = d7
LED = Pin(2, Pin.OUT)   # onboard LED is PIN 2

DEBUG = True

    
########################################
# initialiseAll
########################################
def initialiseAll():
    LED.value(0)
    if DEBUG:
        print("initialiseAll")
    connectAccessPoint()
    
    LED.value(1)

########################################
# Connect to Access Point
########################################
def connectAccessPoint():
    LED.value(False)

    if DEBUG:
        print('Try connectAccessPoint '+WIFI_SSID+" / "+WIFI_PWD)

    wlan = WFConnect.connectToAP(WIFI_SSID, WIFI_PWD)
    if wlan.isconnected():
        print('Connected '+str(wlan))
    else:
        print('Not Connected')
    if DEBUG:
        print('network config:', wlan.ifconfig())

    return wlan   

########################################
# test an individual button and return boolean
########################################
def testButton(buttonKey):
    buttonToTest = buttons[buttonKey]
    if buttonToTest is None:
        print("Cannot find button "+buttonKey)
    else:
        if buttonToTest.isPressed():
            return True 
    return False 


########################################
# Has the command changed ?
########################################
def commandChanged(command, previousCommand):
    
    if command != previousCommand:
        LED.value(0)
        if DEBUG:
            print("*** CHANGED ***" + str(command))
        LED.value(1)
        return True
    
    return False 


########################################
# Get Connection
########################################
def getConnection():
    # connectToNetwork()
    # make sure we're connect to Access Point
    connectAccessPoint()
    
    if DEBUG:
        print("target IP: %s" % WC_SERVER)
        print("target port: %s" % WC_PORT)
    
    addr = socket.getaddrinfo(WC_SERVER, WC_PORT)[0][-1]
    sock = socket.socket(socket.AF_INET, # Internet
                         socket.SOCK_STREAM)

    if DEBUG:
        print("Connecting to "+str(addr))
    while True:
        try:
            sock.connect(addr)
            break
        except:
            if DEBUG:
                print("Connect failed - retry "+str(addr))
            sleep(1)
    return sock

 
 
########################################
# Test the buttons to see what commands to send
########################################
def getCommandFromButton():

    command = ""
    
    # What is the left control doing ?
    if testButton(BTN_GREEN):
        command = command + CMD_FWD
    elif testButton(BTN_BLACK):
        command = command + CMD_BACK
    else:
        command = command + CMD_STOP
        
    # What is the right control doing ?
    if testButton(BTN_BLUE):
        command = command + CMD_FWD
    elif testButton(BTN_YELLOW):
        command = command + CMD_BACK
    else:
        command = command + CMD_STOP


    # What is the middle button doing ?
    if testButton(BTN_RED):
        command = command + CMD_FIRE
    else:
        command = command + CMD_STOP

    command = command + CMD_END
    
    return command

 
########################################
# Test the buttons to see what commands to send
########################################
def sendCommand(sock,command):

    if DEBUG:
        print("Sending..."+command+' to '+str(sock))

    sock.send(bytes(command, "utf-8"))
    

    
########################################
# main program loop
########################################
def mainLoop():
    initialiseAll()
    # take a copy to hold the previous values
    command = ""
    while True:
        conn = getConnection()
        while True:
            previousCommand = command
            try:
                # get command from buttons
                command = getCommandFromButton()
                if commandChanged(command, previousCommand):
                    # send command
                    sendCommand(conn, command)
                
            except Exception as e:
                text = str(e)
                print("mainLoop Exception: "+text)
                break

            sleep(0.1)

########################################
# start it
########################################

mainLoop()