#!/usr/bin/python3

from uio.utils import fix_ctypes_struct
from uio.ti.icss import Icss
import ctypes
from ctypes import c_uint32 as uint32, c_uint16 as uint16, c_int32 as int32, c_uint8 as uint8
from time import sleep
from sys import exit

pruss = Icss( "/dev/uio/pruss/module" )
pruss.initialize()
ddr = pruss.ddr

core = pruss.core0
core.load( 'fw-c/stream.out' )

class Message( ctypes.Structure ):
    _fields_ = [
            ("id",uint32), # id
            ("time", uint32),   # time in cycles
            ("position", uint32), # in encoder pulsest 1140 per Revo
            ("force", int32),  # in grams i
            ("duty_cycle", uint8), # duty cycle 0%-100%
            ("wave_shape",uint8), # wave shape input

        ]

# used to communicate ddr layout to C program
class DDRLayout( ctypes.Structure ):
    _fields_ = [
            ( 'msgbuf',      uint32 ),
            ( 'num_msgs',    uint16 ),
            ( 'msg_size',    uint16 ),
        ]

# volatile variables in pruss shared memory
class SharedVars( ctypes.Structure ):
    _fields_ = [
            ( 'abort_file', uint32 ),
            ( 'abort_line', uint32 ),

            ( 'ridx',       uint16 ),
        ]

# if you don't want the ringbuffer at the start of the ddr region, specify offset here
MSGBUF_OFFSET = 0

# you can use a fixed ringbuffer size:
#NUM_MSGS = 1024
# or you can scale the ringbuffer to fit the size of the ddr region:
NUM_MSGS = ( ddr.size - MSGBUF_OFFSET - ctypes.sizeof( u16 ) ) // ctypes.sizeof( Message )
NUM_MSGS = min( NUM_MSGS, 65535 )  # make sure number of messages fits in u16

# map shared memory variables
ddr_layout = core.map( DDRLayout, 0x10000 )
shmem = core.map( SharedVars, 0x10100 )
msgbuf = ddr.map( Message * NUM_MSGS, MSGBUF_OFFSET )
ddr_widx = ddr.map( u16, MSGBUF_OFFSET + ctypes.sizeof( msgbuf ) )

# inform pru about layout of shared ddr memory region
ddr_layout.msgbuf       = ddr.address + MSGBUF_OFFSET
ddr_layout.num_msgs     = NUM_MSGS
ddr_layout.msg_size     = ctypes.sizeof( Message )

# local copies of read-pointer and write-pointer
ridx = 0
widx = 0

# initialize pointers in shared memory
shmem.ridx = ridx
ddr_widx.value = widx

# ready, set, go!
core.run()

def check_core():
    if not core.halted:
        return

    if core.state.crashed:
        msg = f'core crashed at pc={core.pc}'
    elif shmem.abort_file == 0:
        msg = f'core halted at pc={core.pc}'
    else:
        # FIXME figure out better way to read C-string from PRU memory
        abort_file = core.read( ctypes.c_char * 32, shmem.abort_file ).value
        abort_file = abort_file.decode("ascii")
        msg = f'core aborted at pc={core.pc} ({abort_file}:{shmem.abort_line})'

    # dump some potentially interesting information:
    msg += f'\n   ridx       = {ridx}'
    msg += f'\n   shmem.ridx = {shmem.ridx}'
    msg += f'\n   ddr_widx   = {ddr_widx.value}'

    exit( msg )

lastid = 0

def recv_messages():
    global ridx, widx, lastid

    # read updated write-pointer
    widx = ddr_widx.value
    assert widx < NUM_MSGS  # sanity-check

    # process messages
    while ridx != widx:
        # note: it may be faster to copy a batch of messages from shared memory
        # instead of directly accessing individual messages and their fields.
        msg = msgbuf[ ridx ]

        # sanity-check that message id increments monotonically
        lastid = ( lastid + 1 ) & 0xffffffff
        assert msg.id == lastid

        #checking to see if I got message
        print('Message contents position {} and force {}.format(msg.position,msg.force))
        # consume message and update read pointer
        del msg  # direct access to message forbidden beyond this point
        ridx += 1
        if ridx == NUM_MSGS:
            ridx = 0
        shmem.ridx = ridx

    # update user interface
    print( f'\ridx=0x{ridx:04x} id=0x{lastid:08x} ', end='', flush=True )


try:
    while True:
        recv_messages()
        check_core()

        sleep( 0.01 )

except KeyboardInterrupt:
    pass

finally:
    print( '', flush=True )
    core.halt()