# Route Setter
# ============
issue = "0.70 09-Jan-17"

# Sets up Model Railway Routes using Servo Motors
# controlled by 3 Pololu Maestro Servo Controllers
#   at present just 1 Pololu Maestro Servo Controller

# GPIO Assigments avoid the GPIO used by SPROG PI

#TO DO
##### Check what SEE is showing??



# Get Required Modules
# ====================
# This MUST appear FIRST in script
from os import system
from smbus import SMBus
from gpiozero import LED
from gpiozero import LEDBoard as KeyBoard
from gpiozero import Button as Key
from signal import pause
from time import time
from time import sleep
from datetime import datetime as clock
from threading import Thread

import RPi.GPIO as GPIO
GPIO.setwarnings(False)

# System Assignments
# ==================
# This MUST be Done NEXT
# and CAN NOT be in a procedure as
# the variables would NOT then be Global.


# Define Alphanumeric 4x20 lcd on GPIO
# ====================================

# LCD Configuration
# -----------------

# Define LCD to GPIO mapping 
# Using "Adafruit Perma-Proto Pi HAT"
LCD_RS = 4
LCD_E  = 17
LCD_D4 = 18
LCD_D5 = 27
LCD_D6 = 22
LCD_D7 = 23

GPIO.setmode(GPIO.BCM)  # Use BCM GPIO numbers
GPIO.setup(LCD_E, GPIO.OUT)  # E
GPIO.setup(LCD_RS, GPIO.OUT) # RS
GPIO.setup(LCD_D4, GPIO.OUT) # DB4
GPIO.setup(LCD_D5, GPIO.OUT) # DB5
GPIO.setup(LCD_D6, GPIO.OUT) # DB6
GPIO.setup(LCD_D7, GPIO.OUT) # DB7

# Define some LCD constants
LCD_WIDTH = 20
LCD_CHR = True
LCD_CMD = False

# LCD RAM address for the 1st to 4th lines
LCD_line =[0x80,0xC0,0x94,0xD4]

# Timing constants
E_PULSE = 0.0005
E_DELAY = 0.0005

LCD_Buffer = ["","","",""]
locked = False


# Assignments of I/O
# ==================
# CAN NOT be in a procedure as
# the variables would NOT then be Global.
# Must be defined BEFORE referenced in procedures.


# GPIO Assignments for Control Panel Keys
# =======================================
# Using "Adafruit Perma-Proto Pi HAT"
# GPIO assigned to Rows
rows = KeyBoard(24,25,9,7,5)
# Sets up Row Patterns; Low is ON.
row = (0,1,1,1,1),(1,0,1,1,1),(1,1,0,1,1),(1,1,1,0,1),(1,1,1,1,0)
# GPIO assigned to Columns
columns = (Key(6), Key(12), Key(13), Key(16),
           Key(19), Key(20), Key(21),Key(26))


# Define RunMode Switch
# =====================
stop_switch = Key(10) # GPIO 10 - May be used by future SPROG-Pi


# Procedure used to Assign I2C I/O
# ================================

i2c_bus = SMBus(1) # Port 1
# I2C Device Addresses shown by using Terminal "i2cdetect -y 1"

def assign(addr,a,b):
    """ Device Address, Assign Bits a to b """
    # Address is as shown by Terminal "i2cdetect -y 1"
    v = 0
    for i in range(a,b+1):
        v += 2**(i-1)
    m = 2**(a-1)
    v = (addr * 0x10000) + (v * 0x100) + m
    # aaaaaaaa vvvvvvvv mmmmmmmm
    # address , bits assigned & data multiplier
    return v


# Assign Control Panel LEDs
# =========================

# Outputs on PCF8574  @ #20
addr = 0x20
inner_leds = assign(addr,1,6)
busy_led = assign(addr,8,8)  # Large Red
    # Spare 7
    # The Small Green DCC LED is driven by SPROG-Pi directly

# Outputs on PCF8574 @ #21
addr = 0x21
outer_leds = assign(addr,1,4)
quarry_leds = assign(addr,5,8)


# Assign Servo Control Signals
# ============================
# Long I2C-1 bus

# Assign Servo Controller for Sidings and Inner/Outer Holdings
# ------------------------------------------------------------
# Outputs on PCF8574 @ #22
addr = 0x22
sidings_op = assign(addr,1,5) # 5 bits gives 31 Sidings routes
sidings_busy = assign(addr,6,6) # Servo moving flag
short_DCC_relay = assign(addr,8,8) # E-Stop Relay
    # Spare 7


# Servo Controller for Main Line to Sidings
# -----------------------------------------
# I/O on PCF8574 @ #23
# *******************************
# Insert "Servo Mainline - Sidings" here:


# Servo Controller for Quarry
# ---------------------------
# I/O on PCF8574 @ #24
# *******************************
# Insert "Servo Quarry" here:


# Assign Track Sensors and ABC Relays
# ===================================
# Long I2C-1 bus

# Track Sensors Inner & Holding
# -----------------------------
# Inputs on PCF8574 @ #25
addr = 0x25
inner_sensors = assign(addr,1,6)
full_inner_sensor_Y = 1  # Yellow
full_inner_sensor_R = 2  # Red
inner_holding_2_sensor_Y = 4  # Yellow
inner_holding_2_sensor_R = 8  # Red
inner_holding_1_sensor_Y = 16  # Yellow
inner_holding_1_sensor_R = 32  # Red
    # Spare 7,8

# ABC NC Relays & R-PiZ Turntable 
# -------------------------------
# Ouputs on PCF8574 @ #26
addr = 0x26
ABC_relays = assign(addr,1,5)
full_outer_ABC_relay = assign(addr,1,1)
outer_holding_ABC_relay = assign(addr,2,2)
full_inner_ABC_relay = assign(addr,3,3)
inner_holding_2_ABC_relay = assign(addr,4,4)
inner_holding_1_ABC_relay = assign(addr,5,5)
# Spare 6
turntable = assign(addr,7,8)
turntable_turn = assign(addr,7,7)
turntable_RPiZ_stop = assign(addr,8,8)

 

