#!/usr/bin/env python3
# SPDX-License-Identifier: MIT
# SPDX-FileCopyrightText: 2026 Martino Ferrari
"""A stand-in for the MARTe2 UDPSender datasource, for the flatudp source's self-check.

One datagram is one cycle: the signals concatenated in declaration order, no padding, in the
chosen byte order. Packet i carries, for the signal of index k (from 0) and its element e,
the value (i * (k + 1) + e) % 100, so consecutive datagrams are predictable.

  --signals     name:type[count],...   default Trigger:uint8,Ia:float32,Cnt:uint64,Wave:int16[4]
  --rate        cycles per second
  --big-endian  pack big endian (default little)
  --gap S,D     send nothing from S to S+D seconds after the start (a sender that stops)
  --bad-every S one datagram one byte too long every S seconds (the wrong-size path)
  --multicast G --mcast-if A   send to group G out of interface A instead of --host
  --log FILE    one line per event
"""
import argparse
import socket
import struct
import sys
import time

CODES = {"uint8": "B", "int8": "b", "uint16": "H", "int16": "h", "uint32": "I", "int32": "i",
         "uint64": "Q", "int64": "q", "float32": "f", "float64": "d"}


def parse_signals(text):
    out = []
    for tok in text.split(","):
        name, _, ty = tok.strip().partition(":")
        count = 1
        if "[" in ty:
            ty, _, n = ty.partition("[")
            count = int(n.rstrip("]"))
        out.append((name.strip(), CODES[ty.strip()], count))
    return out


def make_packer(sigs, big):
    st = struct.Struct((">" if big else "<") + "".join(code * count for _, code, count in sigs))

    def pack(i):
        vals = []
        for k, (_, code, count) in enumerate(sigs):
            for e in range(count):
                v = (i * (k + 1) + e) % 100
                vals.append(float(v) if code in "fd" else v)
        return st.pack(*vals)

    return pack, st.size


def main():
    ap = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawTextHelpFormatter)
    ap.add_argument("--host", default="127.0.0.1")
    ap.add_argument("--port", type=int, default=44488)
    ap.add_argument("--rate", type=float, default=1000.0)
    ap.add_argument("--signals", default="Trigger:uint8,Ia:float32,Cnt:uint64,Wave:int16[4]")
    ap.add_argument("--big-endian", action="store_true")
    ap.add_argument("--gap", default="")
    ap.add_argument("--bad-every", type=float, default=0.0)
    ap.add_argument("--multicast", default="")
    ap.add_argument("--mcast-if", default="127.0.0.1")
    ap.add_argument("--log", default="")
    args = ap.parse_args()

    logf = open(args.log, "w") if args.log else None

    def log(msg):
        if logf:
            logf.write("%.3f %s\n" % (time.monotonic() - t0, msg))
            logf.flush()

    sigs = parse_signals(args.signals)
    pack, size = make_packer(sigs, args.big_endian)
    gap = tuple(float(x) for x in args.gap.split(",")) if args.gap else None

    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    dest = (args.host, args.port)
    if args.multicast:
        dest = (args.multicast, args.port)
        sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 1)
        sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1)
        sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(args.mcast_if))

    period = 1.0 / args.rate
    t0 = time.monotonic()
    log("start %s rate=%g size=%d %s" % (args.signals, args.rate, size, "big" if args.big_endian else "little"))
    next_t = t0
    next_bad = t0 + args.bad_every
    in_gap = False
    i = 0
    try:
        while True:
            now = time.monotonic()
            if now < next_t:
                time.sleep(min(next_t - now, 0.001))
                continue
            elapsed = now - t0
            if gap and gap[0] <= elapsed < gap[0] + gap[1]:
                if not in_gap:
                    in_gap = True
                    log("gap begins")
                next_t = now + period
                time.sleep(0.01)
                continue
            if in_gap:
                in_gap = False
                log("gap ends")
            if args.bad_every > 0 and now >= next_bad:
                next_bad = now + args.bad_every
                sock.sendto(pack(i) + b"\x00", dest)
                log("bad datagram of %d bytes" % (size + 1))
            burst = 0
            while next_t <= now and burst < 64:  # catch up a late timer, but bounded
                sock.sendto(pack(i), dest)
                i += 1
                next_t += period
                burst += 1
            if next_t < now - 0.5:
                next_t = now
    except KeyboardInterrupt:
        pass
    return 0


if __name__ == "__main__":
    sys.exit(main())