from __future__ import annotations

import time
from datetime import timedelta

from config.definitions import CustomServiceList
from config.object_ids import get_object_ids
from pus_tc.system.tcs import pack_tcs_sys_commands
from tmtccmd.config import TmtcDefinitionWrapper, OpCodeEntry
from tmtccmd.config.tmtc import tmtc_definitions_provider

from tmtccmd.tc import DefaultPusQueueHelper
from tmtccmd.tc.pus_11_tc_sched import (
    generate_time_tagged_cmd,
    generate_enable_tc_sched_cmd,
    generate_reset_tc_sched_cmd,
)
from tmtccmd.tc.pus_3_fsfw_hk import *

import config.object_ids as oids
from pus_tc.system.tcs import OpCodes as TcsOpCodes
from pus_tc.devs.bpx_batt import BpxSetIds
from pus_tc.system.core import SetIds as CoreSetIds
from tmtc.power.common_power import SetIds as GsSetIds
from pus_tc.devs.rad_sensor import SetIds as RadSetIds
from pus_tc.devs.mgms import MgmLis3SetIds as MgmLis3SetIds_0_2
from pus_tc.devs.mgms import MgmRm3100SetIds as MgmRm3100SetIds_1_3
from pus_tc.devs.gyros import AdisGyroSetIds as AdisGyroSetIds_0_2
from pus_tc.devs.gyros import L3gGyroSetIds as L3gGyroSetIds_1_3
from pus_tc.devs.syrlinks_hk_handler import SetIds as SyrlinksSetIds
from pus_tc.devs.gps import SetIds as GpsSetIds
from tmtc.acs.imtq import ImtqSetIds
from pus_tc.devs.sus import SetIds
from pus_tc.devs.star_tracker import SetIds as StrSetIds
from tmtc.acs.reaction_wheels import RwSetIds, rw_speed_up_cmd_consec
from pus_tc.system.controllers import pack_cmd_ctrl_to_off, pack_cmd_ctrl_to_nml
from pus_tc.system.acs import pack_acs_command, pack_sus_cmds
from tmtc.acs.imtq import pack_imtq_test_into, pack_dipole_command
from pus_tc.devs.star_tracker import pack_star_tracker_commands
from tmtc.acs.reaction_wheels import pack_rw_ass_cmds, pack_set_speed_command


class OpCodes:
    """FT: Functional Test"""

    TV_SETUP_TCS_FT_ON = ["s", "tcs-ft-on"]
    RESET_SCHED = ["reset-sched", "rs"]
    HEATER = ["heater"]
    BAT_FT = ["bat-ft"]
    CORE_FT = ["core-ft"]
    PCDU_FT = ["pcdu-ft"]
    RAD_SEN_FT = ["rad-sen-ft"]
    ACS_FT = ["acs-ft"]
    MGT_FT = ["mgt-ft"]
    MGT_FT_DP = ["mgt-ft-dp"]
    SUS_FT = ["sus-ft"]
    STR_FT = ["str-ft"]
    RW_FT_ONE_RW = ["rw-ft-one-rw"]
    RW_FT_TWO_RWS = ["rw-ft-two-rws"]
    SYRLINKS_FT = ["syrlinks-ft"]
    TV_TEARDOWN_TCS_FT_OFF = ["teardown", "tcs-ft-off"]


class KeyAndInfo:
    TV_SETUP_TCS_FT_ON = ["TCS Act.", "TCS functional test activation"]
    RESET_SCHED = ["Reset Sched", "Reset/Clear TC schedule"]
    HEATER = ["Heater", "heater procedure"]
    BAT_FT = ["BPX Battery", "battery functional test"]
    CORE_FT = ["OBC", "OBC functional test"]
    PCDU_FT = ["PCDU", "PCDU functional test"]
    SYRLINKS_FT = ["Syrlinks", "Syrlinks functional test"]
    RAD_SEN_FT = ["Radiation Sensor", "Radiation Sensor functional test"]
    ACS_FT = ["ACS", "ACS functional test"]
    MGT_FT = ["MGT", "MGT functional test"]
    MGT_FT_DP = ["MGT dipole", "MGT functional test with dipole"]
    SUS_FT = ["SUS", "SUS functional test"]
    STR_FT = ["STR", "STR functional test"]
    RW_FT_ONE_RW = ["RW ONE RW", "RW functional test with one RW"]
    RW_FT_TWO_RWS = ["RW TWO RWS", "RW functional test with two RWs"]
    TV_TEARDOWN_TCS_FT_OFF = ["TCS Deact.", "TCS functional test deactivation"]


