#!/usr/bin/env python

import string, struct
from cStringIO import StringIO

short_length      = 2
ushort_length     = 2
long_length       = 4
ulong_length      = 4
long_long_length  = 8
ulong_long_length = 8
float_length      = 4
double_length     = 8
long_double_length= 16

def padding_length(offset, length):

    excess = offset % length
    
    if excess == 0:
        return 0

    return length - excess

def padding(offset, length):

    return "\0" * padding_length(offset, length)

def count(format, octets, offset, byte_order, value):

    if format == 'b':

        # boolean

        return 1

    elif format == 'c':

	# char 

        return 1
    
    elif format == 'C':

        # wide char

        # It's difficult to count the size, since the length has already
        # been emitted
        raise NotImplementedError

    elif format == 'o':

	# octet 

        return 1

    elif format == 'h':

	# short

        return padding_length(offset, short_length) + short_length

    elif format == 'H':

	# unsigned short

        return padding_length(offset, ushort_length) + ushort_length

    elif format == 'l':

	# long

        return padding_length(offset, long_length) + long_length

    elif format == 'L':

	# unsigned long

        return padding_length(offset, ulong_length) + ulong_length

    elif format == 'n':

	# long long

        return padding_length(offset, long_long_length) + long_long_length

    elif format == 'N':

	# unsigned long long

        return padding_length(offset, ulong_long_length) + ulong_long_length

    elif format == 'f':

	# float

        return padding_length(offset, float_length) + float_length

    elif format == 'd':

	# double

        return padding_length(offset, double_length) + double_length

    elif format == 'D':

        # long double

        return padding_length(offset, double_length) + long_double_length

    elif format == 's':

	# string

	# Determine the number of bytes needed to hold the string size

	ulong_padding_length = padding_length(offset, ulong_length)
        string_size_length = ulong_padding_length + ulong_length

	# Now determine the length of the string

	string_length = len(value) + 1

	# Return the total number of bytes needed for the string

	return string_size_length + string_length
    
    elif format == 'S':

        # It's difficult to count the size, since the length has already
        # been emitted
        raise NotImplementedError

    elif format == 'O':

	# sequence of octets

	# Determine the number of bytes needed to hold the sequence size

	ulong_padding_length = padding_length(offset, ulong_length)
        sequence_size_length = ulong_padding_length + ulong_length

	# Now determine the length of the sequence

	sequence_length = len(value)

	# Return the total number of bytes needed for the sequence

	return sequence_size_length + sequence_length

    return -1

