#!/usr/bin/env python3

import sys
import getopt
import glob
import serial
import struct
import tkinter

from matplotlib.backends.backend_tkagg import (FigureCanvasTkAgg, NavigationToolbar2Tk)
# Implement the default Matplotlib key bindings.
from matplotlib.backend_bases import key_press_handler
from matplotlib.figure import Figure

import numpy as np

tempe = 0

tSetGlobal = 0.0

controllerDebugWindowOpen = 0

tick = 0
tickArray = []

tActArray = []
tSetArray = []

tickDebug = 0
tickDebugArray = []

tActDebugArray = []
tSetDebugArray = []
errDebugArray = []
errSumDebugArray = []
outOrigDebugArray = []
outDebugArray = []
outAvgDebugArray = []
sleepCounterDebugArray = []

rxInputBuffer = bytearray()
rxStatemachine = 0

def serial_ports():
  """ Lists serial port names

        :raises EnvironmentError:
            On unsupported or unknown platforms
        :returns:
            A list of the serial ports available on the system
  """
  if sys.platform.startswith('win'):
    ports = ['COM%s' % (i + 1) for i in range(256)]
  elif sys.platform.startswith('linux') or sys.platform.startswith('cygwin'):
    # this excludes your current terminal "/dev/tty"
    ports = glob.glob('/dev/tty[A-Za-z]*')
  elif sys.platform.startswith('darwin'):
    ports = glob.glob('/dev/tty.*')
  else:
    raise EnvironmentError('Unsupported platform')

  result = []
  for port in ports:
    try:
      s = serial.Serial(port)
      s.close()
      result.append(port)
    except (OSError, serial.SerialException):
      pass
  return result

#print(serial_ports())
availableSerialPorts = serial_ports()

print('choose a port:')
counter = 0
for port in availableSerialPorts:
  print('  [' + str(counter) + '] ' + port)
  counter += 1

choosenPort = input('(type a number (0)) ')

if choosenPort == '':
  choosenPortNumber = 0
else:
  choosenPortNumber = int(choosenPort)

counter = 0

#serialPort = '/dev/pts/19'
serialPort = '/dev/ttyACM0'

serialPort = availableSerialPorts[choosenPortNumber]

try:
  ser = serial.Serial(serialPort, 19200, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout=0, writeTimeout=0)
except:
  serialPort = '/dev/ttyACM1'
  ser = serial.Serial(serialPort, 19200, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout=0, writeTimeout=0)

def readSerial():
  while True:
    c = ser.read() # attempt to read a character from Serial
    
    #was anything read?
    if len(c) == 0:
      break
    else:
      #print('DATA')
      #log.insert(END, ':' + c.hex())
      
      global rxInputBuffer
      global rxStatemachine
      
      rxInputBuffer.append(c[0])
      if rxStatemachine == 0:
        if rxInputBuffer[0] == 0xff:
          rxStatemachine = 1
        rxInputBuffer = rxInputBuffer[1:]
      elif rxStatemachine == 1:
        if rxInputBuffer[0] == 0xf1:
          rxStatemachine = 2
        else:
          rxStatemachine = 0
        rxInputBuffer = rxInputBuffer[1:]
      elif rxStatemachine == 2:
        if len(rxInputBuffer) >= rxInputBuffer[0] + 1:
          processData(rxInputBuffer[1:rxInputBuffer[0] + 1])
          rxInputBuffer = bytearray()
          rxStatemachine = 0
        else:
          if len(rxInputBuffer) > 256:
            print('rxInputBuffer reset!')
            rxInputBuffer = bytearray()
            rxStatemachine = 0
  root.after(10, readSerial) # check serial again soon