# Track Sensors Outer & Holding & also DCC Ok
# -------------------------------------------
# Inputs on PCF8574 @ #27
addr = 0x27
outer_sensors = assign(addr,1,4)
full_outer_sensor_Y = 1  # Yellow
full_outer_sensor_R = 2  # Red
outer_holding_sensor_Y = 4  # Yellow
outer_holding_sensor_R = 8  # Red
    # Spare 5,6,7,8

# Track Sensors Quarry
# --------------------
# Inputs on PCF8574 @ #28
# *******************************
# Insert "Sensors Quarry" here:


# Assign Route Numbers
# ====================

# Define Sidings Route Numbers
# these will be the numbers got from Keypad - on Rows 1, 2 & 3
# then adjusted by the Route Look-Up Array
# and output to 24 channel Servo Controller A as 5 bit word
# on "sidings_op"
incoming = 1
run_round = 2
outgoing = 3
good_shed = 4
small_goods = 5
cattle_dock = 6
factory = 7
coal_yard = 8
engine_shed = 9
turntable = 10
shunter = 11
sidings = 12
inner_holding_1 = 13
inner_holding_2 = 14
full_inner_loop = 15
outer_holding = 16
full_outer_loop = 17


# Define Mainline Route Numbers
# these will be the numbers got from Keypad - on Row 3
# then adjusted by the Route Look-Up Array
# and output to 12 Channel Servo Controller B as 3 bit word
# on "mainline_op"
main_inner_loop = 18
main_outer_loop = 19
sidings_in = 20
sidings_out = 21
# Possibly use Spare Channels for Section Isolation.

# Define Quarry Route Numbers
# these will be the number got from Keypad - on Row 4
# then adjusted by the Route Look-Up Array
# and output to 12 Channel Servo Controller C as 3 bit word
# on "quarry_op"
quarry_mainline = 25
quarry_holding = 26
quarry_siding_1 = 27
quarry_siding_2 = 28
quarry_siding_3 = 29
# Possibly use Spare Channels for Section Isolation.

# Define valus for Tasks
control_task = 1
sensor_task = 2
outer_hold = 4
outer_hold_1 = 8
inner_hold = 16
inner_hold_1 = 32
inner_hold_2 = 64
quarry_hold = 128
quarry_hold_1 = 256


# Assign Route Numbers & Names
# ===========================

# Keypad Number to Route Number Look-Up Array
route_number = (0,  1,   2,   3,   4,  5,  6, 7, 8,    9, 10, 11,  13, 14, 15, 16, 17, 
                               18, 19, 12, 20, 21, 0, 0, 0,     25, 26, 27, 28, 29  )

route_name = ("","Incoming","Run Round","Outgoing","Goods Shed","Small Goods",
              "Cattle Dock","Factory","Coal Yard","Engine Shed","Turntable",
              "Shunter","Sidings", "Inner Holding 1","Inner Holding 2","Full Inner Loop",
              "Outer Holding","Full Outer Loop",        
              "Main Inner Loop","Main Outer Loop", "Sidings In","Sidings Out",
              "","","",
              "Quarry Mainline","Quarry Holding",
              "Quarry Siding 1", "Quarry Siding 2","Quarry Siding 3",
              "")


# Assign Action Names
# ===================

action_name = ("","","DCC Stopped","Sidings R-IN","Sidings R-OUT")


# Assign Values for Lap Lengths
# =============================

# Adjust when layout is extended
inner_lap = 4435 # mm of full inner lap
outer_lap = 4575 # mm of full outer lap
# Convert mph = mm/sec * mph_factor
mph_factor = 0.3315



# Check I2C devices are all responding
# ====================================
def check_i2c_devices():
    """ Checks required I2C Devices are available."""
    # NOT CONVERTED TO PYTHON YET
    # ***************************
    return True



# LCD Hardware Initialise
# =======================
def lcd_init():
    # Initialise display
    lcd_byte(0x33,LCD_CMD) # 110011 Initialise
    lcd_byte(0x32,LCD_CMD) # 110010 Initialise
    lcd_byte(0x06,LCD_CMD) # 000110 Cursor move direction
    lcd_byte(0x0C,LCD_CMD) # 001100 Display On,Cursor Off, Blink Off
    lcd_byte(0x28,LCD_CMD) # 101000 Data length, number of lines, font size
    lcd_byte(0x01,LCD_CMD) # 000001 Clear display
    sleep(E_DELAY)


def lcd_byte(bits, mode):
    # Send byte to data pins
    # bits = data
    # mode = True  for character
    #        False for command

    GPIO.output(LCD_RS, mode) # RS

    # High bits
    GPIO.output(LCD_D4, False)
    GPIO.output(LCD_D5, False)
    GPIO.output(LCD_D6, False)
    GPIO.output(LCD_D7, False)
    if bits&0x10==0x10:
        GPIO.output(LCD_D4, True)
    if bits&0x20==0x20:
        GPIO.output(LCD_D5, True)
    if bits&0x40==0x40:
        GPIO.output(LCD_D6, True)
    if bits&0x80==0x80:
        GPIO.output(LCD_D7, True)

    # Toggle 'Enable' pin
    lcd_toggle_enable()

    # Low bits
    GPIO.output(LCD_D4, False)
    GPIO.output(LCD_D5, False)
    GPIO.output(LCD_D6, False)
    GPIO.output(LCD_D7, False)
    if bits&0x01==0x01:
        GPIO.output(LCD_D4, True)
    if bits&0x02==0x02:
        GPIO.output(LCD_D5, True)
    if bits&0x04==0x04:
        GPIO.output(LCD_D6, True)
    if bits&0x08==0x08:
        GPIO.output(LCD_D7, True)

    # Toggle 'Enable' pin
    lcd_toggle_enable()