KAI = KeyAndInfo

PROC_INFO_DICT = {
    KAI.TV_SETUP_TCS_FT_ON[0]: [
        OpCodes.TV_SETUP_TCS_FT_ON,
        KAI.TV_SETUP_TCS_FT_ON[1],
        120.0,
        10.0,
    ],
    KAI.TV_TEARDOWN_TCS_FT_OFF[0]: [
        OpCodes.TV_TEARDOWN_TCS_FT_OFF,
        KAI.TV_TEARDOWN_TCS_FT_OFF[1],
        120.0,
        10.0,
    ],
    KAI.BAT_FT[0]: [OpCodes.BAT_FT, KAI.BAT_FT[1], 120.0, 10.0],
    KAI.CORE_FT[0]: [OpCodes.CORE_FT, KAI.CORE_FT[1], 120.0, 10.0],
    KAI.PCDU_FT[0]: [OpCodes.PCDU_FT, KAI.PCDU_FT[1], 120.0, 10.0],
    KAI.RAD_SEN_FT[0]: [OpCodes.RAD_SEN_FT, KAI.RAD_SEN_FT[1], 120.0, 10.0],
    KAI.ACS_FT[0]: [OpCodes.ACS_FT, KAI.ACS_FT[1], 120.0, 10.0],
    KAI.MGT_FT[0]: [OpCodes.MGT_FT, KAI.MGT_FT[1], 120.0, 10.0],
    # collection_time for KAI.MGT_FT_DP maybe be reduced as a full 120
    # seconds is not needed after MGTs are tested
    KAI.MGT_FT_DP[0]: [OpCodes.MGT_FT_DP, KAI.MGT_FT_DP[1], 10.0, 10.0],
    KAI.SUS_FT[0]: [OpCodes.SUS_FT, KAI.SUS_FT[1], 120.0, 10.0],
    KAI.STR_FT[0]: [OpCodes.STR_FT, KAI.STR_FT[1], 120.0, 10.0],
    # collection_time for KAI.RW_FT_ONE_RW maybe be reduced as a full 120
    # seconds is not needed after RWs are tested
    KAI.RW_FT_ONE_RW[0]: [OpCodes.RW_FT_ONE_RW, KAI.RW_FT_ONE_RW[1], 10.0, 1.0],
    # collection_time for KAI.RW_FT_ONE_RW maybe be reduced as a full 120
    # seconds is not needed after RWs are tested
    KAI.RW_FT_TWO_RWS[0]: [OpCodes.RW_FT_TWO_RWS, KAI.RW_FT_TWO_RWS[1], 10.0, 1.0],
}


class GenericHkListeningCfg:
    def __init__(self, mgt: bool = False, one_rw: bool = False, two_rws: bool = False):
        self.use_tc_sched = False
        self.mgt = mgt
        self.one_rw = one_rw
        self.two_rws = two_rws

    @classmethod
    def default(cls) -> GenericHkListeningCfg:
        return GenericHkListeningCfg(False, False, False)


def generic_print(q: DefaultPusQueueHelper, info: dict):
    q.add_log_cmd(f"Executing {info[1]} Procedure (OpCodes: {info[0]})")


@tmtc_definitions_provider
def add_proc_cmds(defs: TmtcDefinitionWrapper):
    oce = OpCodeEntry()
    for proc_entry in PROC_INFO_DICT.values():
        oce.add(keys=proc_entry[0], info=proc_entry[1])
    defs.add_service(
        name=CustomServiceList.PROCEDURE.value,
        info="TV Test Procedures",
        op_code_entry=oce,
    )


