#!/usr/bin/env python3

from alive_progress import alive_bar
import get_specs
import traceback
#import logging
import yaml
from multiprocessing import Process, Manager, Pool, TimeoutError, active_children, log_to_stderr, Pipe, Queue
from multiprocessing.pool import Pool
import multiprocessing
from time import sleep
from util import fprint
from util import run_cmd
import sys
import ur5_control
from ur5_control import Rob
import os
import signal
import socket
from flask import Flask, render_template, request
import requests
from led_control import LEDSystem
import server
import asyncio
import json
import process_video
import search
from search import JukeboxSearch
#multiprocessing.set_start_method('spawn', True)
from pyModbusTCP.client import ModbusClient
from uptime import uptime
import fileserver

# set to false to run without real hardware for development
real = True
skip_scanning = True

mbconn = None
config = None
keeprunning = True
arm_ready = False
led_ready = False
camera_ready = False
sensor_ready = False
vm_ready = False
cable_search_ready = False
killme = None
#pool = None
serverproc = None
camera = None
ledsys = None
arm = None
to_server_queue = Queue()
from_server_queue = Queue()
mainloop_get = Queue()
mode = "Startup"
oldmode = "Startup"
counter = 0
jbs = None
scan_value = None
arm_state = None
cable_list = list()
parse_res = None
cable_list_state = list()
just_placed = -1
ring_animation = None
led_set_mode = None
sensors = [0,0,0,0]
websocket_process = None
arm_updates = None
animation_wait = False
arm_position = (0,0,0,0,0,0)
arm_position_process = None
start_animation = False
failcount = 0
timecount = 0

def arm_start_callback(res):
    fprint("Arm action complete.")
    global arm_ready
    arm_ready = True
    

def led_start_callback(res):
    global led_ready
    led_ready = True
    global ledsys
    ledsys = res

def camera_start_callback(res):
    global camera_ready
    camera_ready = True
    global scan_value
    scan_value = res
    
def sensor_start_callback(res):
    global sensor_ready
    sensor_ready = True

def vm_start_callback(res):
    global vm_ready
    vm_ready = True

def cable_search_callback(res):
    global cable_search_ready
    cable_search_ready = True
    global parse_res 
    parse_res = res

def wait_for(val, name):
    #global val
    if val is False:
        fprint("waiting for " + name + " to complete...")
        while val is False:
            sleep(0.1)

def send_data(type, call, data, client_id="*"):
    out = dict()
    out["type"] = type
    out["call"] = call
    out["data"] = data
    to_server_queue.put((client_id, json.dumps(out)))    