def lcd_toggle_enable():
    # Toggle enable
    sleep(E_DELAY)
    GPIO.output(LCD_E, True)
    sleep(E_PULSE)
    GPIO.output(LCD_E, False)
    sleep(E_DELAY)



# LCD Procedures
# --------------

def lcd_lock(on):
    """Queues access to LCD"""
    global locked
    if on: # wait till unlocked then lock lcd.
        while locked: sleep(0.01)
        locked = True
    else: locked = False
    
def lcd_write(message,ln):
    """ Send message to LCD display on specified line (1-4).
        Message length MUST be 20."""
    lcd_byte(LCD_line[ln - 1], LCD_CMD)
    for i in range(LCD_WIDTH):
        lcd_byte(ord(message[i]),LCD_CHR)

def lcd_update():
    """ Writes all buffer lines to LCD Display."""
    for ln in range(4): # Display the Four Lines
        lcd_write(LCD_Buffer[ln],ln + 1)

def lcd_buff(message,ln):
    """ Checks length of message and adds spaces to fill width,
        Places message in LCD Buffer at specified line (1-4)."""
    if len(message) < LCD_WIDTH:
        message = message.ljust(LCD_WIDTH," ")
    LCD_Buffer[ln - 1] = message

def lcd_print(message):   # Takes ~ 450 mS
    """ Checks length of message and adds spaces to fill width.
        Scrolls LCD Buffer ines up and adds message to bottom line.
        Then updates the LCD Display with all lines in Buffer"""
    lcd_lock(1)
    if len(message) < LCD_WIDTH:
        message = message.ljust(LCD_WIDTH," ")
    for ln in range(3):
        LCD_Buffer[ln] = LCD_Buffer[ln+1]
    LCD_Buffer[3] = message
    lcd_update()
    lcd_lock(0)

def lcd_cls():
    """Clears the LCD Display & the Message Buffer"""
    lcd_byte(0x01, LCD_CMD)
    for ln in range(4): LCD_Buffer[ln] = "".ljust(LCD_WIDTH," ")

def lcd_date_time():
    """Shows date-Time on LCD"""
    lcd_print(clock.now().strftime("%Y-%m-%d %H:%M").center(20," "))
    sleep(2)



# Procedures used to define Control Panel Keys
# ============================================
# 5x8 keypad on using Raspberry PI GPIO
        # first 3 rows are Control Panel Push Buttons
        # last row is keypad under LCD & miscellaneous

def get_key():
    """Returns Key Number pressed, 0 if NO Key"""
    kn = 0
    for r in range(5):
        rows.value = row[r]
        for c in range(8):
            if columns[c].is_pressed:
                kn = r*8 + c+1
                break
    return kn


def await_key():
    """Waits for a Key to be pressed, then returns it's Number"""
    kn = 0
    while kn == 0:
        for r in range(5):
            rows.value = row[r]
            for c in range(8):
                if columns[c].is_pressed:
                    kn = r*8 + c+1
                    break
    return kn


def wait_for_key(k):
    """Waits for a Specified Key to be pressed."""
    kn = 0
    while kn != k:
        for r in range(5):
            rows.value = row[r]
            for c in range(8):
                if columns[c].is_pressed:
                    kn = r*8 + c+1
                    break

def wait_no_key():
    """Waits till No Key is being pressed."""
    while get_key() > 0: sleep(0.01)


# ---- LCD Menu Keys --------

def menu_key_pressed():
    """Checks if Menu Key is Pressed"""
    # Menu Exit Key is Row 4, Column 7
    #    Retuns True if it is Pressed
    rows.value = row[4]
    return columns[7].is_pressed

def menu_up_down_key():
    """Checks if Menu Up or Down Key is Pressed"""
    # Menu Up/Down Keys are Row 4, Column 4/5
    #     Returns +1 -1 or 0
    rows.value = row[4]
    return columns[4].is_pressed + (columns[5].is_pressed) * -1

def cancel_key():
    """Exits from Menu without any Action,
       (Menu Key performs action)"""
    # Menu Cancel Key is Row 4, Column 6
    #     Returns True if it is Pressed
    rows.value = row[4]
    return columns[6].is_pressed

def wait_no_menu_key():
    """Waits till No Menu Key is being pressed."""
    rows.value = row[4]
    wait = True
    while wait:
        wait = False
        for c in range(5,8):
            if columns[c].is_pressed: wait = True


# Procedures used to Operate I2C I/O
# ==================================

def i2c_read(name):
    """ Returns Value from the address & bits defined by the I/O Name"""
    # name = device address * 0x10000
    #        + value of bits * 0x100  + value multiplier
    addr = name // 0x10000
    bits = (name % 0x10000 ) // 0x100
    # get value multiplier
    mult = name & 0xff
    # Get current value of whole port
    v = (i2c_bus.read_byte(addr)) ^ 255
    # Mask required bits
    vm = ( v & bits ) // mult
    return vm


def i2c_write(name,value):  # Takes ~ 0.6 mS
    """ Writes new value to the address & bits defined by the I/O Name"""
    # name = device address * 0x10000
    #        + value of bits * 0x100  + value multiplier
    addr = name // 0x10000
    bits = (name % 0x10000 ) // 0x100
    # get value multiplier
    mult = name & 0xff
    # Keep value in range of set bits
    value = (value * mult ) & bits
    # Get current value of whole port
    pv = (i2c_bus.read_byte(addr)) ^ 255
    # Calculate new value to send to port
    nv = (pv & (255 - bits)) ^ value
    i2c_bus.write_byte(addr, nv ^ 255)


def await_servo(n):
    """Waits till Servo Busy flag is Set then Cleared"""
    i = 0
    while not i2c_read(n): # Wait till set
        sleep(0.01)
        i += 1
        if i > 50: # 0.5 sec timeout
            break
    i = 0
    while i2c_read(n): # wait till cleared
        sleep(0.01)
        i += 1
        if i > 200: # 2 sec timeout
            break




# Sub Routines
# ============
# MUST be Read in Script before being Called



