# -*- coding: utf-8 -*-
from spacepackets.ecss.tc import PusTelecommand

from tmtccmd.config.definitions import QueueCommands
from tmtccmd.tc.packer import TcQueueT
from tmtccmd.tc.pus_200_fsfw_modes import pack_mode_data, Modes

from common_tmtc.config.object_ids import TEST_DEVICE_0_ID, ASSEMBLY_ID


def pack_service_200_commands_into(tc_queue: TcQueueT, op_code: str):
    if op_code == "test":
        pack_service_200_test_into(tc_queue=tc_queue, init_ssc=2000)
    elif op_code == "asm_to_normal" or op_code == "0":
        # Set Normal mode
        tc_queue.appendleft(
            (QueueCommands.PRINT, "Testing Service 200: Set Mode Normal")
        )
        # Command assembly to normal, submode 1 for dual mode,
        mode_data = pack_mode_data(ASSEMBLY_ID, Modes.NORMAL, 1)
        command = PusTelecommand(service=200, subservice=1, ssc=0, app_data=mode_data)
        tc_queue.appendleft(command.pack_command_tuple())


def pack_service_200_test_into(init_ssc: int, tc_queue: TcQueueT) -> int:
    new_ssc = init_ssc
    tc_queue.appendleft((QueueCommands.PRINT, "Testing Service 200"))
    # Object ID: DUMMY Device
    object_id = TEST_DEVICE_0_ID
    # Set On Mode
    tc_queue.appendleft((QueueCommands.PRINT, "Testing Service 200: Set Mode On"))
    mode_data = pack_mode_data(object_id, Modes.ON, 0)
    command = PusTelecommand(service=200, subservice=1, ssc=new_ssc, app_data=mode_data)
    tc_queue.appendleft(command.pack_command_tuple())
    new_ssc += 1
    # Set Normal mode
    tc_queue.appendleft((QueueCommands.PRINT, "Testing Service 200: Set Mode Normal"))
    mode_data = pack_mode_data(object_id, Modes.NORMAL, 0)
    command = PusTelecommand(service=200, subservice=1, ssc=new_ssc, app_data=mode_data)
    tc_queue.appendleft(command.pack_command_tuple())
    new_ssc += 1
    # Set Raw Mode
    tc_queue.appendleft((QueueCommands.PRINT, "Testing Service 200: Set Mode Raw"))
    mode_data = pack_mode_data(object_id, Modes.RAW, 0)
    command = PusTelecommand(service=200, subservice=1, ssc=new_ssc, app_data=mode_data)
    tc_queue.appendleft(command.pack_command_tuple())
    new_ssc += 1
    # Set Off Mode
    tc_queue.appendleft((QueueCommands.PRINT, "Testing Service 200: Set Mode Off"))
    mode_data = pack_mode_data(object_id, Modes.OFF, 0)
    command = PusTelecommand(service=200, subservice=1, ssc=new_ssc, app_data=mode_data)
    tc_queue.appendleft(command.pack_command_tuple())
    new_ssc += 1
    return new_ssc