def check_server():
        #print("HI")
    global cable_list
    global to_server_queue
    global from_server_queue
    global jbs
    if True:
        # Handeling Server Requests Loop, will run forever

        if not from_server_queue.empty():
            client_id, message = from_server_queue.get()
            fprint(f"Message from client {client_id}: {message}")

            # Message handler
            try:
                decoded = json.loads(message)
            except:
                fprint("Non-JSON message recieved")
                return

            if "type" not in decoded:
                fprint("Missing \"type\" field.")
                return
            if "call" not in decoded:
                fprint("Missing \"call\" field.")
                return
            if "data" not in decoded:
                fprint("Missing \"data\" field.")
                return
            # if we get here, we have a "valid" data packet
            data = decoded["data"]
            call = decoded["call"]
            try:
                match decoded["type"]:
                    case "log":
                        fprint("log message")
                        if call == "send":
                            fprint("webapp: " + str(data), sendqueue=to_server_queue)
                        elif call == "request":
                            pass
                    case "cable_map":
                        fprint("cable_map message")
                        if call == "send":
                            pass
                        elif call == "request":
                            tmp = list()
                            for idx in range(len(cable_list)):
                                if cable_list[idx] is not False:
                                    cabledata = jbs.get_position(str(idx))
                                    fs = cabledata["fullspecs"]
                                    tmp1 = {"part_number": cable_list[idx], "position": idx, "name": cable_list[idx], "brand": cabledata["brand"] }
                                    if "Product Overview" in fs and "Product Category" in fs["Product Overview"]:
                                        tmp1["category"] = fs["Product Overview"]["Product Category"]
                                    if "Product Overview" in fs and "Suitable Applications" in fs["Product Overview"]:
                                        if len(fs["Product Overview"]["Suitable Applications"]) == 0 or not isinstance(fs["Product Overview"]["Suitable Applications"], str):
                                            for key, value in fs["Product Overview"].items():
                                                #print(key,value)
                                                if len(value) > 5 and isinstance(value, str):
                                                    tmp1["application"] = value
                                                elif len(key) > 15 and not isinstance(value, str):
                                                    tmp1["application"] = key
                                        else:
                                            tmp1["application"] = fs["Product Overview"]["Suitable Applications"]
                                    elif "Product Overview" in fs and "Suitable Applications:" in fs["Product Overview"]:
                                        if len(fs["Product Overview"]["Suitable Applications:"]) == 0 or not isinstance(fs["Product Overview"]["Suitable Applications:"], str):
                                            for key, value in fs["Product Overview"].items():
                                                #print(key,value)
                                                if len(value) > 5 and isinstance(value, str):
                                                    tmp1["application"] = value
                                                elif len(key) > 15 and not isinstance(value, str):
                                                    tmp1["application"] = key
                                        else:
                                            tmp1["application"] = fs["Product Overview"]["Suitable Applications:"]

                                    if "image" in cabledata:
                                        tmp1["image"] = cabledata["image"]
                                    if "datasheet" in cabledata:
                                        tmp1["datasheet"] = cabledata["datasheet"]
                                    if "description" in cabledata:
                                        tmp1["description"] = cabledata["description"]
                                    if "short_description" in cabledata:
                                        tmp1["short_description"] = cabledata["short_description"]
                                    if "application" in cabledata:
                                        tmp1["application"] = cabledata["application"]
                                    if "category" in cabledata:
                                        tmp1["category"] = cabledata["category"]

                                    tmp.append(tmp1)
                            out = {"map": tmp}
                            fprint(out)
                            send_data(decoded["type"], "send", out, client_id)

                    case "ping":
                        fprint("Pong!!!")
                    # Lucas' notes
                    # Add a ping pong :) response/handler
                    # Add a get cable response/handler
                    #       this will tell the robot arm to move
                    # Call for turning off everything
                    # TODO Helper for converting Python Dictionaries to JSON
                    # make function: pythonData --> { { "type": "...", "call": "...", "data": pythonData } }

                    # to send: to_server_queue.put(("*", "JSON STRING HERE")) # replace * with UUID of client to send to one specific location

                    case "cable_details":
                        fprint("cable_details message")
                        if call == "send":
                            pass
                        elif call == "request":
                            dataout = dict()
                            dataout["cables"] = list()
                            print(data)
                            if "part_number" in data:
                                for part in data["part_number"]:
                                    #print(part)
                                    #print(jbs.get_partnum(part))
                                    dataout["cables"].append(jbs.get_partnum(part)["fullspecs"])
                            if "position" in data:
                                for pos in data["position"]:
                                    #print(pos)
                                    #print(jbs.get_position(str(pos)))
                                    dataout["cables"].append(jbs.get_position(str(pos))["fullspecs"])
                            send_data(decoded["type"], "send", dataout, client_id)

                    case "cable_search":
                        fprint("cable_search message")
                        if call == "send":
                            pass
                        elif call == "request":
                            results = jbs.search(data["string"])["hits"]
                            dataout = dict()
                            dataout["map"] = list()
                            for cabledata in results:

                                fs = cabledata["fullspecs"]
                                tmp1 = {"part_number": cabledata["partnum"], "position": cabledata["position"], "name": cabledata["partnum"], "brand": cabledata["brand"] }
                                if "Product Overview" in fs and "Product Category" in fs["Product Overview"]:
                                    tmp1["category"] = fs["Product Overview"]["Product Category"]
                                if "Product Overview" in fs and "Suitable Applications" in fs["Product Overview"]:
                                    if len(fs["Product Overview"]["Suitable Applications"]) == 0 or not isinstance(fs["Product Overview"]["Suitable Applications"], str):
                                        for key, value in fs["Product Overview"].items():
                                            #print(key,value)
                                            if len(value) > 5 and isinstance(value, str):
                                                tmp1["application"] = value
                                            elif len(key) > 15 and not isinstance(value, str):
                                                tmp1["application"] = key
                                    else:
                                        tmp1["application"] = fs["Product Overview"]["Suitable Applications"]
                                elif "Product Overview" in fs and "Suitable Applications:" in fs["Product Overview"]:
                                    if len(fs["Product Overview"]["Suitable Applications:"]) == 0 or not isinstance(fs["Product Overview"]["Suitable Applications:"], str):
                                        for key, value in fs["Product Overview"].items():
                                            #print(key,value)
                                            if len(value) > 5 and isinstance(value, str):
                                                tmp1["application"] = value
                                            elif len(key) > 15 and not isinstance(value, str):
                                                tmp1["application"] = key
                                    else:
                                        tmp1["application"] = fs["Product Overview"]["Suitable Applications:"]
                                if "image" in cabledata:
                                    tmp1["image"] = cabledata["image"]
                                if "datasheet" in cabledata:
                                    tmp1["datasheet"] = cabledata["datasheet"]
                                if "description" in cabledata:
                                    tmp1["description"] = cabledata["description"]
                                if "short_description" in cabledata:
                                    tmp1["short_description"] = cabledata["short_description"]
                                if "application" in cabledata:
                                    tmp1["application"] = cabledata["application"]
                                if "category" in cabledata:
                                    tmp1["category"] = cabledata["category"]
                                
                                dataout["map"].append(tmp1)
                            
                            # after loop
                            send_data(decoded["type"], "send", dataout, client_id)

                    case "keyboard":
                        fprint("keyboard message")
                        if call == "send":
                            pass
                        elif call == "request":
                            if data["enabled"] == True:
                                # todo : send this to client
                                p = Process(target=run_cmd, args=("./keyboard-up.ps1",))
                                p.start()
                            elif data["enabled"] == False:
                                p = Process(target=run_cmd, args=("./keyboard-down.ps1",))
                                p.start()
                    case "machine_settings":
                        fprint("machine_settings message")
                        if call == "send":
                            pass
                        elif call == "request":
                            pass

                    case "cable_get":
                        fprint("cable_get message")
                        if call == "send":
                            global mainloop_get
                            
                            if "part_number" in data:
                                for cableidx in range(len(cable_list)):
                                    cable = cable_list[cableidx]
                                    if cable == data["part_number"]:
                                        fprint("Adding cable to dispense queue")
                                        mainloop_get.put(("pickup", cableidx))
                            elif "position" in data:
                                fprint("Adding cable to dispense queue")
                                mainloop_get.put(("pickup", data["position"]))
                            elif "tray" in data:
                                fprint("Adding tray return to dispense queue")
                                mainloop_get.put(("return", data["tray"]))
                            else:
                                fprint("Invalid data.")
                    
                    case _:
                        fprint("Unknown/unimplemented data type: " + decoded["type"])
            except Exception as e:
                fprint(traceback.format_exc())
                fprint(e)

            


        #sleep(0.001)  # Sleep to prevent tight loop
    