# Main Procedures
# ===============

def title(issue):
    lcd_cls()
    lcd_buff("Rowanburn Model Rail",1)
    lcd_buff("Route Setter".center(20," "),2)
    lcd_buff(("Issue "+issue).rjust(20," "),3)
    lcd_buff("Starting".center(20," "),4)
    lcd_update()
    sleep(3)
    lcd_cls()



def initialise():
    global halt, tasks, locked, key, previous_key, control_key
    global route, previous_route, hold_train, hold_all_trains, hold_message
    global sidings_route, mainline_route, quarry_route
    global action, cancel_actions, testing_panel_leds
    global value_inner_sensors, value_outer_sensors, value_quarry_sensors
    global inner_lap_time, ILTime, outer_lap_time, OLTime, manual_time, MMTime

    halt = False
    tasks = 0
    locked = False
    key = 0
    previous_key = 0
    control_key = 0
    route = 0
    previous_route = 0
    hold_train = False
    hold_all_trains = False
    hold_message = ""
    action = 0
    cancel_actions = False
    testing_panel_leds = False
    sidings_route = 0
    mainline_route = 0
    quarry_route = 0
    value_inner_sensors = 0
    value_outer_sensors = 0
    value_quarry_sensors = 0
    inner_lap_time = 0
    ILTime = False
    outer_lap_time = 0
    OLTime = False
    manual_time = 0
    MMTime = False
    
    rows.off()
    i2c_write(short_DCC_relay,0)
    # * Not till Extension* isolate_sidings()
    # * Not till Extension* i2c_write(sidings_relay_incoming,0)
    # * Not till Extension* i2c_write(sidings_relay_outgoing,0)


def set_default_routes():
    lcd_lock(1)
    lcd_cls()
    lcd_buff("Initial Routes",3)
    lcd_buff("    are being Set:",4)
    lcd_update()
    lcd_lock(0)
    route = 1
    set_sidings_route(incoming)
    sleep(0.5)
    set_sidings_route(full_inner_loop)
    sleep(0.5)
    set_sidings_route(full_outer_loop)
    sleep(0.5)
    # * Not till Extension*      route = 17
    # * Not till Extension*      set_mainline_route(main_inner_loop)
    # * Not till Extension*      sleep(0.5)
    # * Not till Extension*      route = 18
    # * Not till Extension*      set_mainline_route(main_outer_loop)
    # * Not till Extension*      sleep(0.5)
    # * Not till Extension*      route = 25
    # * Not till Extension*      set_quarry_route(quarry_mainline)
    # * Not till Extension*      sleep(0.5)
    # * Not till Extension*      isolate_sidings
    sleep(1)
    lcd_lock(1)
    lcd_cls()
    lcd_buff("Initial Routes",2)
    lcd_buff("    have been Set.",3)
    lcd_update()
    sleep(1)
    lcd_cls()
    lcd_lock(0)
    lcd_print("Ready")



# Set Routes on the Servo Controllers & shows Busy
# ================================================

def set_sidings_route(r):
    """ Sends Command to Sidings & Inner/Outer Loop Route Controller 'A'.
              and Sets up Threads for Holding Trains"""
    global sidings_route, previous_route, hold_train, hold_message
    sidings_route = r
    i2c_write(busy_led,1) 
    i2c_write(sidings_op,sidings_route)
    if len(hold_message) > 0:
        lcd_print(hold_message)
        hold_message = ""
    lcd_print(route_name[r])
    sleep(0.1) ############ ???????????????? await_servo(sidings_busy)
    i2c_write(sidings_op,0) ### Shouldn't this be BEORE the LCD Updates
    i2c_write(busy_led,0)   #### AND this.
    # * Not till Extension* i2c_write(sidings_connection_relay,0)
    previous_route = route
    if hold_all_trains:
        hold_train = True
        set_hold_train()


def set_mainline_route(r):
    """ Sends Command to Mainline Route Controller 'B'."""
    global mainline_route, previous_route
    mainline_route = r - 16
    i2c_write(busy_led,1)
    lcd_print(route_name[r])
    # * Not till Extension* i2c_write(mainline_op,mainline_route)
    # * Not till Extension* await_servo(mainline_busy)
    # * Not till Extension* i2c_write(mainline_op,0)
    i2c_write(busy_led,0)
    # * Not till Extension* if mainline_route == 3:
    # * Not till Extension*     set_sidings_isolation_incoming()
    # * Not till Extension* elif mainline_route == 4:
    # * Not till Extension*     set_sidings_isolation_outgoing()
    # * Not till Extension* else:
    # * Not till Extension*    isolate_sidings()
    previous_route = route


def set_quarry_route(r):
    """ Sends Command to Quarry Route Controller 'C'."""
    global quarry_route, previous_route
    quarry_route = r - 24
    i2c_write(busy_led,1)
    lcd_print(route_name[r])
    # * Not till Extension* i2c_write(quarry_op,quarry_route)
    # * Not till Extension* await_servo(quarry_busy)
    # * Not till Extension* i2c_write(quarry_op,0)
    i2c_write(busy_led,0)
    previous_route = route


def set_hold_train():
    """ Starts a Thread to wait for relevant Track Sensor,
                then Isolate that Segment of Track."""
    if (previous_route == full_outer_loop) and not (tasks & outer_hold):
        Thread(target = hold_in_outer).start()
    if (previous_route == outer_holding) and not (tasks & outer_hold_1):
        Thread(target = hold_in_outer_1).start()
    if (previous_route == full_inner_loop) and not (tasks & inner_hold):
        Thread(target = hold_in_inner).start()
    if (previous_route == inner_holding_1) and not (tasks & inner_hold_1):
        Thread(target = hold_in_inner_1).start()
    if (previous_route == inner_holding_2) and not (tasks & inner_hold_2):
        Thread(target = hold_in_inner_2).start()
   
        
def start_the_turntable():
	i2c_write(turntable_turn,1)
	sleep(0.5)
    i2c_write(turntable_turn,0)	


