from typing import Optional

from config.definitions import CustomServiceList
from spacepackets.ecss import PusTelecommand
from tmtccmd.config import (
    ServiceOpCodeDictT,
    add_op_code_entry,
    add_service_op_code_entry,
)
from tmtccmd.tc.definitions import TcQueueT
from tmtccmd.utility import ObjectId
from tmtccmd.tc.pus_200_fsfw_modes import Modes, pack_mode_data, Subservices
import config.object_ids as oids
from config.object_ids import get_object_ids

RTD_IDS = [
    oids.RTD_0_PLOC_HSPD,
    oids.RTD_1_PLOC_MISSIONBRD,
    oids.RTD_2_4K_CAM,
    oids.RTD_3_DAC_HSPD,
    oids.RTD_4_STR,
    oids.RTD_5_RW1_MX_MY,
    oids.RTD_6_DRO,
    oids.RTD_7_SCEX,
    oids.RTD_8_X8,
    oids.RTD_9_HPA,
    oids.RTD_10_PL_TX,
    oids.RTD_11_MPA,
    oids.RTD_12_ACU,
    oids.RTD_13_PLPCDU_HSPD,
    oids.RTD_14_TCS_BRD,
    oids.RTD_15_IMTQ,
]


class OpCodes:
    ON = ["0", "on"]
    OFF = ["1", "off"]
    NORMAL = ["2", "normal"]


class Info:
    ON = "Switch handler on"
    OFF = "Switch handler off"
    NORMAL = "Switch handler normal"


def specify_rtd_cmds(cmd_dict: ServiceOpCodeDictT):
    op_code_dict = dict()
    add_op_code_entry(op_code_dict=op_code_dict, keys=OpCodes.ON, info=Info.ON)
    add_op_code_entry(op_code_dict=op_code_dict, keys=OpCodes.NORMAL, info=Info.NORMAL)
    add_op_code_entry(op_code_dict=op_code_dict, keys=OpCodes.OFF, info=Info.OFF)
    add_service_op_code_entry(
        srv_op_code_dict=cmd_dict,
        op_code_entry=op_code_dict,
        name=CustomServiceList.RTD.value,
        info="RTD commands",
    )


def pack_rtd_commands(op_code: str, object_id: Optional[ObjectId], tc_queue: TcQueueT):
    if object_id is not None and object_id not in RTD_IDS:
        print("Specified object ID not a valid RTD ID")
        object_id = None
    if object_id is None:
        tgt_rtd_idx = prompt_rtd_idx()
        object_id_dict = get_object_ids()
        object_id = object_id_dict.get(RTD_IDS[tgt_rtd_idx])
    if op_code in OpCodes.ON:
        app_data = pack_mode_data(
            object_id=object_id.as_bytes, mode=Modes.ON, submode=0
        )
        cmd = PusTelecommand(
            service=200, subservice=Subservices.TC_MODE_COMMAND, app_data=app_data
        )
        tc_queue.appendleft(cmd.pack_command_tuple())
    if op_code in OpCodes.NORMAL:
        app_data = pack_mode_data(
            object_id=object_id.as_bytes, mode=Modes.NORMAL, submode=0
        )
        cmd = PusTelecommand(
            service=200, subservice=Subservices.TC_MODE_COMMAND, app_data=app_data
        )
        tc_queue.appendleft(cmd.pack_command_tuple())
    if op_code in OpCodes.OFF:
        app_data = pack_mode_data(
            object_id=object_id.as_bytes, mode=Modes.OFF, submode=0
        )
        cmd = PusTelecommand(
            service=200, subservice=Subservices.TC_MODE_COMMAND, app_data=app_data
        )
        tc_queue.appendleft(cmd.pack_command_tuple())


def prompt_rtd_idx():
    while True:
        rtd_idx = input("Please specify RTD index [0-15]: ")
        if not rtd_idx.isdigit():
            print("Invalid input")
            continue
        rtd_idx = int(rtd_idx)
        if rtd_idx < 0 or rtd_idx > 15:
            print("Invalid device index")
            continue
        return rtd_idx