def find_packet(serial):
    if hex(ord(serial.read())) == START_IOPACKET:
        lengthMSB = ord(serial.read())
        lengthLSB = ord(serial.read())
        length = (lengthLSB + (lengthMSB << 8)) + 1
        return serial.read(length)
    else:
        return None

	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 getPosition(serial, device, channel):
    """ Accesses the position of a channel using the pololu protocol.
    SERIAL is the serial device the maestro is attached to.
    DEVICE is the maestro device number. (default 12)
    CHANNEL is the channel to sample.

    When the channel is an analog channel, it reports a value [0...1023].
    """
    serial.write(chr(0xAA))
    serial.write(chr(device))
    serial.write(chr(0x10))
    serial.write(chr(channel))
    return ord(serial.read(1)) + 256 * ord(serial.read(1))

  def read_from(self, serial):
    self.cmd.extend(serial.read(3))

    if self.cmd[_IDX_STX] != _STX:
      raise MalformedCommand(_IDX_STX, _STX, self.cmd[_IDX_STX])

    # the length includes prelude and appendix, which are six bytes total.
    if self.length > 6:
      self.cmd.extend(serial.read(self.length - 6))

    self.cmd.extend(serial.read(3))

    if self.cmd[_IDX_ETX] != _ETX:
      raise MalformedCommand(_IDX_ETX, _ETX, self.cmd[_IDX_ETX])

    def reader(self, serial, color):
        """loop and copy serial->console"""
        first = True
        try:

            line = []

            while self.alive:
                data = serial.read(1)
                if not data:
                    continue

                # don't print a NULL if it's the first character we
                # read.  This hides startup/port-opening glitches with
                # some serial devices.
                if self.suppress_read_firstnull and first and data == "\0":
                    first = False
                    continue
                first = False

                if color != self.last_color:
                    self.last_color = color
                    sys.stdout.write(color)

                line.append(data)
                if data == "\r":
                    # if len(data) > 1:

                    if line != ["\r"]:
                        try:
                            msg = MkMsg.createFromData(line)
                            # if ((msg.command == 'o') or (msg.command == 'O')):
                            print time.strftime("%X") + str(msg)
                        except Exception, e:

                            print e, line
                    line = []
                    # sys.stdout.write("".join(line))
                    # sys.stdout.write("\n")

                # if (self.raw or
                #     (ord(data) >= 32 and ord(data) < 128) or
                #     data == '\r' or data == '\n' or data == '\t'):
                #     if self.add_cr and data == '\n':
                #         sys.stdout.write('\r' + data)
                #     else:
                #         sys.stdout.write(data)
                # else:
                #     sys.stdout.write('\\x'+("0"+hex(ord(data))[2:])[-2:])

                sys.stdout.flush()

        except Exception as e:
            sys.stdout.write(color)
            sys.stdout.flush()
            traceback.print_exc()
            sys.stdout.write(self.color.reset)
            sys.stdout.flush()
            self.console.cleanup()
            os._exit(1)

def dlt645_get_addr(serial):
    #print 'dlt645_get_addr ...'
    try:
        #cmd2 = '\xfe\xfe\xfe\xfe\x68\xaa\xaa\xaa\xaa\xaa\xaa\x68\x13\x00\xdf\x16'
        cmd2 = '\xfe\xfe\xfe\xfe' + encode_dlt645('\xaa\xaa\xaa\xaa\xaa\xaa',0x13,0,'')

        serial.write(cmd2)
        time.sleep(0.5)
        resp = ''
        c = ''
        i = 0
        while c != '\x16' and i < SERIAL_TIMEOUT_CNT:
            c = serial.read(1)
            if len(c) > 0:
                resp += c
            else:
                print '.'
                i += 1
        
        if i >= SERIAL_TIMEOUT_CNT:
            return -1,0
        
        resp1 = dlt_645_rm_fe(resp)

        #print 'resp1:',resp1

        ret,addr,data,ctl = decode_dlt645(resp1)
        if ret == 0:
            return ret,addr
            
        
    except:
        print 'dlt645_get_addr exception!'
        return -1,''

def serial_read():
	global port, serial, isDone, mainMenu, playing, gameOver, speed, char
	while not isDone:
		try:
			value = serial.read()
			if mainMenu:
				if value == 'y':
					char.lastHundred = int(char.distXTraveled/100)
					mainMenu = False
					playing = True
			if playing:
				if value == 'r':
					char.updateComm('right')
				if value == 'l':
					char.updateComm('left')	
				if value == 'j':
					char.updateComm('jump')
				if value == 'y':
					speed = min(max_speed, speed + 0.005)
				if value == 'n':
					speed = max(min_speed, speed - 0.001)
			print value

		except:
			continue

