#!/usr/bin/python3

from uio.utils import fix_ctypes_struct
from uio.ti.icss import Icss
import ctypes
from ctypes import c_uint32 as u32, c_uint16 as u16, c_int32 as int32, c_bool as boolean
from time import sleep
from sys import exit

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

core_0 = pruss.core0
core_0.load( 'pulse_train.out' )

core_1 = pruss.core1
core_1.load( 'stream_pulse.out' )

class Message( ctypes.Structure ):
    _fields_ = [
            ( 'id',     u32 ),
            ('timestamp',u32),
            ('position',   u32),
            ('force', int32),
        ]

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

# volatile variables in pruss shared memory
class StepperVars( ctypes.Structure ):
    _fields_ = [
            ( 'target_pos',  u32 ),
            ( 'current_pos', u32 ),
            ( 'speed', u32 ),
            ( 'hault_request', boolean ),
            ( 'moving', boolean ),
        ]

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

# map shared memory variables
shmem = core_0.map( StepperVars, 0x00000 )

# ready, set, go!
shmem.hault_request = False
shmem.current_pos = 0
core_0.run()
print('starting PRU0')


# 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_1.map( DDRLayout, 0x10000 )
shmem = core_1.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
id_value = 0
timestamp_cycles = 0
lastid = 0

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

# ready, set, go!
pruss.ecap.pwm.initialize( 2**32 )
core_1.run()


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

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

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

        position = 0
        force = 0
        timestamp = 0
        # 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
        # get 32-bit timestamp (in cycles) from message and unwrap it:
                timestamp_cycles += ( msg.timestamp - timestamp_cycles ) & 0xffffffff
                timestamp_ms = timestamp_cycles // 200000
                timestamp_s = ( timestamp_ms % 60000 ) / 1000
                timestamp_m = timestamp_ms // 60000
                timestamp_h = timestamp_m // 60
                timestamp_m = timestamp_m % 60
                timestamp = f'{timestamp_h:02d}:{timestamp_m:02d}:{timestamp_s:06.3f}'
        # convert to timestamp in seconds:

                force = 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} time={timestamp} position={position:08d} force={force:08d}', end='', flush=True )
        print(f'ridx=0x{ridx:04x} id=0x{lastid:08x} time={timestamp} position={position:08d} force={force:08d}')


try:
        shmem.hault_request = False
        shmem.target_pos = 3000
        shmem.speed = 2
        while True:
            recv_messages()
            check_core()
            sleep( 0.01 )

except KeyboardInterrupt:
    pass

finally:
    print( '', flush=True )
    core_1.halt()
    pruss.uart.io.write( b'FFV\r', discard=True, flush=True )  # interrupt continuous transmission
    sleep( 0.01 )  # make sure that response has been received
    pruss.uart.io.discard()  # discard response