#!/usr/bin/python2.7
from pymodbus.client.sync import ModbusSerialClient as ModbusClient
import hal, time

h = hal.component("hpmx")

#Input Pins
h.newpin("set_current", hal.HAL_FLOAT, hal.HAL_IN)	#set cutting current 
h.newpin("set_mode", hal.HAL_FLOAT, hal.HAL_IN)		#Set cutting mode
h.newpin("set_pressure", hal.HAL_FLOAT, hal.HAL_IN)	#Set air pressure

#Out Pins
h.newpin("current_fb", hal.HAL_FLOAT, hal.HAL_IN)	#set cutting current 
h.newpin("mode_fb", hal.HAL_FLOAT, hal.HAL_IN)		#Set cutting mode
h.newpin("pressure_fb", hal.HAL_FLOAT, hal.HAL_IN)	#Set air pressure
h.newpin("fault_code", hal.HAL_FLOAT, hal.HAL_OUT)	#Error code
h.newpin("status_fb", hal.HAL_BIT, hal.HAL_OUT)	#conection status out
h.ready()


tmp_mode	= 	0    #tmp mode reg
tmp_current	= 	0    #tmp current reg
tmp_pressure	=	0    #tmp pressure reg
Reg04W 		= 	0    #h.Max_VW
 


def SetRemote():
   	m_fb = Thc.write_register(0x2093,0x0001, unit=0x01) #mode
	if(m_fb):
		h.mode_fb = m_fb.value

    	c_fb = Thc.write_register(0x2094,0x0000, unit=0x01) #current
	if(c_fb):
		h.current_fb = c_fb.value/64

    	p_fb = Thc.write_register(0x2096,0x0000, unit=0x01) #pressure
	if(p_fb):	
		h.pressure_fb = p_fb.value/128

	if(m_fb and c_fb and p_fb):
		st = 1
	else:
		st = 0
    	return(st)		


Thc = ModbusClient(method='ascii', port='/dev/ttyUSB0',
                          stopbits=1, parity = 'E',bytesize=8, timeout=0.1,baudrate=19200)

con = Thc.connect()
#print (con)

reg = SetRemote() #Set remote 
restart_flag = 0  #Flag to signal restart connection 
counter = 0		
	
try:
    #Main Loop
	while (1):
	
		time.sleep(0.1)
		counter = counter + 1
		
		##########################################################
		######## Start Set current mode and presure values #######
		########################################################## 
        	#if (tmp_mode != h.set_mode):
		if (h.mode_fb != h.set_mode):
            		#tmp_mode = h.set_mode
	    		m_fb = Thc.write_register(0x2093,h.set_mode, unit=1)
			h.mode_fb = m_fb.value

		#if (tmp_current != h.set_current):
		if (h.current_fb != h.set_current):
            		#tmp_current = h.set_current
	    		c_fb = Thc.write_register(0x2094,h.set_current * 64, unit=1)
			h.current_fb = c_fb.value/64

		if (h.pressure_fb != h.set_pressure):
            		#tmp_pressure = h.set_pressure
	    		p_fb = Thc.write_register(0x2096,h.set_pressure * 128, unit=1)
			h.pressure_fb = p_fb.value/128	
          		

		###############################################################
		########### Start get plasma values and status    #############
		###############################################################
		if (counter >= 3):
			counter = 0

			if (restart_flag == 1):	#restart connection if we need
				con = Thc.connect()
				time.sleep(0.3)
				reg = SetRemote() #Set remote
				if(reg == 1):
					restart_flag = 0
					#print("ready")
			if(restart_flag == 0):
		
				e_fb = Thc.read_holding_registers(0x2098,1,unit=1) #Get error code
				if(e_fb):
					h.fault_code = e_fb.registers[0]
				else:
					h.fault_code = 0

				m_fb = Thc.read_holding_registers(0x2093,1,unit=1) #Get mode
				if(m_fb):
					h.mode_fb = m_fb.registers[0]
				else:
					h.mode_fb = 0

				c_fb = Thc.read_holding_registers(0x2094,1,unit=1) #get current
				if(c_fb):
					h.current_fb = c_fb.registers[0]/64
				else:
					h.current_fb = 0
		
				p_fb = Thc.read_holding_registers(0x2096,1,unit=1) #Get Pressure
				if(p_fb):
					h.pressure_fb = p_fb.registers[0]/128
				else:
					h.pressure_fb = 0

				

			if (e_fb and m_fb and c_fb and p_fb):	
				h.status_fb = 1 	#set status to 1 we are ok
			else :
				Thc.close()  		#close port 
				h.status_fb = 0		#set status to 0
				restart_flag = 1 	# connection need to be resarted
			
			
	Thc.close()  #close port 
      	
except KeyboardInterrupt:
    raise SystemExit