def processData(input):
  global tSetGlobal
  
  print([ "0x%02x" % b for b in input ])
  # ~ print(str([hex(x) for x in input]))
  # ~ if input[0] == 0x11:
    # ~ if input[1] == 100:
      # ~ temperatureString.set(str(round(((input[2] * 256) + input[3]) * 0.4833149757 + 35.1689715661)) + "degC")
      # ~ processControllerDebugData(((input[2] * 256) + input[3]) * 0.4833149757 + 35.1689715661, 20, 10, 0, -10, -20)
  if input[0] == 0x33:
    print("debugDataEnabled")
    tickDebugArray.clear()
    tActDebugArray.clear()
    tSetDebugArray.clear()
    errDebugArray.clear()
    errSumDebugArray.clear()
    outOrigDebugArray.clear()
    outDebugArray.clear()
    outAvgDebugArray.clear()
    sleepCounterDebugArray.clear()
  elif input[0] == 0x34:
    print("debugDataDisabled")
  elif input[0] == 0x8d:
    cartridge_temperature_adc_raw = ((input[1] * 256) + input[2])
    tAct = struct.unpack('>f', input[3:7])[0]
    tSet = struct.unpack('>f', input[7:11])[0]
    err = struct.unpack('>f', input[11:15])[0]
    errSum = struct.unpack('>f', input[15:19])[0]
    outOrig = struct.unpack('>f', input[19:23])[0]
    out = input[23] * 256 + input[24]
    outAvg = input[25] * 256 + input[26]
    sleepCounter = input[27] * 256 + input[28]
    if out > 32767:
      out -= 65536
    tActString.set(str(round(tAct)) + "degC")
    tSetString.set("(" + str(round(tSet)) + "degC)")
    tSetGlobal = tSet
    processTemperatureData(tAct, tSet)
    processControllerDebugData(tAct, tSet, err, errSum, outOrig, out, outAvg, sleepCounter)
  elif input[0] == 0x8c:
    tAct = struct.unpack('>f', input[1:5])[0]
    tSet = struct.unpack('>f', input[5:9])[0]
    tActString.set(str(round(tAct)) + "degC")
    tSetString.set("(" + str(round(tSet)) + "degC)")
    tSetGlobal = tSet
    processTemperatureData(tAct, tSetGlobal)
  # ~ elif input[0] == 0x91:
    # ~ tSet = struct.unpack('>f', input[1:5])[0]
    # ~ tSetString.set("(" + str(round(tSet)) + "degC)")
  # ~ elif input[0] == 0x92:
    # ~ controllerValueKpString.set("Kp: " + str(struct.unpack('>f', input[1:5])[0]))
  # ~ elif input[0] == 0x93:
    # ~ controllerValueKpString.set("Kp: " + str(struct.unpack('>f', input[1:5])[0]))
  # ~ elif input[0] == 0x94:
    # ~ controllerValueKiString.set("Ki: " + str(struct.unpack('>f', input[1:5])[0]))
  # ~ elif input[0] == 0x95:
    # ~ controllerValueKiString.set("Ki: " + str(struct.unpack('>f', input[1:5])[0]))
  # ~ elif input[0] == 0x96:
    # ~ controllerValueKdString.set("Kd: " + str(struct.unpack('>f', input[1:5])[0]))
  # ~ elif input[0] == 0x97:
    # ~ controllerValueKdString.set("Kd: " + str(struct.unpack('>f', input[1:5])[0]))
  # ~ elif input[0] == 0x98:
    # ~ controllerValueKitString.set("Kit: " + str(struct.unpack('>f', input[1:5])[0]))
  # ~ elif input[0] == 0x99:
    # ~ controllerValueKitString.set("Kit: " + str(struct.unpack('>f', input[1:5])[0]))
  elif input[0] == 0xa0:
    controllerValueKpString.set("Kp: " + str(struct.unpack('>f', input[1:5])[0]))
  elif input[0] == 0xa1:
    controllerValueKiString.set("Ki: " + str(struct.unpack('>f', input[1:5])[0]))
  elif input[0] == 0xa2:
    controllerValueKdString.set("Kd: " + str(struct.unpack('>f', input[1:5])[0]))
  elif input[0] == 0xa3:
    controllerValueKitString.set("Kit: " + str(struct.unpack('>f', input[1:5])[0]))
  elif input[0] == 0xf0:
    tSet = struct.unpack('>f', input[1:5])[0]
    controllerKp = struct.unpack('>f', input[5:9])[0]
    controllerKi = struct.unpack('>f', input[9:13])[0]
    controllerKd = struct.unpack('>f', input[13:17])[0]
    controllerKit = struct.unpack('>f', input[17:21])[0]
    tSetString.set("(" + str(round(tSet)) + "degC)")
    tSetGlobal = tSet
    controllerValueKpString.set("Kp: " + str(controllerKp))
    controllerValueKiString.set("Ki: " + str(controllerKi))
    controllerValueKdString.set("Kd: " + str(controllerKd))
    controllerValueKitString.set("Kit: " + str(controllerKit))

