#!/usr/bin/env python3
"""Send a differential-drive velocity pair to SolarOS over OSC/UDP."""
import argparse
import math
import socket
import struct
import time


def osc_string(value):
    data = value.encode("ascii") + b"\0"
    return data + b"\0" * (-len(data) % 4)


def packet(linear, angular):
    return osc_string("/base/cmd_vel") + osc_string(",ff") + struct.pack(">ff", linear, angular)


def main():
    parser = argparse.ArgumentParser(description=__doc__)
    parser.add_argument("host", help="Robot IPv4 address or hostname")
    parser.add_argument("--port", type=int, default=9000)
    parser.add_argument("--linear", type=float, default=0)
    parser.add_argument("--angular", type=float, default=0)
    parser.add_argument("--rate", type=int, default=20)
    parser.add_argument("--duration", type=float, default=2)
    args = parser.parse_args()
    if not 1 <= args.port <= 65535 or not 1 <= args.rate <= 100:
        parser.error("port must be 1..65535 and rate must be 1..100 Hz")
    if not all(math.isfinite(v) for v in (args.linear, args.angular, args.duration)) or args.duration <= 0:
        parser.error("velocities must be finite and duration must be finite and positive")
    try:
        command = packet(args.linear, args.angular)
    except (OverflowError, struct.error):
        parser.error("velocities must fit OSC float32")
    with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sender:
        sender.connect((args.host, args.port))
        end = time.monotonic() + args.duration
        try:
            while time.monotonic() < end:
                started = time.monotonic()
                sender.send(command)
                time.sleep(max(0, min(1 / args.rate - (time.monotonic() - started), end - time.monotonic())))
        except KeyboardInterrupt:
            pass
        finally:
            sender.send(packet(0, 0))


if __name__ == "__main__":
    main()