def start_client_socket():
    app = Flask(__name__)

    @app.route('/control_client', methods=['POST'])
    def message_from_server():
        # Handle message from server
        data = request.json
        fprint(f"Message from server: {data.get('message')}")
        return "Message received", 200
    
    app.run(host='0.0.0.0', port=6000)


def check_server_online(serverip, clientip):
    def send_ip_to_server(server_url, client_ip):
        try:
            response = requests.post(server_url, json={'ip': client_ip}, timeout=1)
            fprint(f"Server response: {response.text}")
            return True
        except requests.exceptions.RequestException as e:
            fprint(f"Error sending IP to server: {e}")
            return False
        
    server_url = 'http://' + serverip + ':5000/report_ip'
    while not send_ip_to_server(server_url, clientip):
        sleep(1)

    fprint("Successfully connected to server.")
    return True
    
        
def setup_server(pool, manager):
    # linux server setup
    global config
    global counter
    global sensor_ready
    global camera_ready
    global led_ready
    global arm_ready
    global serverproc
    global camera
    global arm
    global jbs
    global to_server_queue
    global from_server_queue
    global websocket_process
    global arm_updates
    global arm_position_process
    global ledsys
    arm_updates = manager.Queue()
    arm = Rob(config)
    if real:
        pool.apply_async(ur5_control.powerup_arm, (arm,), callback=arm_start_callback, error_callback=handle_error)
    else:
        arm_ready = True
    
    if real:
        ledsys = LEDSystem()
    #pool.apply_async(ledsys.init, callback=led_start_callback)
    #pool.apply_async(sensor_control.init, callback=sensor_start_callback)
    jbs = JukeboxSearch()
    
    
    
    if led_ready is False:
        fprint("waiting for " + "LED controller initialization" + " to complete...", sendqueue=to_server_queue)
        if real:
            ledsys.init()
        led_ready = True
    fprint("LED controllers initialized.", sendqueue=to_server_queue)

    if sensor_ready is False:
        fprint("waiting for " + "Sensor Initialization" + " to complete...", sendqueue=to_server_queue)
        global mbconn
        mbconn = ModbusClient(host="192.168.1.20", port=502, auto_open=False, auto_close=False)
        get_sensors()
    fprint("Sensors initialized.", sendqueue=to_server_queue)

    if camera_ready is False:
        fprint("waiting for " + "Camera initilization" + " to complete...", sendqueue=to_server_queue)
        camera = process_video.qr_reader(config["cameras"]["banner"]["ip"], int(config["cameras"]["banner"]["port"]))

    fprint("Camera initialized.", sendqueue=to_server_queue)

    #arm_ready = True
    if arm_ready is False:
        fprint("waiting for " + "UR5 powerup" + " to complete...", sendqueue=to_server_queue)
        while arm_ready is False:
            sleep(0.1)

    if real:
        ur5_control.init_arm(arm)
    fprint("Arm initialized.", sendqueue=to_server_queue)

    fprint("Creating arm position watcher...")
    arm_position_process = pool.apply_async(ur5_control.get_position_thread, (arm,arm_updates))

    fprint("Starting websocket server...", sendqueue=to_server_queue)
    websocket_process = server.start_websocket_server(to_server_queue, from_server_queue)
    
    fprint("Starting image file server...", sendqueue=to_server_queue)
    image_server_process = Process(target=fileserver.run_server, args=(config["cables"]["port"], config["cables"]["directory"]))
    image_server_process.start()
    return True

