#! /usr/bin/env python3
import serial
import argparse

if __name__ == "__main__":
    parser = argparse.ArgumentParser(description="Set the position of the PWM driver.")
    parser.add_argument("x", type=float, help="X coordinate [0 - 1]")
    parser.add_argument("y", type=float, help="Y coordinate [0 - 1]")
    parser.add_argument("pen", type=int, help="pen down (True) or pen up (False)")
    parser.add_argument("-p", "--port", type=str, required=True, help="Serial port")
    args = parser.parse_args()

    # Open the serial port
    ser = serial.Serial(args.port, 115200)

    # Saturate values to [0, 1]
    args.x = int(max(0, min(1, args.x)) * 0xFFFF)
    args.y = int(max(0, min(1, args.y)) * 0xFFFF)
    args.z = int(max(0, min(1, args.pen)))

    # Send the position
    print(f"{args.x},{args.y},{args.z}\n")
    ser.write(f"{args.x},{args.y},{args.z}\n".encode())

    # Close the serial port
    ser.close()