from typing import Union

from tmtccmd.tc.definitions import TcQueueT, QueueCommands
from spacepackets.ecss.tc import PusTelecommand
from tmtccmd.tc.pus_200_fsfw_modes import pack_mode_data, Modes, Subservices


def command_mode(
    object_id: bytes,
    mode: Union[int, Modes],
    submode: int,
    tc_queue: TcQueueT,
    info: str,
):
    tc_queue.appendleft((QueueCommands.PRINT, info))
    mode_data = pack_mode_data(
        object_id=object_id,
        mode=mode,
        submode=submode,
    )
    cmd = PusTelecommand(
        service=200, subservice=Subservices.TC_MODE_COMMAND, app_data=mode_data
    )
    tc_queue.appendleft(cmd.pack_command_tuple())