def sendSerial(payloadType, payload):
  # ~ print('send ' + str(len(input)) + ' byte(s): ' + str([hex(x) for x in input]))
  data = bytearray([0xff, 0xf1])
  data.append(1 + len(payload))
  data.append(payloadType)
  data.extend(bytearray(payload))
  print("send " + str([ "0x%02x" % b for b in data ]))
  ser.write(data)

def createNewWindow():
  global controllerDebugWindowOpen
  
  newWindow = tkinter.Toplevel(root)
  newWindow.protocol("WM_DELETE_WINDOW", lambda: closeWindow(newWindow))
  # ~ labelExample = tkinter.Label(master=newWindow, text="new window")
  # ~ labelExample.pack()
  canvas2 = FigureCanvasTkAgg(controllerDebugFigure, master=newWindow)  # A tk.DrawingArea.
  canvas2.draw()
  canvas2.get_tk_widget().pack()
  controllerDebugWindowOpen = 1

def closeWindow(myVar):
  print("closeWindow " + str(myVar))
  controllerDebugWindowOpen = 0
  myVar.destroy()

def increaseTemperature():
  global tempe, temperatureString
  tempe = tempe + 10
  temperatureString.set(str(tempe) + "degC")

def processTemperatureData(tAct, tSet):
  global tick, tickArray, tActArray, tSetArray
  
  tickArray.append(tick)
  tickArray = tickArray[-30:]
  tick = tick + 1
  
  tActArray.append(tAct)
  tActArray = tActArray[-30:]
  tSetArray.append(tSet)
  tSetArray = tSetArray[-30:]
  
  yMin = tSet - 10
  yMax = tSet + 10
  
  if min(tActArray) < yMin:
    yMin = min(tActArray) - 2
  
  if max(tActArray) > yMax:
    yMax = max(tActArray) + 2
  
  temperatureFigureAxis1.clear()
  temperatureFigureAxis1.plot(tickArray, tActArray, linewidth=1, color="black")
  temperatureFigureAxis1.plot(tickArray, tSetArray, linewidth=1, color="green")
  temperatureFigureAxis1.set_ylim(yMin, yMax)
  # ~ temperatureFigure.tight_layout()
  temperatureFigure.canvas.draw()

def processControllerDebugData(tAct, tSet, err, errSum, outOrig, out, outAvg, sleepCounter):
  # ~ print("processControllerDebugData")
  global tickDebug, tickDebugArray, tActDebugArray, tSetDebugArray, errDebugArray, errSumDebugArray, outOrigDebugArray, outDebugArray, outAvgDebugArray, sleepCounterDebugArray
  global controllerDebugWindowOpen
  
  tickDebugArray.append(tickDebug)
  tickDebugArray = tickDebugArray[-1000:]
  tickDebug = tickDebug + 1
  
  tActDebugArray.append(tAct)
  tActDebugArray = tActDebugArray[-1000:]
  tSetDebugArray.append(tSet)
  tSetDebugArray = tSetDebugArray[-1000:]
  errDebugArray.append(err)
  errDebugArray = errDebugArray[-1000:]
  errSumDebugArray.append(errSum)
  errSumDebugArray = errSumDebugArray[-1000:]
  outOrigDebugArray.append(outOrig)
  outOrigDebugArray = outOrigDebugArray[-1000:]
  outDebugArray.append(out)
  outDebugArray = outDebugArray[-1000:]
  outAvgDebugArray.append(outAvg)
  outAvgDebugArray = outAvgDebugArray[-1000:]
  sleepCounterDebugArray.append(sleepCounter)
  sleepCounterDebugArray = sleepCounterDebugArray[-1000:]
  
  if controllerDebugWindowOpen == 1:
    # ~ ax1.clear()
    # ~ ax1.plot(tickArray, tActArray, linewidth=2, color='black')
    # ~ fig.canvas.draw()
    controllerDebugFigureAxis1.clear()
    controllerDebugFigureAxis1.plot(tickDebugArray, tActDebugArray, linewidth=1, color="black", label="tAct")
    controllerDebugFigureAxis1.plot(tickDebugArray, tSetDebugArray, linewidth=1, color="green", label="tSet")
    controllerDebugFigureAxis1.plot(tickDebugArray, errDebugArray, linewidth=1, color="red", label="err")
    controllerDebugFigureAxis1.plot(tickDebugArray, errSumDebugArray, linewidth=1, color="orange", label="errSum")
    controllerDebugFigureAxis1.plot(tickDebugArray, outOrigDebugArray, linewidth=1, color="royalblue", label="outOrig")
    controllerDebugFigureAxis1.plot(tickDebugArray, outDebugArray, linewidth=1, color="blueviolet", label="out")
    controllerDebugFigureAxis1.plot(tickDebugArray, outAvgDebugArray, linewidth=1, color="blue", label="outAvg")
    controllerDebugFigureAxis1.plot(tickDebugArray, sleepCounterDebugArray, linewidth=1, color="grey", label="sleepCounter")
    controllerDebugFigureAxis1.legend(loc="lower left")
    controllerDebugFigureAxis1.set_ylim(-100, 1000)
    # ~ controllerDebugFigure.tight_layout()
    controllerDebugFigure.canvas.draw()