def read_from_port(serial):
    alldata = ""
    dlen = 0
    while stopFlag:
        data = serial.read(256)
        if len(data) != 0:
            alldata = alldata + data

        if (dlen > 0) and (len(alldata) > 0):
            print alldata[0]
            alldata = alldata[1:]
            dlen = dlen - 1

        elif(len(alldata) > 2):
            if alldata[0] == '\xAB' and alldata[1] == '\xAD':
                dlen = ord(alldata[2])
                if dlen <= (len(alldata) - 3):
                    print "receive data:" + alldata[3:3+dlen]
                    dlen = 0
                    alldata = alldata[3+dlen:]
                else:
                    dlen -= (len(alldata) - 3)
                    print "receive data:" + alldata[3:]
                    alldata = ""
            elif alldata[0] == '\xAB' and alldata[1] == '\xA5':
                print "send data success len:" + str(ord(alldata[2]))
                #skip 3 bytes
                alldata = alldata[3:]
            else:
                #skip one byte and continue
                alldata = alldata[1:]

    def _send_and_wait_for_ack(self, packet, serial):
        ack = 0
        MAX_RETRIES = 1

        while ack != 0x06 and int(chr(packet[4])) < MAX_RETRIES:
            serial.write(packet)
            ack = serial.read(1)

            # This violates the principle that we do high level
            # client-side and low level posbox-side but the retry
            # counter is always in a fixed position in the high level
            # message so it's safe to do it. Also it would be a pain
            # to have to throw this all the way back to js just so it
            # can increment the retry counter and then try again.
            packet = packet[:4] + str(int(packet[4]) + 1) + packet[5:]

            if ack:
                ack = ord(ack)
            else:
                _logger.warning("did not get ACK, retrying...")
                ack = 0

        if ack == 0x06:
            return True
        else:
            _logger.error("retried " + str(MAX_RETRIES) + " times without receiving ACK, is blackbox properly connected?")
            return False

def dlt645_read_time(serial,addr,data_tag):
    #print 'dlt645_read_time ...'
    try:
        cmd2 = '\xfe\xfe\xfe\xfe' + encode_dlt645(addr,0x11,4,data_tag)
        serial.write(cmd2)
        time.sleep(0.5)
        resp = ''
        c = ''
        i = 0
        while c != '\x16' and i < SERIAL_TIMEOUT_CNT:
            c = serial.read(1)
            if len(c) > 0:
                resp += c
            else:
                print '.'
                i += 1
            
        if i >= SERIAL_TIMEOUT_CNT:
            return -1,0
            
        resp1 = dlt_645_rm_fe(resp)    
        ret,addr,data,ctl = decode_dlt645(resp1)
        #print data.encode('hex')
        # need to convert to Unix time stamp
        if ret == 0 and len(data) >= 7:
            list1 =  list(bcd2digits(data[4:7]))
            str1 = ''.join(str(e) for e in list1)
            return ret,str1
        else:
            return -1,0
    except:
        print 'dlt645_read_data exception!'
        return -1,0

def read_speed(motor_num):
	if (motor_num == 1):
		board_num = 0x80
		command = 30
		serial = ser_x80
	elif (motor_num == 2):
		board_num = 0x81
		command = 30
		serial = ser_x81
	elif (motor_num == 3):
		board_num = 0x80
		command = 31  ## OK, this is seriously a major WTF here. I need to see if the board is wired wrong or something.
		serial = ser_x80
	data = []
	serial.write(chr(board_num))
	serial.write(chr(command)) 
	for i in range(6):
		data.append(serial.read())


	speed = (data[0].encode("hex")) + (data[1].encode("hex")) + (data[2].encode("hex")) + (data[3].encode("hex"))
	speed = int(speed, 16)
	if ((ord(data[4]) == 1) and (speed != 0)):
		speed = ~(0xffffffff - speed) + 1
	rotations_per_second = float(speed) * 125 / 8192 	
	return rotations_per_second

def read_from_port(serial):
	while True:
		#print ("teste")
		dado=serial.read(64)
		if(len(dado)>0):
			p=":".join("{:02x}".format(ord(c)) for c in dado)
			print "rcv "+p

	def send(self, serial, response_length):
		serial.write(self.frame)
		try:
			if self.parameter:
				response = serial.read(response_length)
				response = binascii.hexlify(response)
				response = response[-4:-2]
				return responseType[response]
			else:
				response = serial.read(response_length)
				response = binascii.hexlify(response)
				status = response[-6:-4]
				data = response[-4:-2]
				data = int(data,16)
				return [ responseType[status], data ]
		except KeyError:
			return 'Invalid response from remote radio'

def read_float(serial):
    """
    helper function to read a float from the serial port
    """
    #convert from little endian(Razor) to big endian
    hex_array = reversed([chr(ord(serial.read(1))) for i in range(4)])
    hex_string = ''.join(hex_array)
    return struct.unpack('>f', hex_string)[0]

def recv(serial):
	while True:
		data = serial.read(30)
		if data == '':
			continue
		else:
			break
		sleep(1)
	return data

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 setDH1():
     serial.write(b'\x41\x54\x44\x48\x31\x33\x41\x32\x30\x30\x0D\x0A')
     time.sleep(1)
     resp = ""
     while (serial.inWaiting() > 0):
         data = serial.read()
         resp += data
         if '\x4F\x4B\x0D' in resp:
                 print "Set DH to Router1"
                 break

def main():
    while True:
        text = raw_input("Enter text to send to Arduino ")
        if not text:
            continue

        arduino.write(text)
        print("Sent %s to arduino " % text)
        time.sleep(1)

        result = serial.read()

def serial_read(num_bytes):
    if not hasattr(serial_read, "warned"):
        serial_read.warned = False
    data = serial.read(num_bytes)
    if len(data) != num_bytes:
        serial_read.warned = True
        print "*** Error reading Scorbot-ER III serial port"
        print "*** Is the cable plugged in and the control box powered on?"
    else:
        serial_read.warned = False
    return data

 def setDL2():
     serial.write(b'\x41\x54\x44\x4C\x34\x30\x45\x38\x33\x33\x46\x31\x0D\x0A')
     time.sleep(1)
     resp = ""
     while (serial.inWaiting() > 0):
         data = serial.read()
         resp += data
         if '\x4F\x4B\x0D' in resp:
                 print "Set DL to Router2"
                 break
     return 0