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 get_demand_chunk(serial):

    buf = ""
    in_element = False
    closestring = "</InstantaneousDemand>"

    while True:
        in_buf = serial.readline()
        in_buf_stripped = in_buf.strip()
        log.debug(">" + in_buf_stripped)

        if not in_element:
            if in_buf_stripped == "<InstantaneousDemand>":
                in_element = True
                buf += in_buf
                closestring = "</InstantaneousDemand>"
                continue
            elif in_buf_stripped == "<CurrentSummationDelivered>":
                in_element = True
                buf += in_buf
                closestring = "</CurrentSummationDelivered>"
                continue
            else:  # Keep waiting for start of element we want
                continue

        if in_element:
            buf += in_buf

        if in_buf_stripped == closestring:
            log.debug("got end of xml")
            return buf

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 macro(code,expected_reply,timeout,error_msg,delay_after):
	global s_error	
	serial_reply=""
	macro_start_time = time.time()
	serial.write(code+"\r\n")
	time.sleep(0.3) #give it some toie 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):
			#trace_msg="failed macro (timeout):"+ code+ " expected "+ expected_reply+ ", got:"+ serial_reply
			trace(trace_msg,log_trace)
			if not error_msg=="":
				trace("FAILED : "+error_msg,log_trace)
				s_error+=1
				summary()
			#print "error!"
			return False #leave the function
			
		serial_reply=serial.readline().rstrip()
		#print serial_reply
		#add safety timeout
		time.sleep(0.1) #no hammering 
		pass

	trace("OK : " + error_msg,log_trace)	
	time.sleep(delay_after) #wait the desired amount
	return serial_reply

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 serializeFromSerial():
 serial.open()
 data = serial.readline() # Read the next line from serial
 serial.close()
 print(data) # Print the data to terminal for debugging purposes
 j = json.loads(data) # Load the raw string as JSON
 return j # Return the JSON

def updateData(serial):
    try:
        if serial.in_waiting > 0:
            linha_de_dados = serial.readline().decode('utf-8')
            print(linha_de_dados)

            if(linha_de_dados != ""):
                linha_de_dados = linha_de_dados.replace("\r\n", "")
                if linha_de_dados.startswith("!") and linha_de_dados.endswith("@"):
                    linha_de_dados = linha_de_dados.replace(" ", "")
                    linha_de_dados = linha_de_dados.replace("!", "")
                    linha_de_dados = linha_de_dados.replace("@", "")
                    dados = linha_de_dados.split(";")
                    receivedChecksum = 0
                    calculatedChecksum = 0
                    tempDicioDeDados = {}
                    for dado in dados:
                        try:
                            apelido, valor = dado.split("=")
                            if apelido == 'gyz':
                                print(valor)
                            if apelido != 'cks':
                                calculatedChecksum += float(valor)
                                tempDicioDeDados[apelido] = float(valor)
                            else:
                                receivedChecksum = valor
                        except:
                            pass

                        # if abs(float(receivedChecksum) - float(calculatedChecksum)) <=1:
                        #   dicioDeDados.update(tempDicioDeDados)
    except:
        pass

def get_demand_chunk(serial):
    # FIXME Bug where it never re-syncs if reads start mid-block. Needs a rewrite.

    buf = ''
    in_element = False
    closestring = '</InstantaneousDemand>'

    while True:
        in_buf = serial.readline()
        in_buf_stripped = in_buf.strip()
        log.debug('>' + in_buf_stripped)

        if not in_element:
            if in_buf_stripped == '<InstantaneousDemand>':
                in_element = True
                buf += in_buf
                closestring = '</InstantaneousDemand>'
                continue
            elif in_buf_stripped == '<CurrentSummationDelivered>':
                in_element = True
                buf += in_buf
                closestring = '</CurrentSummationDelivered>'
                continue
            else:
                continue

        if in_element:
            buf += in_buf

        if in_buf_stripped == closestring:
            log.debug('got end of xml')
            return buf

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 jsControl():
    buttonSelectMem = False
    zProbeDown = False
    
#     updateConsole(True, consoleStr)
    
    while 1:
        try:
            status = js.getStatus()
        except:
            console1.setPrependString('Joystick disconnected!\n')
            break
            
    
        if status['ButtonCircle']:
            console1.setPrependString('Joystick Jog aborted\n')
            break
        
        if status['ButtonTriangle']:
    		macro("M999", "ok", 1, "reset", 0)
    		
    
    		
    	''' Lower and raise z-probe with select button '''	
        if status['ButtonSelect'] and (not buttonSelectMem):
            buttonSelectMem = True
            if not zProbeDown:
#                 macro("M401","ok",1,"probe down",0)
                serial.write("M401\r\n")
                serial.readall()
                zProbeDown = True
            else:
#                 macro("M402","ok",1,"probe up",0)
                serial.write("M402\r\n")
                serial.readall()
                zProbeDown = False
        elif (not status['ButtonSelect']) and buttonSelectMem:
            buttonSelectMem = False
    		
    		
        gCode, move = calculateGcode(status)
    
        if move:
    
            serial.write(gCode + "\r\n")
            serial.readline().rstrip()
            console1.updatePosition(console1.stringToPos(gCode))

