#!/usr/bin/python3
from uio.utils import fix_ctypes_struct
from uio.ti.icss import Icss
from uio.ti.pwmss import Pwmss
from ctypes import c_uint32 as uint32, c_uint16 as uint16, c_int32 as int32, Structure
from sys import exit
from time import sleep


class Message( Structure ):
    _fields_ = [
        ("position", uint32), # in encoder pulsest 1140 per Revo
        ("force", int32),  # in grams
    ]



def check_core():
    if not core_1.halted:
        return
    if core_1.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_1.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:
        exit( msg )


# variable declarations
print('delacaring variables')
divider = 1
frequency= 10000
pruss = Icss( "/dev/uio/pruss/module" )
period = round( 100e6 / divider / frequency )
motor = Pwmss( "/dev/uio/pwmss1" ).pwm
core_0 = pruss.core0
core_1 = pruss.core1
position = pruss.dram0.map( uint32 )

# initializing subsystems
print('intiatilizing subsystem')
pruss.initialize()
pruss.uart.initialize(baudrate = 460800)
motor.initialize(period,divider)
core_0.load('fw/decoder.bin' )
core_1.load('fw-c/find_top.out')

# Starting process
print(' statrting find max procedure')
core_0.run()
core_1.run()

sleep(3)

print(check_core())

# stoppping
pruss.uart.io.write( b'FFV\r', discard=True, flush=True )
sleep( 0.1 )
pruss.uart.io.discard()
core_1.halt()
core_0.halt()

print(f'top position is {position.value}')