def pack_generic_hk_listening_cmds(
    q: DefaultPusQueueHelper,
    proc_key: str,
    sid_list: list[bytearray],
    diag_list: list[bool],
    cfg: GenericHkListeningCfg,
):
    info = PROC_INFO_DICT[proc_key]
    collection_time = info[2]
    generic_print(q=q, info=info)

    for i in range(len(sid_list)):
        enable_listen_to_hk_for_x_seconds(
            diag=diag_list[i],
            q=q,
            device=proc_key,
            sid=sid_list[i],
            interval_seconds=info[3],
        )
    if not cfg.use_tc_sched:
        q.add_wait_seconds(2.0)

    if cfg.mgt:
        activate_mgts_alternately(q)

    elif cfg.one_rw:
        activate_all_rws_in_sequence(
            q=q, test_speed=20000, test_ramp_time=10000, init_ssc=0
        )
    elif cfg.two_rws:
        activate_all_rws_two_consecutively(q=q)
    else:
        pass

    if not cfg.use_tc_sched:
        q.add_wait_seconds(collection_time)
    disable_cmd_list = []
    for i in range(len(sid_list)):
        disable_cmd_list.append(
            gen_disable_listen_to_hk_for_x_seconds(
                diag=diag_list[i],
                q=q,
                device=proc_key,
                sid=sid_list[i],
            )
        )
    if cfg.one_rw or cfg.two_rws:
        activate_all_rws_in_sequence(q=q, test_speed=0, test_ramp_time=5000, init_ssc=0)
    q.add_wait_seconds(60.0)
    current_time = time.time()
    current_time += collection_time
    if not cfg.use_tc_sched:
        for cmd in disable_cmd_list:
            q.add_pus_tc(cmd)
    else:
        for cmd in disable_cmd_list:
            q.add_pus_tc(
                generate_time_tagged_cmd(
                    release_time=struct.pack("!I", int(current_time)), tc_to_insert=cmd
                )
            )

    if not cfg.use_tc_sched:
        q.add_wait_seconds(60.0)
    sid_list.clear()
    diag_list.clear()


