#!/usr/bin/env python

import socket
import struct

UDP_IP = "127.0.0.1"
UDP_PORT = 56789
BUFFER_SIZE = 128

METHOD_READ = 0
METHOD_WRITE = 1

PROTOCOL_VERSION_MASK = 0x3F
HEADER_SIZE = 12

sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind((UDP_IP, UDP_PORT))

while True:
    data, addr = sock.recvfrom(BUFFER_SIZE)
    print "Connection address:", addr
    if not data:
        break
    print "Received data:", ' '.join(x.encode('hex') for x in data)
    
    if len(data) < HEADER_SIZE:
        print "MALFORMED DATA"
        break
    protocol = int(data[0].encode('hex'), 16)
    protocol_version = protocol & PROTOCOL_VERSION_MASK
    method = (protocol >> 7) & 0x01
    endian = (protocol >> 6) & 0x01
    message_id = int(data[1].encode('hex'), 16)
    address = struct.unpack("!I", data[4:8])[0]
    count = struct.unpack("!H", data[8:10])[0]
    print "Protocol version:", str(protocol_version)
    print "Method:", str(method)
    print "Endian:", str(endian)
    print "Message ID:", str(message_id)
    print "Address:", str(address)
    print "Count:", str(count)
    rem_data = []
    if len(data) > HEADER_SIZE:
        rem_data = data[HEADER_SIZE:]
        if len(rem_data) != count * 2:
            print "rem_data != count"
            break
        # decoding by words - 2 bytes
        words = []
        for i in range(count):
            byte_id = i * 2
            words.extend(struct.unpack("!H", rem_data[byte_id:byte_id + 2]))
        print "Values data:", ' '.join(hex(x) for x in words)
    
    if method == METHOD_WRITE:
        count = 0
    msg = bytearray()
    msg.append(protocol)
    msg.append(message_id)
    msg.extend(struct.pack("!H", 0)) # 2 bytes padding
    msg.extend(struct.pack("!I", address))
    msg.extend(struct.pack("!H", count))
    msg.extend(struct.pack("!H", 0)) # 2 bytes padding
    if method == METHOD_READ:
        for i in range(count):
            msg.extend(struct.pack("!H", i))
    
    print "msg:", ' '.join(hex(x) for x in msg)
    sock.sendto(msg, addr)
    print "--------------------------------------------------"
