#!/bin/env python3

import os
import sys
import mmap
import time
import select
import struct
# import math
# import numpy as np


f = os.open("/dev/mem", os.O_RDWR | os.O_SYNC)

cap = mmap.mmap(f, 0x1000, mmap.MAP_SHARED,
    mmap.PROT_READ | mmap.PROT_WRITE,
    offset=0x40000000)

gen = mmap.mmap(f, 0x1000, mmap.MAP_SHARED,
    mmap.PROT_READ | mmap.PROT_WRITE,
    offset=0x40100000)

mem = mmap.mmap(f, 0x30000000, mmap.MAP_SHARED,
    mmap.PROT_READ | mmap.PROT_WRITE,
    offset=0x10000000)

def oreg_get(m, x):
    v = struct.unpack("<L", m[x*4+0x100:x*4+0x104])[0]
    return v

def oreg_set(m, x, v):
    m[x*4+0x100:x*4+0x104] = struct.pack("<L", v)

def ireg_get(m, x):
    v = struct.unpack("<L", m[x*4+0x0:x*4+0x4])[0]
    return v

def u64_set(m, o, v):
    m[o:o+8] = struct.pack("<Q", v)


u0 = os.open("/dev/uio0", os.O_RDWR | os.O_SYNC)
os.write(u0, (1).to_bytes(4, byteorder='little', signed=False))

u4 = os.open("/dev/uio4", os.O_RDWR | os.O_SYNC)
os.write(u4, (1).to_bytes(4, byteorder='little', signed=False))
# u5 = os.open("/dev/uio5", os.O_RDWR | os.O_SYNC)
# os.write(u5, (1).to_bytes(4, byteorder='little', signed=False))
# u6 = os.open("/dev/uio6", os.O_RDWR | os.O_SYNC)
# os.write(u6, (1).to_bytes(4, byteorder='little', signed=False))

iframe = 0;
oframe = 0;

icnt = 0
ocnt = 0

ibuf = 0
obuf = 0

imax = 0x80 - 1;
omax = 0x40 - 1;

stime = time.time()
ltime = stime

print([u0, u4])

u0v, u4v = 0, 0

oreg_set(cap, 1, 0x00060F82)
oreg_set(cap, 1, 0x00000F82)
oreg_set(cap, 1, 0x00080F82)

while True:
    r, w, e = select.select([u0, u4], [], [], 1.0)

    ctime = time.time()

    if u4 in r:
        u4v = int.from_bytes(os.read(u4, 4), byteorder='little')

        offs = 0x600000 * ibuf

        bufid = ((ibuf << 5) | obuf) & 0xFFF
        magic = (bufid << 16) | ((iframe & 0xFFF) << 28) | \
                (bufid << 40) | ((oframe & 0xFFF) << 52)
        # magic = 0x123456789ABC0000
        u64_set(mem, offs + 0x000, magic)

        stamp = int(ctime * 1024)
        stampu = (stamp >> 24) & 0xFFFFF
        stamp1 = ((stampu << 24) | stampu | 0x800000) << 16
        stampl = stamp & 0xFFFFFF
        stamp2 = ((stampl << 24) | stampl) << 16

        u64_set(mem, offs + 0x008, stamp1)
        u64_set(mem, offs + 0x010, stamp2)

        ibuf = ibuf + 1 if ibuf < imax else 0
        offs = 0x600000 * ibuf

        oreg_set(cap, 0, 0x10000000 + offs) 
        size = (4096*3072*2) >> 2
        oreg_set(cap, 5, 0x10000000 + offs + size) 

        oreg_set(cap, 1, 0x00060F82)
        oreg_set(cap, 1, 0x00000F82)
        oreg_set(cap, 1, 0x00080F82)

        icnt += 1
        iframe += 1

        os.write(u4, (1).to_bytes(4, byteorder='little', signed=False))

    if u0 in r:
        u0v = int.from_bytes(os.read(u0, 4), byteorder='little')

        if (icnt >> 1) > (ocnt + 1):
            obuf = obuf + 1 if obuf < omax else 0
            offs = 0xC00000 * obuf 

            oreg_set(gen, 0, 0x10000000 + offs) 
            size = ((2048*1536*8) >> 1) + 0x400
            oreg_set(gen, 5, 0x10000000 + offs + size) 
            ocnt += 1

        oframe += 1

        os.write(u0, (1).to_bytes(4, byteorder='little', signed=False))

    delta = ctime - stime

    if ctime - ltime < 0.0/25:
        continue
    ltime = ctime

    print("I: {:3d} {:5d} {:6.2f} FPS  "
          "O: {:3d} {:5d} {:6.2f}/{:6.2f} FPS  "
          "FC: {:d} {:d} {:d} {:d} {:15.3f} ".format(
        ibuf, icnt, min(iframe/delta, 999),
        obuf, ocnt, min(oframe/delta, 999),
        min(ocnt/delta, 999),
        iframe, oframe, u0v, u4v, ctime),
        end = '\r')