def pack_proc_commands(q: DefaultPusQueueHelper, op_code: str):
    sid_list = []
    obj_id_dict = get_object_ids()
    if op_code in OpCodes.RESET_SCHED:
        q.add_log_cmd("Resetting/Clearing TC schedule")
        q.add_pus_tc(generate_reset_tc_sched_cmd())
    if op_code in OpCodes.BAT_FT:
        key = KAI.BAT_FT[0]
        sid_list.append(make_sid(oids.BPX_HANDLER_ID, BpxSetIds.GET_HK_SET))
        diag_list = [False]
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg.default(),
        )

    if op_code in OpCodes.CORE_FT:
        key = KAI.CORE_FT[0]
        sid_list.append(make_sid(oids.CORE_CONTROLLER_ID, CoreSetIds.HK))
        diag_list = [False]
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg.default(),
        )

    if op_code in OpCodes.PCDU_FT:
        key = KAI.PCDU_FT[0]
        pcdu_pairs = [
            (oids.P60_DOCK_HANDLER, GsSetIds.CORE),
            (oids.PDU_1_HANDLER_ID, GsSetIds.CORE),
            (oids.PDU_2_HANDLER_ID, GsSetIds.CORE),
            (oids.ACU_HANDLER_ID, GsSetIds.CORE),
            (oids.P60_DOCK_HANDLER, GsSetIds.AUX),
            (oids.PDU_1_HANDLER_ID, GsSetIds.AUX),
            (oids.PDU_2_HANDLER_ID, GsSetIds.AUX),
            (oids.ACU_HANDLER_ID, GsSetIds.AUX),
        ]

        diag_list = [
            False,
            True,
            True,
            True,
            False,
            False,
            False,
            False,
        ]

        for pcdu_dev in pcdu_pairs:
            oid = pcdu_dev[0]
            set_id = pcdu_dev[1]
            sid_list.append(make_sid(oid, set_id))
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg.default(),
        )

    if op_code in OpCodes.RAD_SEN_FT:
        key = KAI.RAD_SEN_FT[0]
        sid_list.append(make_sid(oids.RAD_SENSOR_ID, RadSetIds.HK))
        diag_list = [False]
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg.default(),
        )

    if op_code in OpCodes.TV_SETUP_TCS_FT_ON:
        # Enable scheduling
        q.add_pus_tc(generate_enable_tc_sched_cmd())
        # check whether tcs_assembly also has to be commanded to NORMAL Mode
        pack_tcs_sys_commands(q=q, op_code=TcsOpCodes.TCS_BOARD_ASS_NORMAL[0])
        pack_cmd_ctrl_to_nml(q=q, object_id=obj_id_dict.get(oids.THERMAL_CONTROLLER_ID))

    if op_code in OpCodes.TV_TEARDOWN_TCS_FT_OFF:
        # TCS board should always be on anyway, do not command it off here
        pack_cmd_ctrl_to_off(q=q, object_id=obj_id_dict.get(oids.THERMAL_CONTROLLER_ID))

    if op_code in OpCodes.ACS_FT:
        key = KAI.ACS_FT[0]
        a_side_pairs = [
            (oids.MGM_0_LIS3_HANDLER_ID, MgmLis3SetIds_0_2.CORE_HK),
            (oids.MGM_1_RM3100_HANDLER_ID, MgmRm3100SetIds_1_3.CORE_HK),
            (oids.GYRO_0_ADIS_HANDLER_ID, AdisGyroSetIds_0_2.CORE_HK),
            (oids.GYRO_1_L3G_HANDLER_ID, L3gGyroSetIds_1_3.CORE_HK),
            (oids.GPS_CONTROLLER, GpsSetIds.HK),
        ]
        b_side_pairs = [
            (oids.MGM_2_LIS3_HANDLER_ID, MgmLis3SetIds_0_2.CORE_HK),
            (oids.MGM_3_RM3100_HANDLER_ID, MgmRm3100SetIds_1_3.CORE_HK),
            (oids.GYRO_2_ADIS_HANDLER_ID, AdisGyroSetIds_0_2.CORE_HK),
            (oids.GYRO_3_L3G_HANDLER_ID, L3gGyroSetIds_1_3.CORE_HK),
            (oids.GPS_CONTROLLER, GpsSetIds.HK),
        ]
        d_side_pairs = a_side_pairs + b_side_pairs
        diag_list = [False, False, True, False, False]
        pack_acs_command(q=q, op_code="acs-a")

        for a_side_dev in a_side_pairs:
            oid = a_side_dev[0]
            set_id = a_side_dev[1]
            sid_list.append(make_sid(oid, set_id))
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg.default(),
        )

        pack_acs_command(q=q, op_code="acs-off")
        q.add_wait_seconds(5.0)
        pack_acs_command(q=q, op_code="acs-b")

        sid_list.clear()
        diag_list = [False, False, True, False, False]

        for b_side_dev in b_side_pairs:
            oid = b_side_dev[0]
            set_id = b_side_dev[1]
            sid_list.append(make_sid(oid, set_id))
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg.default(),
        )

        pack_acs_command(q=q, op_code="acs-off")
        q.add_wait_seconds(5.0)
        pack_acs_command(q=q, op_code="acs-d")

        sid_list.clear()

        for d_side_dev in d_side_pairs:
            oid = d_side_dev[0]
            set_id = d_side_dev[1]
            sid_list.append(make_sid(oid, set_id))
        diag_list = [
            False,
            False,
            True,
            False,
            False,
            False,
            False,
            True,
            False,
            False,
        ]
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg.default(),
        )

        pack_acs_command(q=q, op_code="acs-off")

    if op_code in OpCodes.MGT_FT:
        key = KAI.MGT_FT[0]
        imtq_pairs = [
            (oids.IMTQ_HANDLER_ID, ImtqSetIds.ENG_HK_SET),
            (oids.IMTQ_HANDLER_ID, ImtqSetIds.CAL_MTM_SET),
            (oids.IMTQ_HANDLER_ID, ImtqSetIds.RAW_MTM_SET),
        ]
        diag_list = [
            True,
            True,
            True,
        ]

        # Command MGT to mode on
        pack_imtq_test_into(oids.IMTQ_HANDLER_ID, q=q, op_code="1")
        q.add_wait_seconds(5.0)
        # Command MGT to normal mode
        pack_imtq_test_into(oids.IMTQ_HANDLER_ID, q=q, op_code="2")

        for imtq_dev in imtq_pairs:
            oid = imtq_dev[0]
            set_id = imtq_dev[1]
            sid_list.append(make_sid(oid, set_id))
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg.default(),
        )

        pack_imtq_test_into(oids.IMTQ_HANDLER_ID, q=q, op_code="0")

    if op_code in OpCodes.MGT_FT_DP:
        key = KAI.MGT_FT_DP[0]
        a_side_pairs = [
            (oids.MGM_0_LIS3_HANDLER_ID, MgmLis3SetIds_0_2.CORE_HK),
            (oids.MGM_1_RM3100_HANDLER_ID, MgmRm3100SetIds_1_3.CORE_HK),
            (oids.GYRO_0_ADIS_HANDLER_ID, AdisGyroSetIds_0_2.CORE_HK),
            (oids.GYRO_1_L3G_HANDLER_ID, L3gGyroSetIds_1_3.CORE_HK),
            (oids.GPS_CONTROLLER, GpsSetIds.HK),
        ]
        b_side_pairs = [
            (oids.MGM_2_LIS3_HANDLER_ID, MgmLis3SetIds_0_2.CORE_HK),
            (oids.MGM_3_RM3100_HANDLER_ID, MgmRm3100SetIds_1_3.CORE_HK),
            (oids.GYRO_2_ADIS_HANDLER_ID, AdisGyroSetIds_0_2.CORE_HK),
            (oids.GYRO_3_L3G_HANDLER_ID, L3gGyroSetIds_1_3.CORE_HK),
            (oids.GPS_CONTROLLER, GpsSetIds.HK),
        ]
        imtq_pairs = [
            (oids.IMTQ_HANDLER_ID, ImtqSetIds.ENG_HK_SET),
            (oids.IMTQ_HANDLER_ID, ImtqSetIds.CAL_MTM_SET),
            (oids.IMTQ_HANDLER_ID, ImtqSetIds.RAW_MTM_SET),
        ]
        d_side_and_imtq_pairs = a_side_pairs + b_side_pairs + imtq_pairs
        diag_list = [
            False,
            False,
            True,
            False,
            False,
            False,
            False,
            True,
            False,
            False,
            True,
            True,
            True,
        ]
        pack_acs_command(q=q, op_code="acs-d")
        # Command MGT to mode on
        pack_imtq_test_into(oids.IMTQ_HANDLER_ID, q=q, op_code="1")
        q.add_wait_seconds(20.0)
        # Command MGT to normal mode
        pack_imtq_test_into(oids.IMTQ_HANDLER_ID, q=q, op_code="2")

        for d_side_and_imtq_dev in d_side_and_imtq_pairs:
            oid = d_side_and_imtq_dev[0]
            set_id = d_side_and_imtq_dev[1]
            sid_list.append(make_sid(oid, set_id))

        cfg = GenericHkListeningCfg.default()
        cfg.mgt = True
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=cfg,
        )

        pack_imtq_test_into(oids.IMTQ_HANDLER_ID, q=q, op_code="0")
        pack_acs_command(q=q, op_code="acs-off")

    if op_code in OpCodes.SUS_FT:
        key = KAI.SUS_FT[0]

        pack_sus_cmds(q=q, op_code="sus-nom")

        sus_n_ids = [
            oids.SUS_0_N_LOC_XFYFZM_PT_XF,
            oids.SUS_1_N_LOC_XBYFZM_PT_XB,
            oids.SUS_2_N_LOC_XFYBZB_PT_YB,
            oids.SUS_3_N_LOC_XFYBZF_PT_YF,
            oids.SUS_4_N_LOC_XMYFZF_PT_ZF,
            oids.SUS_5_N_LOC_XFYMZB_PT_ZB,
        ]
        sus_r_ids = [
            oids.SUS_6_R_LOC_XFYBZM_PT_XF,
            oids.SUS_7_R_LOC_XBYBZM_PT_XB,
            oids.SUS_8_R_LOC_XBYBZB_PT_YB,
            oids.SUS_9_R_LOC_XBYBZB_PT_YF,
            oids.SUS_10_R_LOC_XMYBZF_PT_ZF,
            oids.SUS_11_R_LOC_XBYMZB_PT_ZB,
        ]
        diag_list = [
            True,
            True,
            True,
            True,
            True,
            True,
        ]

        # SUSs
        for nom_sus in sus_n_ids:
            sid_list.append(make_sid(nom_sus, SetIds.HK))
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg.default(),
        )

        pack_acs_command(q=q, op_code="sus-off")
        q.add_wait_seconds(5.0)
        pack_sus_cmds(q=q, op_code="sus-red")

        diag_list = [
            True,
            True,
            True,
            True,
            True,
            True,
        ]

        # SUSs
        for red_sus in sus_r_ids:
            sid_list.append(make_sid(red_sus, SetIds.HK))
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg.default(),
        )

        pack_acs_command(q=q, op_code="sus-off")
        q.add_wait_seconds(5.0)
        pack_sus_cmds(q=q, op_code="sus-d")

        # SUSs
        for nom_sus in sus_n_ids:
            sid_list.append(make_sid(nom_sus, SetIds.HK))
        for red_sus in sus_r_ids:
            sid_list.append(make_sid(red_sus, SetIds.HK))
        diag_list = [
            True,
            True,
            True,
            True,
            True,
            True,
            True,
            True,
            True,
            True,
            True,
            True,
        ]
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg.default(),
        )

        pack_acs_command(q=q, op_code="sus-off")

    if op_code in OpCodes.SYRLINKS_FT:
        key = KAI.SYRLINKS_FT[0]
        sid_list = [
            make_sid(oids.SYRLINKS_HANDLER_ID, SyrlinksSetIds.RX_REGISTERS_DATASET),
            make_sid(oids.SYRLINKS_HANDLER_ID, SyrlinksSetIds.TX_REGISTERS_DATASET),
        ]
        # HK listening
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=[False],
            cfg=GenericHkListeningCfg.default(),
        )
    if op_code in OpCodes.STR_FT:
        key = KAI.STR_FT[0]

        pack_star_tracker_commands(object_id=oids.STAR_TRACKER_ID, q=q, op_code="2")

        # STR
        sid_list.append(make_sid(oids.STAR_TRACKER_ID, StrSetIds.TEMPERATURE))
        diag_list = [False]
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg.default(),
        )

        pack_star_tracker_commands(object_id=oids.STAR_TRACKER_ID, q=q, op_code="3")

    if op_code in OpCodes.RW_FT_ONE_RW:
        key = KAI.RW_FT_ONE_RW[0]
        rw_pairs = [
            (oids.RW1_ID, RwSetIds.STATUS_SET_ID),
            (oids.RW1_ID, RwSetIds.LAST_RESET),
            (oids.RW1_ID, RwSetIds.TM_SET),
            (oids.RW2_ID, RwSetIds.STATUS_SET_ID),
            (oids.RW2_ID, RwSetIds.LAST_RESET),
            (oids.RW2_ID, RwSetIds.TM_SET),
            (oids.RW3_ID, RwSetIds.STATUS_SET_ID),
            (oids.RW3_ID, RwSetIds.LAST_RESET),
            (oids.RW3_ID, RwSetIds.TM_SET),
            (oids.RW4_ID, RwSetIds.STATUS_SET_ID),
            (oids.RW4_ID, RwSetIds.LAST_RESET),
            (oids.RW4_ID, RwSetIds.TM_SET),
        ]
        diag_list = [
            True,
            False,
            False,
            True,
            False,
            False,
            True,
            False,
            False,
            True,
            False,
            False,
        ]
        # RW NORMAL
        pack_rw_ass_cmds(object_id=oids.RW_ASSEMBLY, q=q, op_code="nml")

        # RW HK für alle RWs nur einzeln
        for rw_dev in rw_pairs:
            oid = rw_dev[0]
            set_id = rw_dev[1]
            sid_list.append(make_sid(oid, set_id))
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg(mgt=False, one_rw=True, two_rws=False),
        )
        # RW OFF
        pack_rw_ass_cmds(object_id=oids.RW_ASSEMBLY, q=q, op_code="off")

    # ass command with 2 rws to speed
    if op_code in OpCodes.RW_FT_TWO_RWS:
        key = KAI.RW_FT_TWO_RWS[0]
        rw_pairs = [
            (oids.RW1_ID, RwSetIds.STATUS_SET_ID),
            (oids.RW1_ID, RwSetIds.TEMPERATURE_SET_ID),
            (oids.RW1_ID, RwSetIds.LAST_RESET),
            (oids.RW1_ID, RwSetIds.TM_SET),
            (oids.RW2_ID, RwSetIds.STATUS_SET_ID),
            (oids.RW2_ID, RwSetIds.TEMPERATURE_SET_ID),
            (oids.RW2_ID, RwSetIds.LAST_RESET),
            (oids.RW2_ID, RwSetIds.TM_SET),
            (oids.RW3_ID, RwSetIds.STATUS_SET_ID),
            (oids.RW3_ID, RwSetIds.TEMPERATURE_SET_ID),
            (oids.RW3_ID, RwSetIds.LAST_RESET),
            (oids.RW3_ID, RwSetIds.TM_SET),
            (oids.RW4_ID, RwSetIds.STATUS_SET_ID),
            (oids.RW4_ID, RwSetIds.TEMPERATURE_SET_ID),
            (oids.RW4_ID, RwSetIds.LAST_RESET),
            (oids.RW4_ID, RwSetIds.TM_SET),
        ]
        diag_list = [
            False,
            False,
            False,
            False,
            False,
            False,
            False,
            False,
            False,
            False,
            False,
            False,
            False,
            False,
            False,
            False,
        ]
        # RW NORMAL
        pack_rw_ass_cmds(object_id=oids.RW_ASSEMBLY, q=q, op_code="nml")

        # RW
        for rw_dev in rw_pairs:
            oid = rw_dev[0]
            set_id = rw_dev[1]
            sid_list.append(make_sid(oid, set_id))
        pack_generic_hk_listening_cmds(
            q=q,
            proc_key=key,
            sid_list=sid_list,
            diag_list=diag_list,
            cfg=GenericHkListeningCfg(mgt=False, one_rw=False, two_rws=True),
        )
        # RW OFF
        pack_rw_ass_cmds(object_id=oids.RW_ASSEMBLY, q=q, op_code="off")