def handle_error(error):
	print(error, flush=True)

def get_sensors():
    global mbconn
    global sensors
    #print("Reading sensors")
    if not mbconn.is_open:
        mbconn.open()
    """
    port 1: 256
    port 2: 272
    port 3: 288
    port 4: 304
    port 5: 320
    port 6: 336
    port 7: 352
    port 8: 368
    
    """
    if real:
        sens = [352, 288, 304, 368]
        for idx in range(len(sens)):
            reg = sens[idx]
            val = mbconn.read_holding_registers(reg)
            if val is not None:
                val = val[0]
                if val == 1 and sensors[idx] >= 0: # skip negative values
                    sensors[idx] += 1
                elif val == 0:
                    if sensors[idx] >= 4:
                        sensors[idx] -= 4
                    else:
                        sensors[idx] += 4
    else:
        sensors = [0, 0, 0, 0]

    #fprint("Values: " + str(sensors))
    #mbconn.close()
    for x in range(len(sensors)):
        if sensors[x] >= 180: # 3 sec
            # cable newly detected on tray
            sensors[x] = -180
            fprint("Precense detected: slot " + str(x))
            return x
        
    return -1
def get_open_spot(sensordata):
    for x in range(len(sensordata)):
        sens = sensordata[x]
        if not sens:
            return x
        
    # if we get here, every spot is full
        
    return False
    