# Set any special Isolation Requirements
# ======================================

def stop_DCC():
    """ Sets a relay which Shorts Out the DCC, so as to do an E-Stop."""
    i2c_write(short_DCC_relay,1)
    lcd_cls()
    lcd_buff("".center(20,"*"),1)
    lcd_buff(action_name[action].center(20," "),2)
    lcd_buff("".center(20,"*"),3)
    lcd_update()
    while get_key() > 0:
        sleep(0.1)
    i2c_write(short_DCC_relay,0)
    sleep(2)


def isolate_sidings():
    """ Sets Sidings Isolation Gap to Open-Circuit"""
    i2c_write(busy_led,1)
    # * Not till Extension* i2c_write(sidings_connection_relay,0)
    sleep(1)
    i2c_write(busy_led,0)


def set_sidings_isolation_outgoing():
    """ Sets Sidings Outgoing Isolation remote relays."""
    i2c_write(busy_led,1)
    if action > 0:
        lcd_print(action_name[action])
    # * Not till Extension* i2c_write(sidings_relay_incoming,0)
    # * Not till Extension* i2c_write(sidings_relay_outgoing,1)
    # * Not till Extension* sleep(0.01)
    # * Not till Extension* i2c_write(sidings_connection_relay,1)
    sleep(1)
    i2c_write(busy_led,0)


def set_sidings_isolation_incoming():
    """ Sets Sidings Incoming Isolation remote relays."""
    i2c_write(busy_led,1)
    if action > 0:
        lcd_print(action_name[action])
    # * Not till Extension* i2c_write(sidings_relay_outgoing,0)
    # * Not till Extension* i2c_write(sidings_relay_incoming,1)
    # * Not till Extension* sleep(0.01)
    # * Not till Extension* i2c_write(sidings_connection_relay,1)
    sleep(1)
    i2c_write(busy_led,0)


def null():
    """ No Action Defined """
    global action
    lcd_print("No such Action")
    sleep(1)
    action = 0


# Diagnostic Procedures
# =====================

def test_panel_leds():
    """ Toggles the Front Panel LEDs at 1 second interval
        The Display Sensors Thread will interact with this."""
    global testing_panel_leds
    lcd_cls()
    lcd_print("Testing LEDs")
    lcd_print("    on Control Panel")
    lcd_print("")
    testing_panel_leds = True
    while True:
        i2c_write(inner_leds,63)
        i2c_write(busy_led,1)
        i2c_write(outer_leds,15)
        i2c_write(quarry_leds,15)
        sleep(0.8)
        i2c_write(inner_leds,0)
        i2c_write(busy_led,0)
        i2c_write(outer_leds,0)
        i2c_write(quarry_leds,0)
        sleep(0.2)
        if cancel_key():
            testing_panel_leds = False
            lcd_cls()
            lcd_print("Ready")
            break


def see_track_sensors():
    """ Display Track Sensors on LCD as binary."""
    lcd_cls()
    lcd_buff("Track Sensors:",1)
    lcd_update()
    while True:
        lcd_write(("Inner:  " + ( bin(i2c_read(inner_sensors))[2:].zfill(6))).rjust(20," "),2)
        lcd_write(("Outer:    " + ( bin(i2c_read(outer_sensors))[2:].zfill(4))).rjust(20," "),3)
        # *** Not till Extension:
        # lcd_write(("  Quarry   " + ( bin(i2c_read(quarry_sensors) ^ 15)[2:].zfill(4))).rjust(20," "),4)
        sleep(0.05)
        if cancel_key():
            lcd_cls()
            lcd_print("Ready")
            break
            
            
def abc(v):
    """ Sets All ABC Modules; 0 off, 1 enable."""
    if v == 0:  # Relays Off - shorts out ABC modules
        i2c_write(ABC_relays, 0)
    else:  # Relays On - enables ABC modules
        i2c_write(ABC_relays, 31)