def enable_listen_to_hk_for_x_seconds(
    q: DefaultPusQueueHelper,
    diag: bool,
    device: str,
    sid: bytes,
    interval_seconds: float,
):
    q.add_log_cmd(f"Enabling periodic HK for {device}")
    cmd_tuple = enable_periodic_hk_command_with_interval(
        diag=diag, sid=sid, interval_seconds=interval_seconds
    )
    for cmd in cmd_tuple:
        q.add_pus_tc(cmd)
    generate_time_tagged_cmd(
        struct.pack("!I", int(round(time.time() + interval_seconds))),
        gen_disable_listen_to_hk_for_x_seconds(q, diag, device, sid),
    )


def gen_disable_listen_to_hk_for_x_seconds(
    q: DefaultPusQueueHelper,
    diag: bool,
    device: str,
    sid: bytes,
) -> PusTelecommand:
    q.add_log_cmd(f"Disabling periodic HK for {device}")
    return disable_periodic_hk_command(diag=diag, sid=sid)


def activate_mgts_alternately(
    q: DefaultPusQueueHelper,
):

    q.add_pus_tc(
        pack_dipole_command(
            object_id=oids.IMTQ_HANDLER_ID,
            x_dipole=2000,
            y_dipole=0,
            z_dipole=0,
            duration=30000,
        )
    )
    q.add_wait_seconds(40.0)

    q.add_pus_tc(
        pack_dipole_command(
            object_id=oids.IMTQ_HANDLER_ID,
            x_dipole=-2000,
            y_dipole=0,
            z_dipole=0,
            duration=30000,
        )
    )
    q.add_wait_seconds(40.0)

    q.add_pus_tc(
        pack_dipole_command(
            object_id=oids.IMTQ_HANDLER_ID,
            x_dipole=0,
            y_dipole=2000,
            z_dipole=0,
            duration=30000,
        )
    )
    q.add_wait_seconds(40.0)

    q.add_pus_tc(
        pack_dipole_command(
            object_id=oids.IMTQ_HANDLER_ID,
            x_dipole=0,
            y_dipole=-2000,
            z_dipole=0,
            duration=30000,
        )
    )
    q.add_wait_seconds(40.0)

    q.add_pus_tc(
        pack_dipole_command(
            object_id=oids.IMTQ_HANDLER_ID,
            x_dipole=0,
            y_dipole=0,
            z_dipole=2000,
            duration=30000,
        )
    )
    q.add_wait_seconds(40.0)

    q.add_pus_tc(
        pack_dipole_command(
            object_id=oids.IMTQ_HANDLER_ID,
            x_dipole=0,
            y_dipole=0,
            z_dipole=-2000,
            duration=30000,
        )
    )
    q.add_wait_seconds(40.0)


