from typing import Union

from spacepackets.ecss.tc import PusTelecommand
from tmtccmd.tmtc import DefaultPusQueueHelper
from tmtccmd.pus.s200_fsfw_mode import pack_mode_data, Mode, Subservice
from tmtccmd.util import ObjectIdU32


def pack_mode_cmd_with_info(
    object_id: Union[ObjectIdU32, bytes],
    mode: Union[int, Mode],
    submode: int,
    q: DefaultPusQueueHelper,
    info: str,
):
    if isinstance(object_id, ObjectIdU32):
        object_id_bytes = object_id.as_bytes
    elif isinstance(object_id, bytes):
        object_id_bytes = object_id
    else:
        raise ValueError("Invalid Object ID type")
    q.add_log_cmd(info)
    mode_data = pack_mode_data(
        object_id=object_id_bytes,
        mode=mode,
        submode=submode,
    )
    q.add_pus_tc(
        PusTelecommand(
            service=200, subservice=Subservice.TC_MODE_COMMAND, app_data=mode_data
        )
    )