def mainloop_server(pool, manager):
    # NON-blocking loop
    global real
    global ring_animation
    global led_set_mode
    global just_placed
    global config
    global counter
    global killme
    global mode
    global jbs
    global arm
    global ledsys
    global camera
    global arm_ready
    global arm_state
    global camera_ready
    global cable_search_ready
    global cable_list
    global mainloop_get
    global cable_list_state
    global scan_value
    global oldmode
    global arm_updates
    global animation_wait
    global arm_position
    global arm_position_process
    global start_animation
    global failcount
    global timecount

    if mode != oldmode:
        print(" ***** Running mode:", mode, "***** ")
        oldmode = mode
        if mode == "Startup": # very first loop
            pass

    if killme.value > 0:
        killall()

    # check for messages
    check_server()

    # do every loop!
    checkpoint = None
    if not arm_updates.empty():
        val = arm_updates.get()
        
        if isinstance(val, tuple):
            arm_position = val
        else:
            print("Arm queue message " + str(val))
            checkpoint = val
            print(ring_animation, animation_wait, ledsys.mode, arm_position)
            

    if start_animation:
        # animation start requested
        # may not be immediate
        if ring_animation is not None:
            if animation_wait:
                # wait for checkpoint
                if checkpoint is not None:
                    fprint("Starting checkpointed animation " + str(led_set_mode) + " for ring " + str(ring_animation))
                    ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
                    led_set_mode = None
                    animation_wait = False
                    start_animation = False

                else:
                    # still waiting
                    ledsys.mainloop(None, ring_animation, arm_position=arm_position)

            else:
                # no waiting, just start
                fprint("Starting immediate animation " + str(led_set_mode) + " for ring " + str(ring_animation))
                ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
                led_set_mode = None
                animation_wait = False
                start_animation = False
        else:
            # no ring animation specified
            pass
    
    else:
        # no new animation
        if ring_animation is not None:
            ledsys.mainloop(None, ring_animation, arm_position=arm_position)
            
        else:
            pass

    # if start_animation is False and ring_animation is not None and ledsys.mode != "Idle" and real:
    #     ledsys.mainloop(None, ring_animation, arm_position=arm_position)

    # elif start_animation is True and ring_animation is not None and real:
    #     if animation_wait:
    #         if checkpoint is not None: # got to checkpoint from UR5
    #             fprint("Starting checkpointed animation " + str(led_set_mode) + " for ring " + str(ring_animation))
    #             ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
    #             led_set_mode = None
    #             animation_wait = False
    #             start_animation = False
    #     else:
    #         fprint("Starting immediate animation " + str(led_set_mode) + " for ring " + str(ring_animation))
    #         ledsys.mainloop(led_set_mode, ring_animation, arm_position=arm_position)
    #         led_set_mode = None
    #         start_animation = False
    # else:
    #     ledsys.mainloop(None, 49, arm_position=arm_position)
    #     pass
    #     #fprint("Not triggering LED loop: no ring animation")

    if mode == "Startup":
        if not real or skip_scanning:
            counter = 54
            
        if counter < 54:
            # scanning cables
            if arm_state is None:
                #pool.apply_async(arm_start_callback, ("",))
                arm_ready = False
                pool.apply_async(ur5_control.holder_to_camera, (arm,arm_updates,counter), callback=arm_start_callback, error_callback=handle_error)
                fprint("Getting cable index " + str(counter) + " and scanning...")
                arm_state = "GET"
                ring_animation = counter
                animation_wait = True
                start_animation = True
                led_set_mode = "GrabA"
                #ur5_control.to_camera(arm, counter)
                #arm_ready = True

            elif arm_ready and arm_state == "GET":
                fprint("Looking for QR code...")
                pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
                arm_ready = False

            elif camera_ready:
                ring_animation = counter
                animation_wait = True
                start_animation = True
                led_set_mode = "GrabC"
                fprint("Adding cable to list...")
                global scan_value
                if scan_value is False:
                    cable_list.append(scan_value)
                elif scan_value.find("bldn.app/") > -1:
                    scan_value = scan_value[scan_value.find("bldn.app/")+9:]
                else:
                    cable_list.append(scan_value)
                fprint(scan_value)
                pool.apply_async(ur5_control.camera_to_holder, (arm,arm_updates,counter), callback=arm_start_callback, error_callback=handle_error)
                #ur5_control.return_camera(arm, counter)
                #arm_ready = True
                arm_state = "RETURN"
                camera_ready = False

            elif arm_ready and arm_state == "RETURN":
                counter += 1
                arm_state = None
            else:
                # just wait til arm/camera is ready
                pass
        else:
            # scanned everything
            ring_animation = None
            led_set_mode == "Idle"
            start_animation = True
            tmp = [
                    # Actual cables in Jukebox
                    "BLTF-1LF-006-RS5",
                    "BLTF-SD9-006-RI5",
                    "BLTT-SLG-024-HTN",
                    "BLFISX012W0",
                    "BLFI4X012W0",
                    "BLSPE101",
                    "BLSPE102",
                    "BL7922A",
                    "BL7958A",
                    "BLIOP6U",
                    "BL10GXW13",
                    "BL10GXW53",
                    "BL29501F",
                    "BL29512",
                    "BL3106A",
                    "BL9841",
                    "BL3105A",
                    "BL3092A",
                    "BL8760",
                    "BL6300UE",
                    "BL6300FE",
                    "BLRA500P",
                    "AW86104CY",
                    "AW3050",
                    "AW6714",
                    "AW1172C",
                    "AWFIT-221-1_4"
                ]
            while len(tmp) < 54:
                tmp.append(False) # must have 54 entries

            if not real or skip_scanning:
                cable_list = tmp # comment out for real demo

            for idx in range(len(cable_list)):
                cable_list_state.append(True)

            pool.apply_async(get_specs.get_multi, (cable_list, 0.3, config["cables"]["directory"], config["cables"]["port"]), callback=cable_search_callback, error_callback=handle_error)
            mode = "Parsing"
            fprint("All cables scanned. Finding & parsing datasheets...")
    if mode == "Parsing":
            # waiting for search & parse to complete
            #cable_search_ready = True
            if cable_search_ready is False:
                pass
            else:
                # done
                global parse_res
                success, partnums = parse_res
                for idx in range(len(partnums)):
                    if partnums[idx] is not False:
                        cable_list[idx] = partnums[idx][0].replace("/", "_")
                    else:
                        cable_list[idx] = False
                
                print(partnums)
                if success:
                    # easy mode
                    fprint("All cables inventoried and parsed.")
                    fprint("Adding to database...")
                    for idx in range(len(cable_list)):
                        partnum = cable_list[idx]
                        if partnum is not False:
                            with open("cables/" + partnum + "/search.json", "rb") as f:
                                searchdata = json.load(f)
                                searchdata["position"] = idx
                            with open("cables/" + partnum + "/specs.json", "rb") as f:
                                specs = json.load(f)
                            searchdata["fullspecs"] = specs
                            searchdata["fullspecs"]["position"] = idx
                            jbs.add_document(searchdata)
                    #sleep(0.5)
                    #print(jbs.get_position("1"))
                    
                    fprint("All cables added to database.")
                    mode = "Idle"
                    
                else:
                    # TODO: manual input
                    pass

            
    if mode == "Idle":
        # do nothing
        if arm_ready is False:
            pass

        else:
            global mainloop_get
            #print("Checking sensors..")
            newtube = get_sensors()
            if newtube >= 0:
                # need to return a cable
                mainloop_get.put(("return", newtube))
            
            if not mainloop_get.empty():
                fprint("Movement requested. Keep clear of the machine!")
                action, get_cable = mainloop_get.get()
                if get_cable > -1:
                    global sensors
                    if action == "pickup":
                        spot = get_open_spot(sensors)
                        if spot is not False:
                            arm_ready = False
                            if real:
                                animation_wait = False
                                ring_animation = get_cable
                                start_animation = True
                                led_set_mode = "GrabAA"
                                pool.apply_async(ur5_control.holder_to_tray, (arm, arm_updates, get_cable, spot), callback=arm_start_callback, error_callback=handle_error)
                            else:
                                arm_ready = True
                            fprint("Getting cable at position " + str(get_cable))
                            mode = "Pickup"
                            cable_list_state[get_cable] = False # mark as removed
                            

                    if action == "return":
                        arm_ready = False
                        fprint("Returning cable from tray position " + str(get_cable))
                        if real:
                            failcount = 0
                            pool.apply_async(ur5_control.tray_to_camera, (arm, arm_updates, get_cable), callback=arm_start_callback, error_callback=handle_error)
                        else:
                            arm_ready = True
                        mode = "ReturnC"
            else:
                # LED idle anim
                pass

    if mode == "Pickup":
        # complete
        if arm_ready == True:
            mode = "Idle"
            
        else:
            # getting cable and bringing to tray
            # led animation
            if ledsys.mode == "Idle" and led_set_mode != "GrabAA":
                animation_wait = True
                start_animation = True
                led_set_mode = "GrabAB"
            pass
    
    if mode == "ReturnC":
        # complete
        if arm_ready == True:
            mode = "Scan"
            arm_ready = False
            camera_ready = False
            if real:
                animation_wait = False
                start_animation = True
                ring_animation = 49
                led_set_mode = "Camera"
                timecount = 0
                pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)
            else:
                camera_ready = True
                scan_value = "10GXS13"
            
        else:
            # getting cable from and bringing to camera
            # led animation
            pass

    if mode == "Scan":
        if camera_ready == True:
            if scan_value is False:
                # unable to scan ???? not good
                if failcount > 30:
                    mode = "Idle"
                    fprint("Giving up scanning cable.")
                    failcount = 0
                    timecount = 0
                    arm_ready = True
                else:
                    fprint("Unable to scan cable. Gonna retry.")
                    camera_ready = False
                    #mode = "Idle"
                    failcount += 1
                    timecount = 0
                    pool.apply_async(camera.read_qr, (10,), callback=camera_start_callback, error_callback=handle_error)

            elif scan_value.find("bldn.app/") > -1:
                scan_value = scan_value[scan_value.find("bldn.app/")+9:]
            
                fprint("Got cable: " + str(scan_value))
                if scan_value[0:2] == "BL" or scan_value[0:2] == "AW":
                    scan_value = scan_value[2:]
                for idx in range(len(cable_list)):
                    cable = cable_list[idx]
                    if cable == scan_value and cable_list_state[idx] == False:
                        cable_list_state[idx] = True # mark cable as returned
                        arm_ready = False
                        if real:
                            animation_wait = True
                            ring_animation = idx
                            led_set_mode = "GrabC"
                            start_animation = True
                            pool.apply_async(ur5_control.camera_to_holder, (arm, arm_updates, idx), callback=arm_start_callback, error_callback=handle_error)
                        else:
                            arm_ready = True
                        mode = "Return"
                        break
                    elif cable == scan_value and cable_list_state[idx] == True:
                        fprint("WARNING: Holder still marked as occupied!")
                        arm_ready = False
                        if real:
                            animation_wait = True
                            ring_animation = idx
                            led_set_mode = "GrabC"
                            start_animation = True
                            pool.apply_async(ur5_control.camera_to_holder, (arm, arm_updates, idx), callback=arm_start_callback, error_callback=handle_error)
                        else:
                            arm_ready = True
                        mode = "Return"
                        break
                if mode == "Scan":
                    mode = "Idle"
        else: # camera not ready
            timecount += 1
            if timecount > 180:
                print("Camera timeout reached.")
                timecount = 0
                camera_ready = True
                arm_ready = True
                mode = "Idle"
            

    if mode == "Return":
        if arm_ready == True:
            mode = "Idle"
            #arm_ready = False
            # movement finished

        else: 
            # cable going from camera to holder
            # led animation
            pass
            
    
        


