#!/usr/bin/python

#    HAL userspace component to interface with Arduino board
#    by Colin Kingsbury (https://ckcnc.wordpress.com_)
#    Inspired by the earlier example from Jeff Epler
#
#    This program is free software; you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation; either version 2 of the License, or
#    (at your option) any later version.
#
#    This program is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#
#    You should have received a copy of the GNU General Public License
#    along with this program; if not, write to the Free Software
#    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
import serial
import hal
import sys
import time
import string
 
#First we open the serial port. This should correspond to the port the Arduino
#is connected to. This can be found in the Arduino IDE in Tools->Serial Port
#PORT = "/dev/ttyUSB0"
#time.sleep(2)
PORT = "/dev/ttyACM0"
ser = serial.Serial(PORT, 9600, timeout=0.05)
print "cakam na boot arduina"
time.sleep(3)
#print "malo by byt pripojene"
#linkMsg=ser.readline()
#print linkMsg
vystup=""
mode=""
temp="" 
xos=""
tempo=""
#Now we create the HAL component and its pins
c = hal.component("arduino")
#c.newpin("switch-on",hal.HAL_BIT,hal.HAL_IN)
c.newpin("jogspeed",hal.HAL_FLOAT,hal.HAL_IN)
c.newpin("machine-state",hal.HAL_BIT,hal.HAL_IN)
c.newpin("machine-estop",hal.HAL_BIT,hal.HAL_IN)
c.newpin("machine-running",hal.HAL_BIT,hal.HAL_IN)
c.newpin("machine-paused",hal.HAL_BIT,hal.HAL_IN)
c.newpin("machine-idle",hal.HAL_BIT,hal.HAL_IN)
c.newpin("is-paused",hal.HAL_BIT,hal.HAL_IN)
c.newpin("jogxplus",hal.HAL_BIT,hal.HAL_IN)
c.newpin("jogxminus",hal.HAL_BIT,hal.HAL_IN)
c.newpin("jogyminus",hal.HAL_BIT,hal.HAL_IN)
c.newpin("jogyplus",hal.HAL_BIT,hal.HAL_IN)
c.newpin("jogzminus",hal.HAL_BIT,hal.HAL_IN)
c.newpin("jogzplus",hal.HAL_BIT,hal.HAL_IN)
c.newpin("jogaminus",hal.HAL_BIT,hal.HAL_IN)
c.newpin("jogaplus",hal.HAL_BIT,hal.HAL_IN)
c.newpin("toggleon",hal.HAL_BIT,hal.HAL_IN)
c.newpin("homing",hal.HAL_BIT,hal.HAL_IN)
c.newpin("touchx",hal.HAL_BIT,hal.HAL_IN)
c.newpin("spindleon",hal.HAL_BIT,hal.HAL_IN)
c.newpin("spindleoff",hal.HAL_BIT,hal.HAL_IN)
c.newpin("toggleoff",hal.HAL_BIT,hal.HAL_IN)
c.newpin("jointa",hal.HAL_BIT,hal.HAL_IN)
c.newpin("mdia",hal.HAL_BIT,hal.HAL_IN)
c.newpin("autoa",hal.HAL_BIT,hal.HAL_IN)
c.newpin("manuala",hal.HAL_BIT,hal.HAL_IN)
c.newpin("aauto",hal.HAL_BIT,hal.HAL_IN)
c.newpin("amanual",hal.HAL_BIT,hal.HAL_IN)
c.newpin("amdi",hal.HAL_BIT,hal.HAL_IN)
c.newpin("ajoint",hal.HAL_BIT,hal.HAL_IN)
c.newpin("touchy",hal.HAL_BIT,hal.HAL_IN)
c.newpin("touchz",hal.HAL_BIT,hal.HAL_IN)
#c.newpin("aauto",hal.HAL_BIT,hal.HAL_IN)

c.ready()

#We save the machine state (i.e. whether it's off or on) so that we only
#send a message to the Arduino when it changes
machineState = c['machine-state']
machineEstop = c['machine-estop']
machineRunning = c['machine-running']
machinePaused = c['machine-paused']
machineIdle = c['machine-idle']
paused = c['is-paused']
jogxplus=c['jogxplus']
jogxminus=c['jogxminus']
jogyminus=c['jogyminus']
jogyplus=c['jogyplus']
jogzminus=c['jogzminus']
jogzplus=c['jogzplus']
jogaminus=c['jogaminus']
jogaplus=c['jogaplus']
toggleon=c['toggleon']
homing=c['homing']
touchx=c['touchx']
spindleon=c['spindleon']
spindleoff=c['spindleoff']
toggleoff=c['toggleoff']
jointa=c['jointa']
mdia=c['mdia']
autoa=c['autoa']
manuala=c['manuala']

ajoint=c['ajoint']
amdi=c['amdi']
amanual=c['amanual']
aauto=c['aauto']

touchy=c['touchy']
touchz=c['touchz']
#aauto=c['aauto']


ser.flushInput()
ser.flushOutput()
try:
  while 1:
    time.sleep(.001)
#State
    if(machineState != c['machine-state']):
      if(c['machine-state'] == 1):
        #The machine is on, so turn on the green LED and turn off the red one
        vystup+="G"
      else:
        #opposite of above
        vystup+="F"
      #update the machine state variable
      machineState = c['machine-state']
