import sys, time
import divideCoords, writeSerial, playControl


def playTrack(track_name, coords=None):
    """Play a single .thr file, point by point.

    `coords` is the starting (theta, r). If not given, we ask the device
    for its current position via writeSerial.getPolar() - the firmware
    tracks this itself now, so there's no need to keep a copy in the DB
    anymore (and nothing here writes position to the DB at all).

    Cooperatively checks playControl's command between points, so this can
    be paused/stopped/skipped from the web UI while it's running. Returns
    the ending (theta, r).
    """

    path = "/home/pi/SandTable/media/" + track_name

    if coords is None:
        coords = writeSerial.getPolar()
        if coords is None:
            print("Couldn't read starting position from the device, assuming (0, 0)")
            coords = [0.0, 0.0]
        else:
            coords = list(coords)

    playControl.set_status(playControl.STATUS_PLAYING, current_track=track_name)

    with open(path) as f:
        for line in f:

            # Cooperative pause: block here (not mid-move) until resumed
            while playControl.get_command() == playControl.COMMAND_PAUSE:
                playControl.set_status(playControl.STATUS_PAUSED, current_track=track_name)
                time.sleep(0.1)

            command = playControl.get_command()

            if command == playControl.COMMAND_STOP:
                playControl.set_status(playControl.STATUS_IDLE)
                return coords

            if command == playControl.COMMAND_NEXT:
                playControl.set_command(playControl.COMMAND_PLAY)  # consume the one-shot skip
                return coords

            playControl.set_status(playControl.STATUS_PLAYING, current_track=track_name)

            # If line is comment or line does not contain anything then continue to next line
            if '#' in line or len(line) < 4 or "/" in line:
                continue

            # Getting theta and rho from the line and changing them to float values
            th_str, r_str = line.split(' ')
            th = float(th_str)
            r = float(r_str)

            if r > 1.0:
                print("R Too big")

            # If distance between points is too far (0.03 units), interpolate
            if divideCoords.checkDistance(coords, [th, r], 0.01):
                split_coords = divideCoords.divideBy(coords, [th, r], 0.03)
                if split_coords:
                    for i in split_coords:
                        if not writeSerial.sendPolarWithRetry(i[0], i[1]):
                            print(f"Failed to move to intermediate point {i}, aborting track")
                            return coords
                        coords = [i[0], i[1]]

            # Writing point to the controller and waiting for the move to
            # finish (blocks until COMMAND_POLAR's completion ACK arrives)
            if not writeSerial.sendPolarWithRetry(th, r):
                print(f"Failed to move to ({th}, {r}), aborting track")
                return coords

            coords = [th, r]

    # Delay just in case
    time.sleep(0.1)
    return coords


if __name__ == "__main__":
    if len(sys.argv) > 1:
        name = sys.argv[1]
        playControl.init_control_table()
        playTrack(name)
        playControl.set_status(playControl.STATUS_IDLE)
    else:
        print("Invalid arguments")