import os
import sys

sys.path.insert(
    1,
    os.path.abspath(
        os.path.join(os.path.dirname(__file__), "..", "serial_communication")
    ),
)

from pump import pump_liquid


def cup_detect():
    # insert actual cup detection
    return True


def rotate_table():
    # rotates table
    return


def machine_make_drink(drink_orders, serial):
    standardized_drink_orders = standardize(drink_orders)
    scheduled_drink_orders = schedule_drink_orders(
        standardized_drink_orders, len(drink_orders)
    )
    print("Scheduled drink orders")
    print(scheduled_drink_orders)
    pump_liquid(scheduled_drink_orders, serial)
    # call robin's funciton to pump (takes in scheduled_drink_orders as input)
    # call robin's function to stir
    return


num_pumps = 5
max_num_drinks = 6


def standardize(drink_orders):
    # standarize drink amount to 100ml
    drink_array = []
    for drink in drink_orders:
        standardized_drink = []
        total_drink_amt = sum(drink[:-1])
        for i in range(len(drink) - 1):
            if total_drink_amt == 0:
                standardized_drink.append(0)
            else:
                standardized_drink.append(int(drink[i] / total_drink_amt * 100.0))
        drink_array.append(standardized_drink)
    return drink_array


# drink_orders: dimension (# drinks, # pumps)
# return array, where each row corresponds to a timestep and consists of tuples (pump, amount)
def schedule_drink_orders(drink_orders, num_drinks):
    # pad drink_orders with 0s so that number of rows is 6
    for i in range(max_num_drinks - num_drinks):
        drink_orders.append([0] * (num_pumps + 1))

    drink_orders = [order + [0] for order in drink_orders]
    pumps_schedule = [
        list(range(num_pumps)) + [-1] for _ in range(max_num_drinks)
    ]  # -1 indicates "stir" station

    print(drink_orders)

    # array, where each column corresponds to a timestep and consists of tuples (pump, amount)
    result = []
    for i in range(max_num_drinks):
        row = []
        # left rotate by i
        rotated_drink_order = drink_orders[i][i:] + drink_orders[i][:i]
        rotated_pump_schedule = pumps_schedule[i][i:] + pumps_schedule[i][:i]
        for j in range(num_pumps + 1):
            row.append((rotated_pump_schedule[j], rotated_drink_order[j]))
        result.append(row)

    # transpose the array
    result = [list(col) for col in zip(*result)]

    # sort each row by drink amount
    result = [sorted(row, key=lambda x: (x[1], x[0]))[1:] for row in result]

    for i, row in enumerate(result):
        new_row = []
        for pump, amt in row:
            new_row.append(pump)
            new_row.append(amt)
        result[i] = new_row

    return result


# machine_make_drinks([[10, 20, 20, 40, 10], [30, 40, 0, 0, 30], [20, 10, 10, 20, 40], [50, 0, 0, 0, 50], [0, 30, 30, 30, 10], [80, 20, 0, 0, 0]], serial)
# machine_make_drinks([[10, 20, 20, 40, 10], [30, 40, 0, 0, 30], [20, 10, 10, 20, 40], [50, 0, 0, 0, 50], [0, 30, 30, 30, 10], [80, 20, 0, 0, 0]], serial)