def run_loading_app():
    
    app = Flask(__name__)
    @app.route('/')
    def index():
        return render_template('index.html')
    
    app.run(debug=True, use_reloader=False, port=7000)

def setup_client(pool):
    global config
    global vm_ready
    global serverproc
    if config["core"]["server"] == "Hyper-V":
        run_cmd("Start-VM -Name Jukebox*") # any and all VMs starting with "Jukebox"
    # Windows client setup
    fprint("Opening browser...")
    
    firefox = webdriver.Firefox()
    firefox.fullscreen_window()
    
    # Open loading wepage
    p = Process(target=run_loading_app)
    p.start()

    
    firefox.get('http://localhost:7000')
    
    # start Linux server VM
    

    sleep(35)
    p.terminate()
    firefox.get('http://192.168.1.25:3000')
    # import time
    # while True:
    #     time.sleep(2)  # Wait for a given interval
    #     logs = firefox.get_log('browser')
    #     for entry in logs:
    #         if "WebSocket connection" in entry['message'] or "ERR_" in entry['message'] or "Failed to connect" in entry['message'] or "failed to connect" in entry['message']:
    #             print(f"Error detected in console: {entry['message']}")
    #             firefox.refresh()  # Refresh the page on error
    #         # else:
    #         #     break  # Exit the loop or continue depending on your logic

    return True
    

