import sys # We need sys so that we can pass argv to QApplication
import asyncio
from uio.ti.icss import Icss
from uio.ti.pwmss import Pwmss
from ctypes import c_uint32 as uint32, c_uint8 as uint8, c_uint16 as uint16, c_uint64 as uint64, c_char as char, c_int32 as int32, c_bool as boolean, Structure
from ctypes import sizeof as c_sizeof
from pathlib import Path
from time import sleep
import numpy as np


pruss = Icss( "/dev/uio/pruss/module" )
core_1 = pruss.core1
frequency= 10000
period = None
divider = 1
position = None
weight = None
pruvars = None
top_motor_speed = period
current_motor_speed = None
maxloadposition = None
minloadposition = None
last_timestamp_cycles = 0
stepper_step = None
stepper_direction = None
server1 = None
server2 = None
coro1 = None
coro2 = None
data_queue = None
active_run = None
shut_down = None
NUM_MSGS = None
ddr_layout = None
shmem = None
msgbuf = None
ddr_widx = None
ddr_layout = None
rdix = 0
wdix = 0
lastid = 0
timestamp_cycles = 0
last_encoder_count = uint32(0)
last_encoder_angle = 0
encoder_count = uint32(0)
encoder_angle = 0
tare_coeff = None
emi_coeff = None
val_stream = None
line_count = 0
pruss.initialize()
period = round( 100e6 / divider / frequency )
top_motor_speed = period
pruss.uart.initialize(baudrate = 460800)
magnet = Pwmss( "/dev/uio/pwmss1" ).pwm
magnet.initialize(period,divider)
current_magnet_speed = 0
magnet_direction = Gpio('magnet_dir')
magnet_direction.set_value( 0 )
data_queue = asyncio.Queue()
stop_active_run = False
shut_down = False
ddr = pruss.ddr

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

# volatile variables in pruss shared memory
class SharedVars( Structure ):
	_fields_ = [
		( 'abort_file', uint32 ),
		( 'abort_line', uint32 ),
		( 'ridx',       uint16 ),
		( 'cycle_limit', uint16),
		( 'time_limit', uint16),
		( 'signal_length', uint16),
		( 'signal', uint16*2000)
	]

class Message( ctypes.Structure ):
	_fields_ = [
		( 'id', uint32 ),
		('timestamp', uint32),
		('position', uint32),
		('control_signal', int32),
		('motor_effort', int32),
		('force', int16),
		('padding', int16)
	]

class Gpio:
    def __init__( self, name ):
        self.name = name
        self._value_path = Path( '/dev/gpio', name, 'value' )

    def get_value( self ):
        return int( self._value_path.read_text() )

    def set_value( self, value ):
        self._value_path.write_text( str( value ) )


def check_core(self):
    if not self.core_1.halted:
        return
    if self.core_1.state.crashed:
        msg = f'core crashed at pc={self.core_1.pc}'
    elif self.shmem.abort_file == 0:
        msg = f'core halted at pc={self.core_1.pc}'
    else:
        # FIXME figure out better way to read C-string from PRU memory
        abort_file = self.core_1.read( char * 32, self.shmem.abort_file ).value
        abort_file = abort_file.decode("ascii")
        msg = f'core aborted at pc={self.core_1.pc} ({abort_file}:{self.shmem.abort_line})'
    # dump some potentially interesting information:
    msg += f'\n   ridx       = {self.ridx}'
    msg += f'\n   shmem.ridx = {self.shmem.ridx}'
    msg += f'\n   ddr_widx   = {self.ddr_widx.value}'
    exit( msg )
    astid = 0

def recv_messages():
    # read updated write-pointer
    widx = ddr_widx.value
    assert widx < NUM_MSGS  # sanity-check

    if widx == self.ridx:
        return  # nothing to do

    txdata = ''

    # process batch of messages

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

        # sanity-check that message id increments monotonically
        # (i.e. no messages from pru got dropped)
        dropped = ( msg.id - lastid - 1 ) & 0xffffffff
        if dropped != 0:
            raise RuntimeError( f'{dropped} messages dropped by PRU (0x{self.lastid:08x} -> 0x{msg.id:08x})' )
        lastid = msg.id

        # get 32-bit timestamp (in cycles) from message and unwrap it:
        timestamp_cycles += ( msg.timestamp - timestamp_cycles ) & 0xfffffff

        # convert to timestamp in seconds

        force = int(msg.force )
        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_label = f'{timestamp_h:02d}:{timestamp_m:02d}:{timestamp_s:06.3f}'
        time = timestamp_s + timestamp_m*60 + timestamp_h *360
        txmsg = [time, timestamp_label, force, angle]
        txdata += ','.join(map( str, txmsg )) + '\n'
        # or just:  txdata += f'{self.timestamp_cycles},{force},{angle}\n'

        # consume message and update read pointer
        ridx += 1
        if (ridx == NUM_MSGS):
            ridx = 0
        del msg
        shmem.ridx = ridx  # direct access to message forbidden beyond this point

        # send data to client
        writer.write( txdata.encode('ascii') )

        # TODO maybe bail out with an error if too much data buffered in the writer?
        # you can check with writer.get_write_buffer_size()

        print( f'\ridx=0x{self.ridx:04x} id=0x{self.lastid:08x} time={timestamp_label} angle={angle:5.2f} position= {position:08d} force={force:08d}', end='', flush=True )

core_1.load('pid_control.out')
# 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 - c_sizeof( uint16 ) ) // c_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( uint16, MSGBUF_OFFSET + c_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 = c_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


#loading wave profile
with open('./under_double_hump.csv','r') as f:
    signal_reader = csv.reader(f)
    for line in signal_reader:
        shmem.signal[line_count] = int(line[0])
        line_count += 1
print(f' Loaded {line_count} lines in the file')

#open file for writing
data_file = open('run_data.csv', 'w', newline='')
writer = csv.writer(data_file)
header = ['timestamp', 'force_g', 'voltage','signal']
writer.writerow(header)
# ready, set, go!

print('lauching core 1')
pruss.ecap.pwm.initialize( 2**32 )
core_1.run()
sleep(.00001)
try:
    while (self.stop_active_run == False and self.shut_down == False):
        recv_messages()
        check_core()
        sleep(0)

except KeyboardInterrupt:
    pass

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