def marshal(format, octets, offset, byte_order, value):

    if format == 'b':

        # boolean

	octets.write(chr(value))

        return offset + 1

    if format == 'c':

        # char

	octets.write(value)

        return offset + 1
    
    if format == 'C':

        # wide char

        octets.write(value)

        return offset + len(value)

    if format == 'o':

        # octet

	octets.write(chr(value))

        return offset + 1

    if format == 'h':

        # short

	short_padding_length = padding_length(offset, short_length)
	octets.write("\0" * short_padding_length)

	if byte_order == 0:
	    order_fmt = '>h'
	else:
	    order_fmt = '<h'

	str = struct.pack(order_fmt, value)
	octets.write(str)

        return offset + short_padding_length + short_length

    if format == 'H':

        # unsigned short

	ushort_padding_length = padding_length(offset, ushort_length)
	octets.write("\0" * ushort_padding_length)

	if byte_order == 0:
	    order_fmt = '>H'
	else:
	    order_fmt = '<H'

	str = struct.pack(order_fmt, value)
	octets.write(str)

        return offset + ushort_padding_length + ushort_length

    if format == 'l':

        # long

	long_padding_length = padding_length(offset, long_length)
	octets.write("\0" * long_padding_length)

	if byte_order == 0:
	    order_fmt = '>l'
	else:
	    order_fmt = '<l'

	str = struct.pack(order_fmt, value)
	octets.write(str)

        return offset + long_padding_length + long_length

    if format == 'L':

        # unsigned long

	ulong_padding_length = padding_length(offset, ulong_length)
	octets.write("\0" * ulong_padding_length)

	if byte_order == 0:
	    order_fmt = '>L'
	else:
	    order_fmt = '<L'

	str = struct.pack(order_fmt, value)
	octets.write(str)

        return offset + ulong_padding_length + ulong_length

    if format == 'n':

        # long long

	long_long_padding_length = padding_length(offset, long_long_length)
	octets.write("\0" * long_long_padding_length)

	if byte_order == 0:
	    order_fmt = '>LL'

	    str = struct.pack(order_fmt,
                              (value & 0xffffffff00000000L) >> 32,
                              value & 0x00000000ffffffffL)
	else:
	    order_fmt = '<LL'

	    str = struct.pack(order_fmt,
                              value & 0x00000000ffffffffL,
                              (value & 0xffffffff00000000L) >> 32)

	octets.write(str)

        return offset + long_long_padding_length + long_long_length

    if format == 'N':

        # unsigned long long

	ulong_long_padding_length = padding_length(offset, ulong_long_length)
	octets.write("\0" * ulong_long_padding_length)

	if byte_order == 0:
	    order_fmt = '>LL'

	    str = struct.pack(order_fmt,
                              (value & 0xffffffff00000000L) >> 32,
                              value & 0x00000000ffffffffL)
	else:
	    order_fmt = '<LL'

	    str = struct.pack(order_fmt,
                              value & 0x00000000ffffffffL,
                              (value & 0xffffffff00000000L) >> 32)

	octets.write(str)

        return offset + ulong_long_padding_length + ulong_long_length

    if format == 'f':

        # float

	float_padding_length = padding_length(offset, float_length)
	octets.write("\0" * float_padding_length)

	if byte_order == 0:
	    order_fmt = '>'
	else:
	    order_fmt = '<'

	str = struct.pack(order_fmt + 'f', value)
	octets.write(str)

        return offset + float_padding_length + float_length

    if format == 'd':

        # double

	double_padding_length = padding_length(offset, double_length)
	octets.write("\0" * double_padding_length)

	if byte_order == 0:
	    order_fmt = '>'
	else:
	    order_fmt = '<'

	str = struct.pack(order_fmt + 'd', value)
	octets.write(str)

        return offset + double_padding_length + double_length

    if format == 'D':

        # long double

	double_padding_length = padding_length(offset, double_length)
	octets.write("\0" * double_padding_length)

        if byte_order == 0:
            octets.write(value.big_endian_bytes())
        else:
            octets.write(value.little_endian_bytes())

        return offset + double_padding_length + long_double_length

    if format == 's':

        #
	# string
	#

	if byte_order == 0:
	    order_fmt = '>L'
	else:
	    order_fmt = '<L'

	ulong_padding_length = padding_length(offset, ulong_length)
	octets.write("\0" * ulong_padding_length)

	length_string = struct.pack(order_fmt, len(value)+1)
	octets.write(length_string)

	octets.write(value)
	octets.write("\0")

        return offset + ulong_padding_length + ulong_length + len(value) + 1
    
    if format == 'S':

        #
	# wide string; expecting plain bytes
	#

        octets.write(value)
        return offset + len(value)

    if format == 'O':

        #
	# Sequence of octets
	#

	if byte_order == 0:
	    order_fmt = '>L'
	else:
	    order_fmt = '<L'

	ulong_padding_length = padding_length(offset, ulong_length)
	octets.write("\0" * ulong_padding_length)

	length_string = struct.pack(order_fmt, len(value))
	octets.write(length_string)

	octets.write(value)

        return offset + ulong_padding_length + ulong_length + len(value)

    return -1