def mainloop_client(pool):
    sleep(0.1)

    # listen for & act on commands from VM, if needed
    # mainly just shut down, possibly connect to wifi or something

"""class Logger(object):
    def __init__(self, filename="output.log"):
        self.log = open(filename, "a")
        self.terminal = sys.stdout

    def write(self, message):
        self.log.write(message)
        #close(filename)
        #self.log = open(filename, "a")
        try:
            self.terminal.write(message)
        except:
            sleep(0)
        
    def flush(self):
        print("",end="")"""

def killall():
    procs = active_children()
    for proc in procs:
        proc.kill()
    fprint("All child processes killed")
    os.kill(os.getpid(), 9) # dirty kill of self

    
def killall_signal(a, b):
    global config
    if config["core"]["server"] == "Hyper-V":
        run_cmd("Stop-VM -Name Jukebox*") # any and all VMs starting with "Jukebox"
    killall()

def error(msg, *args):
    return multiprocessing.get_logger().error(msg, *args)

class LogExceptions(object):
    def __init__(self, callable):
        self.__callable = callable

    def __call__(self, *args, **kwargs):
        try:
            result = self.__callable(*args, **kwargs)

        except Exception as e:
            # Here we add some debugging help. If multiprocessing's
            # debugging is on, it will arrange to log the traceback
            error(traceback.format_exc())
            # Re-raise the original exception so the Pool worker can
            # clean up
            raise

        # It was fine, give a normal answer
        return result

