def probe(x,y):
	global points_on_plane
	serial_reply=""
	serial.flushInput()		   #clean buffer
	probe_point= "G30\r\n" #probing comand
	serial.write(probe_point)
	time.sleep(0.5) #give it some to to start  
	probe_start_time = time.time()
	while not serial_reply[:22]=="echo:endstops hit:  Z:":
	
		#issue G30 Xnn Ynn and waits reply.
		#Expected reply
		#endstops hit:  Z: 0.0000	
		#couldn't probe point: = 
		if (time.time() - probe_start_time>10):
			#10 seconds passed, no contact
			trace_msg="Could not probe "+str(x)+ "," +str(y) + " / " + str(deg) + " degrees"			
			trace(trace_msg)
			#change safe-Z
			return False #leave the function,keep probing
		
		serial_reply=serial.readline().rstrip()
		#add safety timeout, exception management
		time.sleep(0.1) #wait
		pass
		
	#get the z position
	#print serial_reply
	z=float(serial_reply.split("Z:")[1].strip())
	
	new_point = [x,y,z,1]
	points_on_plane = np.vstack([points_on_plane, new_point]) #append new point to the cloud.
	
	trace("Probed "+str(x)+ "," +str(y) + " / " + str(deg) + " degrees = " + str(z))
	return True

def probe(x,y):
	global points_on_plane
	serial_reply=""

	serial.flushInput()
	serial.write("G30\r\n")
	
	probe_start_time = time.time()
	while not serial_reply[:22]=="echo:endstops hit:  Z:":
		serial_reply=serial.readline().rstrip()	
		#issue G30 Xnn Ynn and waits reply.
		if (time.time() - probe_start_time>90):  
			#timeout management
			trace("Could not probe this point")
			return False
			break	
		pass
		
	#get the z position
	z=float(serial_reply.split("Z:")[1].strip())
	
	new_point = [x,y,z,1]
	points_on_plane = np.vstack([points_on_plane, new_point]) #append new point to the cloud.
	
	trace("Probed "+str(x)+ "," +str(y) + " / " + str(deg) + " degrees = " + str(z))
	return True

def macro(code,expected_reply,timeout,error_msg,delay_after,warning=False,verbose=True):
	global s_error
	global s_warning
	global s_skipped
	serial.flushInput()
	if s_error==0:
		serial_reply=""
		macro_start_time = time.time()
		serial.write(code+"\r\n")
		if verbose:
			trace(error_msg)
		time.sleep(0.3) #give it some tome to start
		while not (serial_reply==expected_reply or serial_reply[:4]==expected_reply):
			#Expected reply
			#no reply:
			if (time.time()>=macro_start_time+timeout+5):
				if serial_reply=="":
					serial_reply="<nothing>"
				if not warning:
					s_error+=1
					trace(error_msg + ": Failed (" +serial_reply +")")
				else:
					s_warning+=1
					trace(error_msg + ": Warning! ")
				return False #leave the function
			serial_reply=serial.readline().rstrip()
			#add safety timeout
			time.sleep(0.2) #no hammering
			pass
		time.sleep(delay_after) #wait the desired amount
	else:
		trace(error_msg + ": Skipped")
		s_skipped+=1
		return False
	return serial_reply

def safety_callback(channel):
    
    try:
        code=""
        type=""
        
        if(GPIO.input(2) == GPIO.LOW):
            #todo
            type="emergency"
            serial.flushInput()
            serial.write("M730\r\n")
            reply=serial.readline()
            
            try:
                code=float(reply.split("ERROR : ")[1].rstrip())
            except:
                code=100
            
        
        if(GPIO.input(2) == GPIO.HIGH):
            #to do
            type=""
            code=""
                
        message = {'type': str(type), 'code': str(code)}
        ws.send(json.dumps(message))
        write_emergency(json.dumps(message))
        
    except Exception, e:
        logging.info(str(e))

def modem_terminal(serial,string,timeout):
	serial.flushInput()
	serial.write(string)
	while timeout>0:
		if serial.inWaiting()>0:
			sys.stdout.write(serial.read(serial.inWaiting()))
		time.sleep(0.001)
		timeout=timeout-1
	print ""