def bulk_marshal(format, struct_format, data, offset, byte_order, value):

    if format == 'b':

        # boolean

	struct_format.write('B');
	data.append(value);

        return offset + 1

    if format == 'c':

        # char

	struct_format.write('c')
	data.append(value)

        return offset + 1

    if format == 'o':

        # octet

	struct_format.write('B')
	data.append(value)

        return offset + 1

    if format == 'h':

        # short

	short_padding_length = padding_length(offset, short_length)
	struct_format.write('x' * short_padding_length)

	struct_format.write('h')
	data.append(value)

        return offset + short_padding_length + short_length

    if format == 'H':

        # unsigned short

	ushort_padding_length = padding_length(offset, ushort_length)
	struct_format.write('x' * ushort_padding_length)

	struct_format.write('H')
	data.append(value)

        return offset + ushort_padding_length + ushort_length

    if format == 'l':

        # long

	long_padding_length = padding_length(offset, long_length)
	struct_format.write('x' * long_padding_length)

	struct_format.write('l')
	data.append(value)

        return offset + long_padding_length + long_length

    if format == 'L':

        # unsigned long

	ulong_padding_length = padding_length(offset, ulong_length)
	struct_format.write('x' * ulong_padding_length)

	struct_format.write('L')
	data.append(value)

        return offset + ulong_padding_length + ulong_length

    if format == 'n':

        # long long

	long_long_padding_length = padding_length(offset, long_long_length)
	struct_format.write('x' * long_long_padding_length)

	struct_format.write('LL')

	if byte_order == 0:
	    data.append((value & 0xffffffff00000000L) >> 32)
	    data.append(value & 0x00000000ffffffffL)
        else:
	    data.append(value & 0x00000000ffffffffL)
	    data.append((value & 0xffffffff00000000L) >> 32)

        return offset + long_long_padding_length + long_long_length

    if format == 'N':

        # unsigned long long

	long_long_padding_length = padding_length(offset, long_long_length)
	struct_format.write('x' * long_long_padding_length)

	struct_format.write('LL')

	if byte_order == 0:
	    data.append((value & 0xffffffff00000000L) >> 32)
	    data.append(value & 0x00000000ffffffffL)
        else:
	    data.append(value & 0x00000000ffffffffL)
	    data.append((value & 0xffffffff00000000L) >> 32)

        return offset + long_long_padding_length + long_long_length

    if format == 'f':

        # float

	float_padding_length = padding_length(offset, float_length)
	struct_format.write('x' * float_padding_length)

	struct_format.write('f')
	data.append(value)

        return offset + float_padding_length + float_length

    if format == 'd':

        # double

	double_padding_length = padding_length(offset, double_length)
	struct_format.write('x' * double_padding_length)

	struct_format.write('d')
	data.append(value)

        return offset + double_padding_length + double_length

    if format == 's':

        #
	# string
	#

	ulong_padding_length = padding_length(offset, ulong_length)
	struct_format.write('x' * ulong_padding_length)

	struct_format.write('L')
	data.append(len(value)+1)

	struct_format.write(str(len(value))+'sc')
	data.append(value)
	data.append("\0")

        return offset + ulong_padding_length + ulong_length + len(value) + 1

    if format == 'O':

        #
	# Sequence of octets
	#

	ulong_padding_length = padding_length(offset, ulong_length)
	struct_format.write('x' * ulong_padding_length)

	struct_format.write('L')
	data.append(len(value))

	struct_format.write(str(len(value)) + 's')
	data.append(value)

        return offset + ulong_padding_length + ulong_length + len(value)

    return -1