class LoggingPool(Pool):
    def apply_async(self, func, args=(), kwds={}, callback=None, error_callback=None):
        return Pool.apply_async(self, LogExceptions(func), args, kwds, callback, error_callback)


if __name__ == "__main__":
    #sys.stdout = Logger(filename="output.log")
    #sys.stderr = Logger(filename="output.log")
    #log_to_stderr(logging.DEBUG)
    
    fprint("Starting Jukebox control system...")
    with open('config.yml', 'r') as fileread:
        #global config
        config = yaml.safe_load(fileread)
    fprint("Config loaded.")
    with Manager() as manager:
        fprint("Spawning threads...")
        pool = LoggingPool(processes=10)
        counter = 0
        killme = manager.Value('d', 0)
        signal.signal(signal.SIGINT, killall_signal)
        if config["core"]["mode"] == "winclient":
            fprint("Starting in client mode.")
            from selenium import webdriver
            if setup_client(pool):
                fprint("Entering main loop...")
                while(keeprunning):
                    mainloop_client(pool)

        elif config["core"]["mode"] == "linuxserver":
            fprint("Starting in server mode.")
            if setup_server(pool, manager):
                fprint("Entering main loop...")
                start = 0
                speed = config["core"]["loopspeed"]
                while(keeprunning):
                    start = uptime()
                    mainloop_server(pool, manager)
                    #sleep(0.01)
                    # limit to certain "framerate"
                    #print(start, start + 1.0/speed, uptime())
                    while start + 1.0/speed > uptime():
                        sleep(0.001)
        else:
            fprint("Mode unspecified - quitting")
            
            