def read_serial(gcode):
	serial.flushInput()
	serial.write(gcode + "\r\n")

	response=""
	while (response==""):
		response=serial.readline().rstrip
		if response!="":
			return response

def read_serial(gcode):
	serial.flushInput()
	serial.write(gcode + "\r\n")
	time.sleep(0.1)
	
	#return serial.readline().rstrip()
	response=serial.readline().rstrip()
	
	if response=="":
		return "NONE"
	else:
		return response

def jogToBedCorner(corner, height, feedrate):
	serial.flushInput()
	macro("M402","ok",2,"Retracting Probe (safety)",1, verbose=False)
	corner = corner.upper()
	if (corner=="LU"):
		macro("G0 X"+str(minXPhys)+" Y"+str(minYPhys)+" Z "+str(height)+" F"+str(feedrate),"ok",2,"Moving to left down corner point",0.1)
	if (corner=="LO"):
		macro("G0 X"+str(minXPhys)+" Y"+str(maxYPhys)+" Z "+str(height)+" F"+str(feedrate),"ok",2,"Moving to left upper corner point",0.1)
	if (corner=="RU"):
		macro("G0 X"+str(maxXPhys)+" Y"+str(minYPhys)+" Z "+str(height)+" F"+str(feedrate),"ok",2,"Moving to right lower corner point",0.1)
	if (corner=="RO"):
		macro("G0 X"+str(maxXPhys)+" Y"+str(maxYPhys)+" Z "+str(height)+" F"+str(feedrate),"ok",2,"Moving to right upper corner point",0.1)

	def recv(self,serial):
		buffer = ''
		aux = ''
		while len(str(aux.encode('hex'))) == 0:
			aux = serial.read()
		while len(str(aux.encode('hex'))) > 0:
			buffer += aux
			aux = serial.read()
		serial.flushInput()
		print "    ENCODED: %s" % buffer
		buffer = b64decode ( buffer )
		print "    ENCRYPTED: %s" % buffer.encode('hex')
		return buffer

	def recieve_message(self):
		readMsg = ''
		serial.flushInput()
		time.sleep(4)
		orig_time = time.time()
		while((time.time()-orig_time) < 5 ):
			readMsg = serial.read(serial.inWaiting())
			#print("test")
			time.sleep(1)
			if readMsg != '':
				break
			else:
				raise("problem in recieving message")
		self.logger("Recieving package %s" % readMsg)
		return readMsg

def read_hex_from_ee(address):
	serial.flushOutput()
	serial.flushInput()
	msg = '\x02'
	msg += dec2hex(address)
	if serial.isOpen():
		serial.write(msg)
	else:
		print 'Error, the serial port is not open.'
		return
	value = serial.readline()
	if value == '':
		print 'Error, did not receive response from the Micro Controller.'
		return None
	value = value.strip()
	return value

def tagRead():
    import serial
    try:
        serial= serial.Serial("/dev/ttyACM0", baudrate=9600)
        n=0
        serial.flushInput()
        serial.flushOutput()
        while True:
        	data=serial.readline()
                n=n+1
                if data[0:3]=="ISO" and n>3:
                    myString1=data.find('[')+1
                    myString2=data.find(',')
                    serial.flush()
                    serial.close()
                    return data[myString1:myString2]
    except Exception as e:
        return 0

def probe(x,y):
    serial_reply=""
    serial.flushInput()
    serial.write("G30\r\n")
    probe_start_time = time.time()
    while not serial_reply[:22]=="echo:endstops hit:  Z:":
        serial_reply=serial.readline().rstrip()    
        #issue G30 Xnn Ynn and waits reply.
        if (time.time() - probe_start_time>90):  
            #timeout management
            trace("Could not probe this point")
            return False
            break    
        pass
    #get the z position
    z=float(serial_reply.split("Z:")[1].strip())
    
    new_point = [x,y,z,1]
    
    trace("Probed "+str(x)+ "," +str(y))
    return True

