#!/usr/bin/env python

import serial
import serial.rs485
import time
import packet
import crc
import RPi.GPIO as GPIO

def read_reply(sport):
    return sport.read(13)

def print_echo(s, count, before, after):
    barr = bytearray(s)
    print('Rx: %s' % (('STGT','xxxx', 'ARM ', 'GO  ', 'PING')[barr[6]]) ),
    for b in barr:
        print(" %02x" % b),
    print(" [%d src=%04x %1.3fms]" % (count, (ord(s[8])<<8)|ord(s[7]), (after-before)*1000))


def ping_packet(address):
    return packet.ping(address)

def send_packet(sport, s):
    sport.rts = False # Set high... logic inverted
    sport.write([s[0]])
    for c in s[1:]:
        if c == 0xfe or c == 0xff:
            print('escape')
            sport.write([0xfe])
        sport.write([c])
    time.sleep(.01) # Delay
    sport.rts = True # Set low... logic inverted

def main():

    count = 0

    #for b in packet_build(0x0001, 0x42, [0x04, 0xaa, 0x55]):
    #    print("%02x" % b)
    #return

    with serial.Serial('/dev/ttyAMA0', 38400) as ser:
        ser.rts = False # setting it to high. The logic is negated. What a mess.
        # ser.rs485_mode = serial.rs485.RS485Settings(rts_level_for_tx=True, rts_level_for_rx=False, loopback=False)
        while True:
            count += 1
            # 22 col instead of 23 now
            # XXX
            for addr in [1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22]:
                before = time.clock()
                packet.send(ser, ping_packet(addr))
                s = read_reply(ser)
                after = time.clock()
                print_echo(s, count, before, after)
                barr = bytearray(s)
                crc_data = crc.CRC32(barr, len(barr)-4)
                #for b in barr:
                #    print(" %02x" % b),
                crc_rcvd = (barr[len(barr)-1] << 24) |(barr[len(barr)-2] << 16) | (barr[len(barr)-3] << 8) | (barr[len(barr)-4] << 0);
                if (crc_rcvd != crc_data):
                    print(" [!CRC]"),
                print("")
                ser.flush()
                time.sleep(.250)
            break


if __name__ == '__main__':
  main()