def receiveMessage ():
    global ser
    while True:
        findKeyPass()
       # if serInt > 0:
        #    serInt = randomKey
         #   receivedMessage = ser.read() 
    #receivedMessage = ser.readline() #Reads the incoming, wirelessly transmitted encrypted message from other device
    while True:
        if (keyPassFound == True):
            keyPassRead = serial.readline()
            print ("\nReceived encrypted message: ")
            print ("keyPassRead")
        sleep(.5)
        print("\nDecoding..")
    #chars = []
    #Loop through the message and print out the decoded values
    #for ch in receivedMessage:    #Messages are separated by spaces by default to distinguish words
     #   decodedMessage = decodedMessage + chr(ord(receivedMessage.split()[ch] - randomKey))   #Get the corresponding letter to Unicode number
        #codeMsg = eval(splitMsg)    #Converts digits to a number                           #and further decodes it with randomized key generated.
        #chars.append(chr(ord()code))     #Accumulates characters
    #decodedMessage = "".join(chars)           #Message is formed from characters
    for symbol in enterMessageToDecode:
            charCode = ord(symbol)
            randomKey1 = randomKey
            randomKey1 = int(randomKey * randomKey / 2)
            randomKey2 = int(randomKey1 / (randomKey / 3))
            randomKey3 = int(randomKey2) - abs(int(randomKey)) - abs(int(randomKey))
            randomKey3 -= int(abs(randomKey * 1.5))
            randomKey1 = randomKey3
            charCode -= randomKey1    
            if charCode > ord('z'):
                charCode -= 26
            if symbol.isupper():
                if charCode > ord('Z'):
                    charCode -= 26
                elif charCode < ord('A'):
                    charCode += 26    
            elif symbol.islower():
                if charCode > ord('z'):
                    charCode -= 26
                elif charCode < ord('a'):
                    charCode += 26 
                translated += chr(charCode)
            else:
                translated += symbol
    print("\n    Decrypted message: ")
    print(translated)
    seenMessage = input("\nContinue receiving messages? (Yes / No): \n\n")    #Asks if user wants to keep receiving messages or switch
    if (seenMessage == "Yes") & (seenMessage == "yes"):
        changeMode = False
        startInterface()                     #If yes, go back to main interface and skip setup screen
    elif (seenMessage == "No") & (seenMessage == "no"):
        changeMode = True
        startInterface()                     #If no, go back to main interface and set up sending / receiving
    else: 
        print("\nCommand not understood!\n\n")
        changeMode = False
        startInterface()

def motorThread(threadName, delay):
 count = 0
 while 1:
  print "Motor control thread started."
  motorRX = sys.stdin.readline()
  if motorRX is "RX":
    
    	sys.stdout.write(serial.readline())

def keypadThread(threadName, delay):
 count = 0
 while 1:
  print "Keypad thread started."
  keypadRX = sys.stdin.readline()
  if keypadRX is "RX":
    
    	sys.stdout.write(serial.readline())

def waitfordone(serial):
    while True:
        serialmsg = serial.readline().decode("UTF-8")
        print(serialmsg)
        if "Done" in serialmsg:
            return 0  # no error
        elif "Zero Error" in serialmsg:
            return 1  # error

def query_arduino():
    global serial
    serial = serial.Serial('/dev/ttyACM0', 9600)
    serial.write('1')
    query = serial.readline().strip('\r\n').split()
    fo = open('/etc/scripts/.arduino.db', 'wb')
    fo.write(','.join(query))
    fo.close()

def listener():
	global received
	global sent
	global resend
	
	global ext_temp
	global bed_temp
	global ext_temp_target
	global bed_temp_target
	
	serial_in=""	
	while not EOF:
		while serial_in=="":
			serial_in=serial.readline().rstrip()
			#time.sleep(0.05)
			pass #wait!
		
		#if there is serial in:
		#parse actions:
		
		##ok
		if serial_in=="ok":
			#print "received ok"
			received+=1
			#print "sent: "+str(sent) +" rec: " +str(received)

		##error
		if serial_in[:6]=="Resend":
			#resend line
			resend=serial_in.split(":")[1].rstrip()
			received-=1 #lost a line!
			trace("Error: Line no "+str(resend) + " has not been received correctly")
			
		##temperature report	
		if serial_in[:4]=="ok T":
			#Collected M105: Get Extruder & bed Temperature (reply)
			#EXAMPLE:
			#ok T:219.7 /220.0 B:26.3 /0.0 T0:219.7 /220.0 @:35 [email protected]:0
			
			temps=serial_in.split(" ")
			ext_temp=float(temps[1].split(":")[1])
			ext_temp_target=float(temps[2].split("/")[1])

			bed_temp=float(temps[3].split(":")[1])
			bed_temp_target=float(temps[4].split("/")[1])
			received+=1
			
		## temp report (wait)	
		if serial_in[:2]==" T:":	
			#collected M109/M190 Snnn temp (Set temp and  wait until reached)
			# T:187.1 E:0 B:59
			temps=serial_in.split(" ")
			ext_temp=float(temps[0].split(":")[1])
			bed_temp=float(temps[2].split(":")[1])
			#ok is sent separately.
			
		#clear everything not recognized.
		serial_in=""

	def RecieveSerial(self, device):
		for serial in self.serialPorts:
			if serial.port == device:
				print "Trying to read"
				serial.write(NEXT_DATA.encode())
				lastReadData = serial.readline().strip('\r\n')
				print "data read = " + lastReadData
				return lastReadData
		raise ErrorRecievingData("cannot recieve data from serial port {}".format(device))

def send_string(serial, string):
    message = "{0}{1}{2}".format(
        chr(2),
        string,
        chr(13)
    ).encode()
    logger.debug("sending to serial %s: %s\n", serial.name, message)
    serial.write(message)
    return " ".join(serial.readline().decode().split()[1:])

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

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