def rw_speed_cmd_single(
    q: DefaultPusQueueHelper, oid: bytes, speed: int, ramp_time: int
):
    q.add_pus_tc(
        pack_set_speed_command(object_id=oid, speed=speed, ramp_time_ms=ramp_time)
    )
    q.add_wait(timedelta(seconds=70.0))
    q.add_pus_tc(pack_set_speed_command(object_id=oid, speed=0, ramp_time_ms=ramp_time))


def activate_all_rws_in_sequence(
    q: DefaultPusQueueHelper, init_ssc: int, test_speed: int, test_ramp_time: int
):
    new_ssc = init_ssc
    # RW1 speed cmd
    q.add_wait(timedelta(seconds=58.0))
    rw_speed_cmd_single(q, oids.RW1_ID, test_speed, test_ramp_time)
    q.add_wait_seconds(30.0)
    rw_speed_cmd_single(q, oids.RW2_ID, test_speed, test_ramp_time)
    q.add_wait_seconds(30.0)
    rw_speed_cmd_single(q, oids.RW3_ID, test_speed, test_ramp_time)
    q.add_wait_seconds(30.0)
    rw_speed_cmd_single(q, oids.RW4_ID, test_speed, test_ramp_time)
    q.add_wait_seconds(30.0)
    return new_ssc


def activate_all_rws_two_consecutively(q: DefaultPusQueueHelper):
    # RW1+3 speed cmd
    q.add_wait_seconds(5.0)
    rw_speed_up_cmd_consec(q, [oids.RW1_ID, oids.RW3_ID], -20000, 10000)
    q.add_wait_seconds(70.0)
    rw_speed_down_cmd_consec(q, [oids.RW1_ID, oids.RW3_ID], 10000)
    q.add_wait_seconds(15.0)
    # RW2+4 speed cmd
    rw_speed_up_cmd_consec(q, [oids.RW2_ID, oids.RW4_ID], -20000, 10000)
    q.add_wait_seconds(70.0)
    rw_speed_down_cmd_consec(q, [oids.RW2_ID, oids.RW4_ID], 10000)
    q.add_wait_seconds(15.0)