root = tkinter.Tk()
root.wm_title("Embedding in Tk")
root.geometry("320x850")
# ~ root.protocol("WM_DELETE_WINDOW", lambda: closeWindow(root))

# ~ fig = Figure(figsize=(5, 4), dpi=100)
# ~ fig2 = Figure(figsize=(5, 4), dpi=100)
# ~ t = np.arange(0, 3, .01)
# ~ fig.add_subplot(111).plot(t, 2 * np.sin(2 * np.pi * t))
# ~ fig2.add_subplot(111).plot(t, 2 * np.cos(2 * np.pi * t))

temperatureFigure = Figure(figsize=(3, 2))
temperatureFigureAxis1 = temperatureFigure.add_subplot(1, 1, 1)
temperatureFigureAxis1.yaxis.set_label_position("right")
temperatureFigureAxis1.yaxis.tick_right()
temperatureFigure.tight_layout()

controllerDebugFigure = Figure(figsize=(15, 9))
controllerDebugFigureAxis1 = controllerDebugFigure.add_subplot(1, 1, 1)
controllerDebugFigureAxis1.yaxis.set_label_position("right")
controllerDebugFigureAxis1.yaxis.tick_right()
controllerDebugFigureAxis1.set_ylim(-200, 500)
controllerDebugFigure.tight_layout()

# ~ canvas = FigureCanvasTkAgg(fig, master=root)  # A tk.DrawingArea.
# ~ canvas.draw()

# pack_toolbar=False will make it easier to use a layout manager later on.
# ~ toolbar = NavigationToolbar2Tk(canvas, root, pack_toolbar=False)
# ~ toolbar.update()


# ~ canvas.mpl_connect("key_press_event", lambda event: print(f"you pressed {event.key}"))
# ~ canvas.mpl_connect("key_press_event", key_press_handler)

# ~ btn = tk.Button(root, text="Press", command=lambda: myFunction("See this worked!"))
# ~ button = tkinter.Button(master=root, text="Quit", command=root.quit)
buttonNewWindow = tkinter.Button(master=root, text="open controller debug window", command=createNewWindow)

# Packing order is important. Widgets are processed sequentially and if there
# is no space left, because the window is too small, they are not displayed.
# The canvas is rather flexible in its size, so we pack it last which makes
# sure the UI controls are displayed as long as possible.
# ~ button.pack(side=tkinter.BOTTOM)
buttonNewWindow.pack(side=tkinter.BOTTOM)
# ~ toolbar.pack(side=tkinter.BOTTOM, fill=tkinter.X)
# ~ canvas.get_tk_widget().pack(side=tkinter.TOP, fill=tkinter.BOTH, expand=1)

tkinter.Button(master=root, text="set 330degC", command=lambda: sendSerial(0x91, [0x01, 0x4a])).pack()
tkinter.Button(master=root, text="set 200degC", command=lambda: sendSerial(0x91, [0x00, 0xc8])).pack()
tkinter.Button(master=root, text="set 0degC", command=lambda: sendSerial(0x91, [0x00, 0x00])).pack()

