import serial
import time
import threading
import websocket
import time
import queue

#### Load gcode testfile

#f = open("circlesss.gcode", "r")
#gcode = f.readlines()
#print(gcode)



#### Serial treamer 

def stream_gcode_serial(gcode):
  # Open grbl serial port
  s = serial.Serial('/dev/ttyACM0',115200)


  # Wake up grbl
  s.write("\r\n\r\n".encode('utf-8'))
  time.sleep(2)   # Wait for grbl to initialize 

  s.flushInput()  # Flush startup text in serial inp

  for line in gcode:
      l = str(line.strip()) # Strip all EOL characters for consistency
    
    
      print('sending '+ l)
      s.write((l + '\n').encode('utf-8')) # Send g-code block to grbl
	
      grbl_out = s.readline() # Wait for grbl response with carriage return
      print( ' : ' + str(grbl_out.strip()))


  # Close file and serial port
 
  s.close()  
  
  print('job done')




#### Web Socket Streamer

# Global variables
ws = None
response_queue = queue.Queue()  # Thread-safe queue for server responses

def receiver():
    """Receives messages from the WebSocket server."""
    while True:
        try:
            # Receive a message from the WebSocket
            for l in ws.recv().splitlines():
                if isinstance(l, str):
                    response = l
                else:
                    response = str(l, 'utf-8')
                # print("Received:", response)
                response_queue.put(response)  # Add response to the queue
        except Exception as e:
            print("Error in receiver:", e)
            break

def stream_gcode_websocket(gcode):
    """Streams G-code commands to the WebSocket server."""
    global ws
    ws = websocket.WebSocket()
    ws.connect("ws://192.168.0.1:81")  # Replace with your server's address

    # Start the receiver thread
    t = threading.Thread(target=receiver, daemon=True)
    t.start()

    # Wake up GRBL
    ws.send("\r\n\r\n")
    time.sleep(2)  # Wait for GRBL to initialize
    with response_queue.mutex:
      response_queue.queue.clear()

    for line in gcode:
        l = str(line.strip())  # Strip all EOL characters for consistency
        print('Sending:', l)
        ws.send(l + '\n')  # Send G-code block to the server

        # Wait for server response
        try:
            grbl_out = response_queue.get(timeout=5)  # Wait up to 5 seconds for a response
            print('Response:', grbl_out.strip())
            if(grbl_out.strip() != "ok"):
                break
        except queue.Empty:
            print("Timeout waiting for server response.")

    print('Job done')
    ws.close()

# gcode = [
#     "G0 X10 Y10",
#     "G1 X20 Y20",
#     "G1 X30 Y10"
# ]

# stream_gcode_websocket(gcode)