def unmarshal(format, octets, offset, byte_order):

    if format == 'b':

        # boolean

	str = octets.read(1)
	value = ord(str)

        return (offset + 1, value)

    if format == 'c':

        # char

	ch = octets.read(1)

        return (offset + 1, ch)
    
    if format == 'C':

        # wide char; expect length as the parameter

        length = format[1]
        value = octets.read(length)
        return (offset + length, value)

    if format == 'o':

        # octet

	str = octets.read(1)
	value = ord(str)

        return (offset + 1, value)

    if format == 'h':

        # short

	short_padding_length = padding_length(offset, short_length)
	octets.read(short_padding_length)

	if byte_order == 0:
	    order_fmt = '>h'
	else:
	    order_fmt = '<h'

	str = octets.read(short_length)
	value = struct.unpack(order_fmt, str)[0]

        return (offset + short_padding_length + short_length, value)

    if format == 'H':

        # short

	ushort_padding_length = padding_length(offset, ushort_length)
	octets.read(ushort_padding_length)

	if byte_order == 0:
	    order_fmt = '>H'
	else:
	    order_fmt = '<H'

	str = octets.read(ushort_length)
	value = struct.unpack(order_fmt, str)[0]

        return (offset + ushort_padding_length + ushort_length, value)

    if format == 'l':

        # long

	long_padding_length = padding_length(offset, long_length)
	octets.read(long_padding_length)

	if byte_order == 0:
	    order_fmt = '>l'
	else:
	    order_fmt = '<l'

	str = octets.read(long_length)
	value = struct.unpack(order_fmt, str)[0]

        return (offset + long_padding_length + long_length, value)

    if format == 'L':

        # unsigned long

	ulong_padding_length = padding_length(offset, ulong_length)
	octets.read(ulong_padding_length)

	if byte_order == 0:
	    order_fmt = '>L'
	else:
	    order_fmt = '<L'

	str = octets.read(ulong_length)
	value = struct.unpack(order_fmt, str)[0]

        return (offset + ulong_padding_length + ulong_length, value)

    if format == 'n':

        # long long

	long_long_padding_length = padding_length(offset, long_long_length)
	octets.read(long_long_padding_length)
	
	if byte_order == 0:
	    order_fmt = '>LL'
	else:
	    order_fmt = '<LL'

	str = octets.read(ulong_long_length)

	long_values = struct.unpack(order_fmt, str)

	if byte_order == 0:
	    value = long_values[0] << 32
	    value |= long_values[1]
	else:
	    value = long_values[1] << 32
	    value |= long_values[0]

	# Deal with negative (two's complement) numbers
        if value & 0x8000000000000000L != 0:
	    value = ~(0xffffffffffffffffL - value)

        return (offset + long_long_padding_length + long_long_length, value)

    if format == 'N':

        # unsigned long long

	long_long_padding_length = padding_length(offset, long_long_length)
	octets.read(long_long_padding_length)
	
	if byte_order == 0:
	    order_fmt = '>LL'
	else:
	    order_fmt = '<LL'

	str = octets.read(ulong_long_length)

	long_values = struct.unpack(order_fmt, str)

	if byte_order == 0:
	    value = long_values[0] << 32
	    value |= long_values[1]
	else:
	    value = long_values[1] << 32
	    value |= long_values[0]

        return (offset + long_long_padding_length + long_long_length, value)

    if format == 'f':

        # float

	float_padding_length = padding_length(offset, float_length)
	octets.read(float_padding_length)

	if byte_order == 0:
	    order_fmt = '>f'
	else:
	    order_fmt = '<f'

	str = octets.read(float_length)
	value = struct.unpack(order_fmt, str)[0]

        return (offset + float_padding_length + float_length, value)

    if format == 'd':

        # double

	double_padding_length = padding_length(offset, double_length)
	octets.read(double_padding_length)

	if byte_order == 0:
	    order_fmt = '>d'
	else:
	    order_fmt = '<d'

	str = octets.read(double_length)
	value = struct.unpack(order_fmt, str)[0]

        return (offset + double_padding_length + double_length, value)

    if format == 'D':

        # long double

	double_padding_length = padding_length(offset, double_length)
	octets.read(double_padding_length)

        str = octets.read(long_double_length)

        import LongDouble
        value = LongDouble.LongDouble(str, byte_order)
        return (offset + double_padding_length + long_double_length, value)

    if format == 's':

        #
	# string
	#

	if byte_order == 0:
	    order_fmt = '>L'
	else:
	    order_fmt = '<L'

	ulong_padding_length = padding_length(offset, ulong_length)
	octets.read(ulong_padding_length)

	length_string = octets.read(ulong_length)
	length = struct.unpack(order_fmt, length_string)[0]

	value = octets.read(length-1)
	octets.read(1) # Throw away trailing "\0"

        return (offset + ulong_padding_length + ulong_length + length, value)
    
    if format[0] == 'S':

        #
	# wide string, expect length as the parameter
	#

        length = format[1]
        value = octets.read(length)
        return (offset + length, value)

    if format == 'O':

        #
	# Sequence of octets
	#

	if byte_order == 0:
	    order_fmt = '>L'
	else:
	    order_fmt = '<L'

	ulong_padding_length = padding_length(offset, ulong_length)
	octets.read(ulong_padding_length)

	length_string = octets.read(ulong_length)
	length = struct.unpack(order_fmt, length_string)[0]

	value = octets.read(length)

        return (offset + ulong_padding_length + ulong_length + length, value)

    return None

def octet_to_ASCII(octets):

    buffer = StringIO()

    for octet in octets:

        buffer.write('%02X' % ord(octet))

    return buffer.getvalue()

def ASCII_to_octet(str):

    string_buffer = StringIO(str)
    octet_buffer = StringIO()

    char_pair = string_buffer.read(2)
    while char_pair != '':

	octet = int(char_pair, 16)
        octet_buffer.write(chr(octet))

        char_pair = string_buffer.read(2)

    return octet_buffer.getvalue()