#Estop
    if(machineEstop != c['machine-estop']):
      if(c['machine-estop'] == 1):
        vystup+="E"
#        ser.write("+E")
      else:
        vystup+="O"
#        ser.write("-E")
      machineEstop = c['machine-estop']
#Running
    if(machineRunning != c['machine-running']):
      if(c['machine-running'] == 1):
        vystup+="R"
#        ser.write("+R")
      else:
#        ser.write("-R")
        vystup+="S"
      machineRunning = c['machine-running']
#Paused
    if(machinePaused != c['machine-paused']):
      if(c['machine-paused'] == 1):
        vystup+="P"
#        ser.write("+P")
      else:
        vystup+="M"
#        ser.write("-P")
      machinePaused = c['machine-paused']

#Joint/Auto/MDI Active
    mode=""
    if(jointa != c['jointa']):   
     if(c['jointa'] == 1):
      mode+="J"
     jointa = c['jointa']

    if(mdia != c['mdia']):
     if(c['mdia'] == 1):
      mode+="D"
     mdia = c['mdia']

    if(autoa != c['autoa']):
     if(c['autoa'] == 1):
      mode+="A"
     autoa = c['autoa']

#    if(manuala != c['manuala']):
#      if(c['manuala'] == 1):
#       mode+="L"
#      manuala = c['manuala']
      
#Idle
    if(machineIdle != c['machine-idle']):
      if(c['machine-idle'] == 1):
        vystup+="I"
      else:
        vystup+="U"
      machineIdle = c['machine-idle']
    if(vystup!=""):
      vy=len(vystup)
      temp=`vy`+vystup
    vystup=""
   #aktualizuj aktivny mod
    if(mode!=""):
     temp=temp[:2]+mode+temp[3:]

#retazec v premennej temp je pripraveny na odoslanie do arduina
    
    mode=""
    time.sleep(.15)
#skontroluj prikazy z arduina
      #Check to see if we have a message waiting from the Arduino
    while ser.inWaiting():
      #This should be set to the length of whatever fixed-length message
      #you're sending from the arduino. It does not have to be the same length
      #as the outbound messages.
      key = ser.readline()
      if '\n' and ';' in key:
        tempo=key.split(";") 
        if len(tempo) == 18 :
          c['jogspeed']=int(tempo[0])
          c['jogxminus']=int(tempo[1])
          c['jogxplus']=int(tempo[2])
          c['jogyminus']=int(tempo[3])
          c['jogyplus']=int(tempo[4])
          c['jogzminus']=int(tempo[5])
          c['jogzplus']=int(tempo[6])
          c['toggleon']=int(tempo[7])
          c['homing']=int(tempo[8])
          c['jogaplus']=int(tempo[9])
          c['jogaminus']=int(tempo[10])
          c['touchx']=int(tempo[11])
          c['spindleon']=int(tempo[12])
          c['spindleoff']=int(tempo[13])
          c['toggleoff']=int(tempo[14])
          if(int(tempo[15])==1): 
           c['ajoint']=1
           c['amdi']=0
           c['aauto']=0
           c['amanual']=0
            
          if(int(tempo[15])==2): 
           c['amdi']=1 
           c['ajoint']=0
           c['aauto']=0
           c['amanual']=0

          if(int(tempo[15])==3): 
           c['aauto']=1 
           c['ajoint']=0
           c['amdi']=0
           c['amanual']=0

          if(int(tempo[15])==4): 
           c['amanual']=1 
           c['ajoint']=0
           c['aauto']=0
           c['amdi']=0
  
          c['touchy']=int(tempo[16])
          c['touchz']=int(tempo[17])


        else:
          time.sleep(1)
          key = ser.readline()
          tempo=key.split(";") 
          c['jogspeed']=int(tempo[0])
          c['jogxminus']=int(tempo[1])
          c['jogxplus']=int(tempo[2])
          c['jogyminus']=int(tempo[3])
          c['jogyplus']=int(tempo[4])
          c['jogzminus']=int(tempo[5])
          c['jogzplus']=int(tempo[6])
          c['toggleon']=int(tempo[7])
          c['homing']=int(tempo[8])
          c['jogaplus']=int(tempo[9])
          c['jogaminus']=int(tempo[10])
          c['touchx']=int(tempo[11])
          c['spindleon']=int(tempo[12])
          c['spindleoff']=int(tempo[13])
          c['toggleoff']=int(tempo[14])
          if(int(tempo[15])==1): 
           c['ajoint']=1 
           c['amdi']=0 
           c['amanual']=0 
           c['aauto']=0 

          if(int(tempo[15])==2): 
           c['amdi']=1 
           c['ajoint']=0 
           c['aauto']=0 
           c['amanual']=0 

          if(int(tempo[15])==3): 
           c['aauto']=1 
           c['ajoint']=0 
           c['amdi']=0 
           c['amaual']=0 

          if(int(tempo[15])==4): 
           c['amanual']=1 
           c['ajoint']=0 
           c['amdi']=0 
           c['aauto']=0 

          c['touchy']=int(tempo[16])
          c['touchz']=int(tempo[17])


#          int(tempo[xx])

#koniec prikazov z arduina
    ser.write(str(temp))     
    
except KeyboardInterrupt:
    ser.write("0000000") 
    time.sleep(.15)
    ser.close()
    raise SystemExit
