########################################
# WCcar
# Wifi Controlled Car
# This creates a Server running on the Car being controlled remotely over WiFi.
# This Will:
# - connect to a WiFi AccessPoint
# - By default, this will have IP Address of 192.168.4.1  NO - need to check IP Address
# - listen for messages on port 80
# - receive messages on the port
# - parse these into commands
# - use commands to control the motors of the WC Car
# 
########################################
from machine import Pin
from time import sleep  
import network
import socket
from WFServer import WFServer

# WiFi server setup
SERVER = '0.0.0.0'
PORT = 80
WIFI_SSID = "WiFiCar1"
WIFI_PWD = "WiFiCar1"

# Commands that can be received
# Each motor can be fwd, back, stop
CMD_FWD="F"
CMD_BACK="B"
CMD_LEFT="L"
CMD_RIGHT="R"
CMD_STOP="S"
CMD_FIRE="X"

DEBUG=True

########################################
# PINOUTS - the "d" is the pin on the microcontroller
# which is mapped to a Pin number in the code as follows
# MOTOR A is
#    d8=15
#    d7=13
# MOTOR B is
#    d6=12
#    d5=14
# Other pins - not used
# d0 = 16  d1=05  d2=04  d3=0 d4=02
########################################
# Motor A - select appropriate pin numbers - One pin for forward, one for back
motor1Pin1 = Pin(15, Pin.OUT)
motor1Pin2 = Pin(13, Pin.OUT)
# Motor B - select appropriate pin numbers - One pin for forward, one for back
motor2Pin1 = Pin(12, Pin.OUT)
motor2Pin2 = Pin(14, Pin.OUT)
# PIN for the Laser - not used
# laserPin= Pin(5, Pin.OUT)

# To make the LED flash
LED = Pin(2, Pin.OUT)   # onboard LED is PIN 2
    
########################################
# initialiseAll
########################################
def initialiseAll():

    for x in range(4):
        LED.value(False)
        sleep(0.3)
        LED.value(True)
        
    testMotors()


########################################
# test if motors working
########################################
def testMotors():
    
    setMotorPinsFromCommands(CMD_FWD, CMD_FWD, CMD_STOP)
    sleep (0.5)
    runMotorsFromMsg(CMD_BACK + CMD_BACK + CMD_STOP)
    sleep (0.5)
    runMotorsFromMsg(CMD_FWD + CMD_BACK + CMD_STOP)
    sleep (0.5)
    runMotorsFromMsg(CMD_BACK + CMD_FWD + CMD_STOP)
    sleep (0.5)
    runMotorsFromMsg(CMD_STOP + CMD_STOP + CMD_STOP)


########################################
# runMotorsFromMsg
########################################
def runMotorsFromMsg(msg):
    
    if len(msg) > 2:
        setMotorPinsFromCommands(msg[0], msg[1], msg[2])
    else:
        setMotorPinsFromCommands(CMD_STOP, CMD_STOP, CMD_STOP)

########################################
# runMotorsFromCommand
# Commands involve F, B, L, R
# NEED TO RETHINK THIS
########################################
def runMotorsFromCommand(msg):

    if len(msg) >0:
        cmd =msg[0:1]
        
        if cmd == CMD_FWD:
            setMotorPinsFromCommands(CMD_FWD, CMD_FWD, CMD_STOP)
        elif cmd == CMD_BACK:
            setMotorPinsFromCommands(CMD_BACK, CMD_BACK, CMD_STOP)
        elif cmd == CMD_LEFT:
            setMotorPinsFromCommands(CMD_BACK, CMD_FWD, CMD_STOP)
        elif cmd == CMD_RIGHT:
            setMotorPinsFromCommands(CMD_FWD, CMD_BACK, CMD_STOP)
        
        elif cmd == CMD_STOP:
            setMotorPinsFromCommands(CMD_STOP, CMD_STOP, CMD_STOP)

    else:
        setMotorPinsFromCommands(CMD_STOP, CMD_STOP, CMD_STOP)
    



########################################
# use the commands to set the pins that drive the motors
# left
########################################
def setMotorPinsFromCommands(left, right, laser):

    LED.value(False)
    if DEBUG:
        print("setMotorPinsFromCommands |"+str(left)+":"+str(right)+":"+str(laser)+"|")

    motor1_1 = False
    motor1_2 = False
    motor2_1 = False
    motor2_2 = False
    
    # Set the left motor
    if left == CMD_FWD:
        motor1_1 = True
    elif left == CMD_BACK:
        motor1_2 = True
    
    # Set the right motor
    if right == CMD_FWD:
        motor2_1 = True
    elif right == CMD_BACK:
        motor2_2 = True

    # Set the values of the motors
    setMotors(motor1_1, motor1_2, motor2_1, motor2_2)
        
    LED.value(True)
       
########################################
# set each of the 4 motors
########################################
def setMotors( motor1_1,  motor1_2,  motor2_1,  motor2_2):

    motor1Pin1.value(motor1_1)
    motor1Pin2.value(motor1_2)
    motor2Pin1.value(motor2_1)
    motor2Pin2.value(motor2_2)

   

########################################
# main program loop
########################################
def mainLoop():
    initialiseAll()
    
    if DEBUG:
        print('Try connect to AP ')
    wfServer = WFServer(WIFI_SSID, WIFI_PWD)
    wfServer.createAccessPoint()
    
    if DEBUG:
        if wfServer.isAPConnected():
            print('Connected to '+str(wfServer.getIPAddress()))
        else:
            print('Not Connected')
            
    # Initialise Server
    if DEBUG:
        print("call initialiseServer")
    wfServer.createServerSocket()
    
    # Loop getting commands
    while True:
        if DEBUG:
            print('wait for a conection')
            
        wfServer.waitForConnection()
        if wfServer.hasConnection() :
            if DEBUG:
                print("got connection")  
            # Loop getting commands processing anything that we receive
            try:
                while True:
                    if DEBUG:
                        print("Get Message")
                    
                    # get the commnd & then execute it
                    # cmd = wfServer.getCommand()
                    # runMotorsFromCommand(cmd)
                    
                    # Get a message & then execute it  '\n' is terminator for the command
                    msg = wfServer.getMessage('\n')
                    runMotorsFromMsg(msg)
                               
                    
                    
                    sleep(0.05)
                    
            except Exception as e:
                text = str(e)
                print("mainLoop Exception: "+text)
    
        if DEBUG:
            print("Close Connection")
        wfServer.closeConnection()
        setMotorPinsFromCommands(CMD_STOP, CMD_STOP, CMD_STOP)


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

mainLoop()