tActString = tkinter.StringVar()
tActString.set("0degC")
tSetString = tkinter.StringVar()
tSetString.set("(0degC)")
controllerValueKpString = tkinter.StringVar()
controllerValueKpString.set("Kp: 0")
controllerValueKiString = tkinter.StringVar()
controllerValueKiString.set("Ki: 0")
controllerValueKdString = tkinter.StringVar()
controllerValueKdString.set("Kd: 0")
controllerValueKitString = tkinter.StringVar()
controllerValueKitString.set("Kit: 0")

tkinter.Label(master=root, textvariable=tActString, font=("Free Mono", 40)).pack()
tkinter.Label(master=root, textvariable=tSetString, font=("Free Mono", 20)).pack()

canvas = FigureCanvasTkAgg(temperatureFigure, master=root)  # A tk.DrawingArea.
canvas.draw()
canvas.get_tk_widget().pack()

# ~ tkinter.Button(master=root, text="Kp +1", command=lambda: sendSerial(0x92, [])).pack()
tkinter.Label(master=root, textvariable=controllerValueKpString, font=("Free Mono", 20)).pack()
textValueKp = tkinter.Text(master=root, height=1, width=20)
textValueKp.pack()
tkinter.Button(master=root, text="set Kp", command=lambda: sendSerial(0xa0, convertStringToFloatBytearray(textValueKp.get("1.0", "end-1c")))).pack()
# ~ tkinter.Button(master=root, text="Kp -1", command=lambda: sendSerial(0x93, [])).pack()
# ~ tkinter.Button(master=root, text="Ki +1", command=lambda: sendSerial(0x94, [])).pack()
tkinter.Label(master=root, textvariable=controllerValueKiString, font=("Free Mono", 20)).pack()
textValueKi = tkinter.Text(master=root, height=1, width=20)
textValueKi.pack()
tkinter.Button(master=root, text="set Ki", command=lambda: sendSerial(0xa1, convertStringToFloatBytearray(textValueKi.get("1.0", "end-1c")))).pack()
# ~ tkinter.Button(master=root, text="Ki -1", command=lambda: sendSerial(0x95, [])).pack()
# ~ tkinter.Button(master=root, text="Kd +1", command=lambda: sendSerial(0x96, [])).pack()
tkinter.Label(master=root, textvariable=controllerValueKdString, font=("Free Mono", 20)).pack()
textValueKd = tkinter.Text(master=root, height=1, width=20)
textValueKd.pack()
tkinter.Button(master=root, text="set Kd", command=lambda: sendSerial(0xa2, convertStringToFloatBytearray(textValueKd.get("1.0", "end-1c")))).pack()
# ~ tkinter.Button(master=root, text="Kd -1", command=lambda: sendSerial(0x97, [])).pack()
# ~ tkinter.Button(master=root, text="Kit +1", command=lambda: sendSerial(0x98, [])).pack()
tkinter.Label(master=root, textvariable=controllerValueKitString, font=("Free Mono", 20)).pack()
textValueKit = tkinter.Text(master=root, height=1, width=20)
textValueKit.pack()
tkinter.Button(master=root, text="set Kit", command=lambda: sendSerial(0xa3, convertStringToFloatBytearray(textValueKit.get("1.0", "end-1c")))).pack()
# ~ tkinter.Button(master=root, text="Kit -1", command=lambda: sendSerial(0x99, [])).pack()

tkinter.Button(master=root, text="enable debug data", command=lambda: sendSerial(0x33, [])).pack()
tkinter.Button(master=root, text="disable debug data", command=lambda: sendSerial(0x34, [])).pack()

# ~ tkinter.Button(master=root, text="init", command=lambda: sendSerial(0xf0, [])).pack()

def convertStringToFloatBytearray(inputString):
  try:
    floatValue = float(inputString)
    byteArrayValue = bytearray(struct.pack('>f', floatValue))
    # ~ print(str(floatValue))
    # ~ print([ "0x%02x" % b for b in byteArrayValue ])
    return byteArrayValue
  except:
    print("can not convert [" + inputString + "] to float!")
    return []

root.after(100, readSerial)

sendSerial(0xf0, [])

tkinter.mainloop()