def measurePointHeight(x, y, initialHeight, feedrate):
	macro("M402","ok",2,"Retracting Probe (safety)",1, verbose=False)
	macro("G0 Z60 F5000","ok",5,"Moving to start Z height",10) #mandatory!
	
	
	macro("G0 X"+str(x)+" Y"+str(y)+" Z "+str(initialHeight)+" F10000","ok",2,"Moving to left down corner point",10)
	macro("M401","ok",2,"Lowering Probe",1, warning=True, verbose=False)
	serial.flushInput()
	serial_reply = ""
	probe_start_time=time.time()
	serial.write("G30 U"+str(feedrate)+"\r\n")
	
	while not serial_reply[:22]=="echo:endstops hit:  Z:":
		serial_reply=serial.readline().rstrip()
	if (time.time() - probe_start_time>80): #timeout management
		z = initialHeight 
		serial.flushInput()
		return z
	pass
	if serial_reply!="":
		z=serial_reply.split("Z:")[1].strip()
	serial_reply=""
	serial.flushInput()
	macro("G0 X"+str(x)+" Y"+str(y)+" Z "+str(initialHeight)+" F10000","ok",2,"Moving to left down corner point",10)
	macro("M402","ok",2,"Raising Probe",1, warning=True, verbose=False)	
	return z

def writeToTape(serial, array, maxvalue):
    print("Writing")
    print(array)
    data = ""
    if array[0] > 1e1:
        for x in array:
            towrite = [0, 0, 0]               #rgb
            towrite[0] = x
            towrite[1] = 60
            towrite[2] = 254 - x
                
            for x in towrite:
                capped = int(min(254,max(0,x)))
                data += chr(capped)
    else:
#        cleartape(serial)
        return

    # write control
    serial.write(data)
    serial.write(CONTROL)
    serial.flushInput()
    serial.flush()

	trace("Probed "+str(x)+ "," +str(y) + " / " + str(deg) + " degrees = " + str(z))
	return True

printlog(0,0) #create log vuoto

trace("\n ---------- Initializing ---------- \n")

'''#### SERIAL PORT COMMUNICATION ####'''
serial_port = config.get('serial', 'port')
serial_baud = config.get('serial', 'baud')
serial = serial.Serial(serial_port, serial_baud, timeout=0.5)

#initialize serial
#serial = serial.Serial(port, baud, timeout=0.6)
time.sleep(0.5) 						#sleep a while
serial.flushInput()						#clean buffer
	
trace('PROBE MODULE STARTING')
print 'scanning from' + str(x)+ "," +str(y)+ " to " +str(x1)+ "," +str(y1); 
print 'Total points    : ', tot_probes	
print 'Probing density : ', probe_density , " points/mm"
print 'Num of planes   : ', slices
print 'Start/End       : ', begin ,' to ', end, 'deg'

#ESTIMATED SCAN TIME ESTIMATION
estimated=(slices*tot_probes*3)/60
if(estimated<1):
    estimated*=60
    unit= "Seconds"
else:
    unit= "Minutes"

##
import sys
import serial


try:
    code_to_execute=str(sys.argv[1])
except:
    print "Warning - parameter <CODE> is required"
    sys.exit()

if code_to_execute != "" :
    port = '/dev/ttyAMA0'
    baud = 115200
    serial = serial.Serial(port, baud, timeout=0)
    serial.flushInput()
    serial.flushInput()
    serial.write(code_to_execute)
    data=serial.read(8)
    print data
    

}
print "Reading file..."
f = open("helpers.txt", "r+")
for line in f:
    line = line.replace("\n", "")
    data = line.split(", ")
    helpers.append([data[0], data[1]])
f.close()
print "Done. Ready to scan."
while True:
    if serial.inWaiting() > 0:
        read_result = serial.read(12)
        rfid = format(read_result.decode(encoding="utf-8"))
        if len(rfid) == 12:
            rfid = rfid[1:11]
            print("Read card " + rfid)
            for student in helpers:
                if(rfid == student[0]):
                    print "You are " + student[1]
                    send_data["id"] = student[1]
                    send["data"] = json.dumps(send_data)
                    print requests.post(url, data=send).text
        else:
            print("Misread card")
        print("Please wait for 1 second...");
        time.sleep(1)
        serial.flushInput() # ignore errors, no data
    else:
        time.sleep(0.1)

def rfidFlush(serial):
	serial.read(serial.inWaiting())
	serial.flushInput()