def see():
    print("Tasks: M " + ( bin(tasks & 3)[2:].zfill(2))
          + " H " + ( bin(tasks // 128)[2:].zfill(2)) + " "
          + ( bin((tasks & 127) // 4)[2:].zfill(5))
          + "   Route:", route, "  P'Route:", previous_route,
            "   Hold:", hold_train,
            " Hold All:", hold_all_trains )



# Timing Procedures for Software Loop Timings
# -------------------------------------------

def timer_reset():
    """Resets the Millisecond Timer"""
    global tmr_count, tmr_accumulate
    tmr_count = 0
    tmr_accumulate = 0

def timer_start():
    """Millisecond Timer - gets start time"""
        # "start" then "stop" takes 15 uS
    global tmr_start
    tmr_start = time()       

def timer_stop(n,p,r):
    """Millisecond Timer - gets time since start, and after n timings
       calculates the average time.
       If "p" is True, prints the result. If r True, reset timer"""
    global tmr_result, tmr_count, tmr_accumulate
    if tmr_count < n:
        tmr_accumulate += (time() - tmr_start)
    tmr_count += 1
    if tmr_count == n:
        tmr_result = ( tmr_accumulate / tmr_count ) * 1000
        if p: print( '%8.3f' % tmr_result,"ms, averaged over",n,"loops")        
        if r: timer_reset()


# Timing Procedures for Train Timings
# ===================================

# -------Inner Lap Speed ---------

def measure_inner_speed():
    """Measures the Lap Speed using Track Sensor"""
    global ILTime
    if ILTime == 0:
        lcd_cls()
        lcd_print("Measure Inner Speed:")
        sleep(0.1)
        ILTime = -1
        Thread(target = inner_lap_timer).start()


def inner_lap_timer():
    """ Started as a TASK - I/O Name and Start/Stop Bit (1-8)"""
    global ILTime, inner_lap_time
    name = inner_sensors
    bit = 1
    while ((i2c_read(name) & bit) == 0):
        if cancel_actions:
            ILTime = 0
            break
        else: sleep(0.01)
    st = time()
    lcd_print("Inner Speed STARTED:")
    if ILTime < 0 : sleep(5) # Give time for train to clear sensor
    while (ILTime < 0) & ((i2c_read(name) & bit) == 0):
        if cancel_actions:
            ILTime = 0
            break
        else: sleep(0.01)
    et = time()
    if ILTime < 0:
        inner_lap_time = et - st
    else: inner_lap_time = 0
    ILTime = True


def show_inner_speed():
    """Calulates and displays Lap Speed"""
    global ILTime
    lcd_print("Inner Lap Speed:")
    if inner_lap_time > 0:
        mph = (inner_lap / inner_lap_time) * mph_factor
        txt = "= " + str(round(mph,1)) + " mph"
        lcd_print(txt.rjust(20," "))
    else:
        lcd_print("Aborted!".rjust(20," "))
    ILTime = False


# -------Outer Lap Speed ---------

def measure_outer_speed():
    """Measures the Lap Speed using Track Sensor"""
    global OLTime
    if OLTime == 0:
        lcd_cls()
        lcd_print("Measure Outer Speed:")
        sleep(0.1)
        OLTime = -1
        Thread(target = outer_lap_timer).start()


def outer_lap_timer():
    """ Started as a TASK - I/O Name and Start/Stop Bit (1-8)"""
    global OLTime, outer_lap_time
    name = outer_sensors
    bit = 1
    while ((i2c_read(name) & bit) == 0):
        if cancel_actions:
            OLTime = 0
            break
        else: sleep(0.01)
    st = time()
    lcd_print("Outer Speed STARTED:")
    if OLTime < 0 : sleep(5) # Give time for train to clear sensor
    while (OLTime < 0) & ((i2c_read(name) & bit) == 0):
        if cancel_actions:
            OLTime = 0
            break
        else: sleep(0.01)
    et = time()
    if OLTime < 0:
        outer_lap_time = et - st
    else: outer_lap_time = 0
    OLTime = True


def show_outer_speed():
    """Calulates and displays Lap Speed"""
    global OLTime
    lcd_print("Outer Lap Speed:")
    if outer_lap_time > 0:
        mph = (outer_lap / outer_lap_time) * mph_factor
        txt = " = " + str(round(mph,1)) + " mph"
        lcd_print(txt.rjust(20," "))
    else:
        lcd_print("Aborted!".rjust(20," "))
    OLTime = False


# -------Manual Speed over a Metre ---------

def manual_speed():
    """Measures the Speed over a Measured Metre"""
    global MMTime
    if MMTime == 0:
        lcd_cls()
        lcd_print("Measure Speed - 1m:")
        lcd_print("Menu to STOP".rjust(20," "))
        sleep(0.1)
        MMTime = -1
        Thread(target = manual_metre_timer).start()


def manual_metre_timer():
    """ Started as a TASK - Keypad Start/Stop"""
    global MMTime, manual_time
    st = time()
    while True:
        if menu_key_pressed():
            break       
        if cancel_actions:
            MMTime = 0
            break
        else: sleep(0.01)
    et = time()
    if MMTime < 0:
        manual_time = et - st
    else: manual_time = 0
    MMTime = True


def show_manual_speed():
    """Calulates and displays Speed over a Metre"""
    global MMTime
    distance = 1000
    lcd_print("Speed over a Metre:")
    if manual_time > 0:
        mph = (distance / manual_time) * mph_factor
        txt = "= " + str(round(mph,1)) + " mph"
        lcd_print(txt.rjust(20," "))
    else:
        lcd_print("Aborted!".rjust(20," "))
    MMTime = False
    wait_no_key()



# Perform Actions
# ===============

menu_item = ("","Set Default Routes",
             "Speed over 1 metre","Inner Lap Timer","Outer Lap Timer",
             "Test Panel LEDs","See Track Sensors","","","")

def do_menu():
    """Called when Menu Key Pressed, uses Up/Down keys to Display
       the Menu Item, then Menu to Inititate it, Exit to Escape"""
    global cancel_actions
    lcd_cls()
    wait_no_menu_key()
    item = 0
    while True:
        lcd_buff("->" + menu_item[item],4)
        for ln in range(1,4):
            lcd_buff("  " + menu_item[item+ln], 4 - ln)
        lcd_update()
        if menu_key_pressed():
            if item == 1: set_default_routes()
            if item == 2: manual_speed()
            if item == 3: measure_inner_speed()
            if item == 4: measure_outer_speed()
            if item == 5: test_panel_leds()
            if item == 6: see_track_sensors()
            break
        if cancel_key():
            lcd_cls()
            lcd_print("Ready")
            break
        item = item + menu_up_down_key()
        if item < 0: item = 6
        if item > 6: item = 1
    sleep(0.1)
    wait_no_menu_key()


def do_action():
    """Performs required Anciliary Control Action"""
    global cancel_actions, hold_train, hold_all_trains
    cancel_actions = False
    if ( action > 1) and ( action < 5 ) :
        if action == 1: start_the_turntable()
        if action == 2: stop_DCC()
        if action == 3: set_sidings_isolation_outgoing()
        if action == 4: set_sidings_isolation_incoming()
        
    # Keypad below Display Screen
    if action == 5:
        set_hold_train()

    if action == 6:
        hold_all_trains = not hold_all_trains
        if hold_all_trains:
            lcd_print("      (Hold ALL ON )")
        else:
            hold_train = False
            abc_off()
            lcd_print("      (Hold ALL OFF)")
            

    if action == 7:
        cancel_actions = True
        hold_train = False
        hold_all_trains = False

    if action == 8: do_menu()



# Program Shut Down Procedures
# ============================


def stop():
    """Manually called from Idle"""
    global halt, cancel_actions, hold_train, hold_all_trains
    cancel_actions = True
    hold_train = False; hold_all_trains = False
    route = 0; previous_route = 0
    halt = True
    

def RunMode():
    """Checks stop_switch and halt flag
           which is set manually via Idle"""
    return ( stop_switch.is_pressed == False ) and ( halt == False )


def turn_all_off():
    """Turns Off all Outputs"""
    i2c_write(sidings_op,0)
    # * Not till Extension* i2c_write(mainline_op,0)
    # * Not till Extension* i2c_write(quarry_op,0)
    i2c_write(ABC_relays,0)
    i2c_write(turntable,0)
    i2c_write(inner_leds,0)
    i2c_write(outer_leds,0)
    i2c_write(quarry_leds,0)
    i2c_write(busy_led,0)
    rows.off()
    

def shut_down():
    """Option to SHUTDOWn or REBOOT"""
    lcd_cls()
    lcd_write("!! Program EXITED !!",1)
    lcd_write("   as NOT in RunMode",2)
    lcd_write("Will SHUTDOWN in 5s ",3)
    lcd_write("unless RunMode RESET",4)
    print("Program EXITED - as NOT in RunMode")
    # Send Stop to Turntable PiZ
    i2c_write(turntable_RPiZ_stop,1)
    
    # Check if RunMode is reset within 5 seconds
    time_out = 50  # 5 seconds
    while time_out > 0:
        if RunMode():
            break
        sleep(0.1)
        time_out -= 1

    # If TimedOut do a Full Shutdown
    if time_out <= 0:
        lcd_cls()
        lcd_write("!! SHUTTING DOWN !! ",1)
        lcd_write("WAIT 10 seconds     ",3)
        lcd_write("    BEFORE Power Off",4)
        print("SYSTEM SHUTTING DOWN")
        GPIO.cleanup()
        system("sudo shutdown -h now")

        # Otherwise Reboot or Finish
    else:
        lcd_cls()
        lcd_write("! RunMode is reset !",1)
        lcd_write("Press MENU to REBOOT",3)
        lcd_write(" or CANCEL to FINISH",4) 
        # Send Cancel Stop to Turntable PiZ
        i2c_write(turntable_RPiZ_stop,0)   

        while RunMode():
            if menu_key_pressed(): # Do a Reset
                lcd_cls()
                lcd_write("! SYSTEM REBOOTING !",1)
                lcd_write("     Please WAIT    ",3)
                print("SYSTEM REBOOTING")
                # Send Reboot to Turntable PiZ
                i2c_write(turntable_turn,1)

                system("sudo shutdown -r now")
            if cancel_key(): # Do Nothing
                lcd_cls()
                lcd_write("! Program FINISHED !",2)
                print("Program FINISHED")
                break
        


# Program Threads
# ===============
# Threads being Run in Parallel

# Train Holding Threads
# =====================

def abc_off():
    """ Sets All ABC Modules to OFF"""
    i2c_write(ABC_relays,0)
    

def hold_in_outer():
    """Awaits Full Outer Yellow Sensor triggered, then isolates Full Outer"""
    global tasks, route, hold_train, hold_message
    tasks += outer_hold
    hold_train += outer_hold
    lcd_print("     . . . will HOLD")
		########## ?? need to check RED sensor
    sleep(10) # To allow train to clear ABC Section
    i2c_write(full_outer_ABC_relay, 1) # Start ABC module
    while ( value_outer_sensors & full_outer_sensor_Y ) == 0:
        sleep(0.01)
        if not hold_train:
            i2c_write(full_outer_ABC_relay, 0)  # Stop ABC
            lcd_print("  ....HOLD CANCELLED")
            sleep(0.2)
            break
    if hold_train:
        hold_train -= outer_hold
        route = outer_holding
        hold_message = "HOLDING in F-Outer"
        set_sidings_route(route)
        ####### ?? need to check NOT RED sensor of Outer Holding
        #######  in case of over-run ???        Then if OK
        i2c_write(full_outer_ABC_relay, 0)  # Stop ABC module
    tasks -= outer_hold
    

def hold_in_outer_1():
    """Awaits Outer_1 Yellow Sensor triggered, then isolates Outer Holding"""
    global tasks, route, hold_train, hold_message
    tasks += outer_hold_1
    hold_train += outer_hold_1
    lcd_print("     . . . will HOLD")
		########## ?? need to check RED sensor
    sleep(10) # To allow train to clear ABC Section
    i2c_write(outer_holding_ABC_relay, 1) # Start ABC module
    while ( value_outer_sensors & outer_holding_sensor_Y ) == 0:
        sleep(0.01)
        if not hold_train:
            i2c_write(outer_holding_ABC_relay,0) # Stop ABC
            lcd_print("  ....HOLD CANCELLED")
            sleep(0.2)
            break
    if hold_train:
        hold_train -= outer_hold_1
        route = full_outer_loop
        hold_message = "HOLDING in Outer 1"
        set_sidings_route(route)
        ####### ?? need to check NOT RED sensor of Outer Holding
        #######  in case of over-run ???    Then if OK
        i2c_write(outer_holding_ABC_relay,0) # Stop ABC
    tasks -= outer_hold_1
    

def hold_in_inner():
    """Awaits Full Inner Yellow Sensor triggered, then isolates Full Inner"""
    global tasks, route, hold_train, hold_message
    tasks += inner_hold
    hold_train += inner_hold
    lcd_print("     . . . will HOLD")
    sleep(10) # To allow train to clear ABC Section
    i2c_write(full_inner_ABC_relay, 1) # Start ABC module
    while ( value_inner_sensors & full_inner_sensor_Y ) == 0:
        sleep(0.01)
        if not hold_train:
            i2c_write(full_inner_ABC_relay, 0) # Stop ABC
            lcd_print("  ....HOLD CANCELLED")
            sleep(0.2)
            break
    if hold_train:
        hold_train -= inner_hold
        route = inner_holding_2
        hold_message = "HOLDING in F-Inner"
        set_sidings_route(route)
        ####### ?? need to check NOT RED sensor of Outer Holding
        #######  in case of over-run ???    Then if OK
        i2c_write(full_inner_ABC_relay, 0) # Stop ABC
    tasks -= inner_hold

        
def hold_in_inner_1():
    """Awaits Inner_1 Yellow Sensor triggered, then isolates Inner Holding 1"""
    global tasks, route, hold_train, hold_message
    tasks += inner_hold_1
    hold_train += inner_hold_1
    lcd_print("     . . . will HOLD")
    sleep(10) # To allow train to clear ABC Section
    i2c_write(inner_holding_1_ABC_relay, 1) # Start ABC module
    while ( value_inner_sensors & inner_holding_1_sensor_Y ) == 0:
        sleep(0.01)
        if not hold_train:
            i2c_write(inner_holding_1_ABC_relay, 0) # Stop ABC
            lcd_print("  ....HOLD CANCELLED")
            sleep(0.2)
            break
    if hold_train:
        hold_train -= inner_hold_1
        route = full_inner_loop
        hold_message = "HOLDING in Inner 1"
        set_sidings_route(route)
        ####### ?? need to check NOT RED sensor of Outer Holding
        #######  in case of over-run ???    Then if OK
        i2c_write(inner_holding_1_ABC_relay, 0) # Stop ABC
    tasks -= inner_hold_1
    
    
def hold_in_inner_2():
    """Awaits Inner_2 Yellow Sensor triggered, then isolates Inner Holding 2"""
    global tasks, route, hold_train, hold_message
    tasks += inner_hold_2
    hold_train += inner_hold_2
    lcd_print("     . . . will HOLD")
    sleep(10) # To allow train to clear ABC Section
    i2c_write(inner_holding_2_ABC_relay, 1) # Start ABC module
    while ( value_inner_sensors & inner_holding_2_sensor_Y ) == 0:
        sleep(0.01)
        if not hold_train:
            i2c_write(inner_holding_2_ABC_relay, 0) # Stop ABC
            lcd_print("  ....HOLD CANCELLED")
            sleep(0.2)
            break
    if hold_train:
        hold_train -= inner_hold_2
        route = full_inner_loop
        hold_message = "HOLDING in Inner 2"
        set_sidings_route(route)
        ####### ?? need to check NOT RED sensor of Outer Holding
        #######  in case of over-run ???    Then if OK
        i2c_write(inner_holding_2_ABC_relay, 0) # Stop ABC
    tasks -= inner_hold_2

    
# Display Track Senors THREAD
# ===========================
# Current loop of Inner & Outer Sensors, excluding the sleep, takes 1400us
def display_sensors():
    """Read Track Sensors and Display on Control Panel"""
    lcd_print("SHOW SENSORS Started")
    global tasks, value_inner_sensors, value_outer_sensors, value_quarry_sensors
    tasks += sensor_task

    while RunMode():
        # This loop takes 2 + 10 ms while quiescent with RPiZ
        value_inner_sensors = i2c_read(inner_sensors)
        i2c_write(inner_leds,value_inner_sensors)
        value_outer_sensors = i2c_read(outer_sensors)
        i2c_write(outer_leds,value_outer_sensors)
        # * Not till Extension* value_quarry_sensors = i2c_read(quarry_sensors)
        # * Not till Extension* i2c_write(quarry_leds,value_quarry_sensors)
        sleep(0.01) # 10ms,
                    # Without this the display on Main Control Thread is corrupted
        while testing_panel_leds: sleep(0.1) # Pause till Testing cancelled

    # Display Sensors Thread Ending, as Not in RunMode
    # Wait for all HOLD Threads to End
    while tasks > 3: sleep(0.5)
    lcd_cls()
    lcd_print("SHOW SENSORS Ended")
    tasks -= sensor_task


# Main Control THREAD
# ===================
def control():
    """Main Control Loop,
       Reads Control Keys and Takes Required Action"""
    lcd_print("CONTROL is Started")
    lcd_print("")
    lcd_date_time()

    global tasks, control_key, previous_route, route, action, hold_train
    tasks += control_task
    lcd_cls()
    if RunMode():
        set_default_routes()

    while RunMode():
        # This loop takes 109 ms while quiescent with RPiZ

        if ILTime > 0:
            show_inner_speed()
        if OLTime > 0:
            show_outer_speed()
        if MMTime > 0:
            show_manual_speed()

        kn = get_key()

        if kn == 0:
            sleep(0.1)
        else:
            control_key = kn
 
            if control_key < 30:
                route = route_number[ control_key]
                action = 0
            else:
                route = 0
                action = control_key -32
                do_action()

            if route != previous_route:
                if (route >=  1) & (route <= 17):
                    set_sidings_route(route)

                if (route >= 18) & (route <= 21):
                    set_mainline_route(route)
                    if route == sidings_in:
                       set_sidings_isolation_incoming()
                    if route == sidings_out:
                       set_sidings_isolation_outgoing()

                if (route >= 25) & (route <= 29):
                    set_quarry_route(route)
 
    # Control Thread Ending, as Not in RunMode
    # Wait for Display Sensors Thread to End
    hold_train = False
    while tasks > 1: sleep(0.5)
    lcd_print("CONTROL has Ended")
    tasks -= control_task
    turn_all_off()
    sleep(1)
    
    # Do conditional ShutDown
    if halt: # Set by call of 'stop()' in Idle
        print("Program Threads Halted by Idle call of 'stop()'")
        print("Running Tasks:",tasks)
    else: # Otherwise do ShutDown with options
        shut_down()




# Threads START-UP
# ==============
def startup():
    """ Starts the two PROGRAM TASKS"""
    global halt, hold_train, hold_all_trains
    halt = False; hold_train = False; hold_all_trains = False
    lcd_init()
    if RunMode(): # Checks Stop SWitch on Bottom of Panel
        title(issue)
        if check_i2c_devices():
            initialise()
            Thread(target = display_sensors).start()
            sleep(0.5)
            Thread(target = control).start()
    else:
        shut_down()


# End of Start_Up -------------------------


# Script starts itself
# ====================
startup()