# -*- coding: utf-8 -*-
"""
@file   imtq.py
@brief  Tests for the ISIS IMTQ (Magnettorquer) device handler
@author J. Meier
@date   25.03.2021
"""
from tmtccmd.config.definitions import QueueCommands

from tmtccmd.tc.packer import TcQueueT
from spacepackets.ecss.tc import PusTelecommand
from tmtccmd.tc.service_3_housekeeping import make_sid, generate_one_hk_command


class ImtqSetIds:
    ENG_HK_SET = 1
    CAL_MTM_SET = 2
    RAW_MTM_SET = 3
    POSITIVE_X_TEST = 4
    NEGATIVE_X_TEST = 5
    POSITIVE_Y_TEST = 6
    NEGATIVE_Y_TEST = 7
    POSITIVE_Z_TEST = 8
    NEGATIVE_Z_TEST = 9


class ImtqActionIds:
    start_actuation_dipole = bytearray([0x0, 0x0, 0x0, 0x02])
    get_commanded_dipole = bytearray([0x0, 0x0, 0x0, 0x03])
    perform_positive_x_test = bytearray([0x0, 0x0, 0x0, 0x07])
    perform_negative_x_test = bytearray([0x0, 0x0, 0x0, 0x08])
    perform_positive_y_test = bytearray([0x0, 0x0, 0x0, 0x09])
    perform_negative_y_test = bytearray([0x0, 0x0, 0x0, 0x0A])
    perform_positive_z_test = bytearray([0x0, 0x0, 0x0, 0x0B])
    perform_negative_z_test = bytearray([0x0, 0x0, 0x0, 0x0C])
    # Initiates the reading of the last performed self test. After sending this command the results can be downlinked
    # via the housekeeping service by using the appropriate set ids listed above.
    read_self_test_results = bytearray([0x0, 0x0, 0x0, 0x0D])


def pack_imtq_test_into(object_id: bytearray, tc_queue: TcQueueT, op_code: str) -> TcQueueT:
    tc_queue.appendleft(
        (QueueCommands.PRINT,
         "Testing ISIS IMTQ handler with object id: 0x" + object_id.hex())
    )

    if op_code == "0" or op_code == "1":
        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Perform positive x self test"))
        command = object_id + ImtqActionIds.perform_positive_x_test
        command = PusTelecommand(service=8, subservice=128, ssc=22, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Initiate reading of positive x self test results"))
        command = object_id + ImtqActionIds.read_self_test_results
        command = PusTelecommand(service=8, subservice=128, ssc=23, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Request dataset with positive x self test results"))
        sid = make_sid(object_id, ImtqSetIds.POSITIVE_X_TEST)
        command = generate_one_hk_command(sid, 24)
        tc_queue.appendleft(command.pack_command_tuple())

    if op_code == "0" or op_code == "2":
        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Perform negative x self test"))
        command = object_id + ImtqActionIds.perform_negative_x_test
        command = PusTelecommand(service=8, subservice=128, ssc=25, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Initiate reading of negative x self test results"))
        command = object_id + ImtqActionIds.read_self_test_results
        command = PusTelecommand(service=8, subservice=128, ssc=26, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Request dataset with negative x self test results"))
        sid = make_sid(object_id, ImtqSetIds.NEGATIVE_X_TEST)
        command = generate_one_hk_command(sid, 27)
        tc_queue.appendleft(command.pack_command_tuple())

    if op_code == "0" or op_code == "3":
        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Perform positive y self test"))
        command = object_id + ImtqActionIds.perform_positive_y_test
        command = PusTelecommand(service=8, subservice=128, ssc=28, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Initiate reading of positive y self test results"))
        command = object_id + ImtqActionIds.read_self_test_results
        command = PusTelecommand(service=8, subservice=128, ssc=29, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Request dataset with positive y self test results"))
        sid = make_sid(object_id, ImtqSetIds.POSITIVE_Y_TEST)
        command = generate_one_hk_command(sid, 30)
        tc_queue.appendleft(command.pack_command_tuple())

    if op_code == "0" or op_code == "4":
        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Perform negative y self test"))
        command = object_id + ImtqActionIds.perform_negative_y_test
        command = PusTelecommand(service=8, subservice=128, ssc=31, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Initiate reading of negative y self test results"))
        command = object_id + ImtqActionIds.read_self_test_results
        command = PusTelecommand(service=8, subservice=128, ssc=32, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Request dataset with negative y self test results"))
        sid = make_sid(object_id, ImtqSetIds.NEGATIVE_Y_TEST)
        command = generate_one_hk_command(sid, 33)
        tc_queue.appendleft(command.pack_command_tuple())

    if op_code == "0" or op_code == "5":
        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Perform positive z self test"))
        command = object_id + ImtqActionIds.perform_positive_z_test
        command = PusTelecommand(service=8, subservice=128, ssc=34, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Initiate reading of positive z self test results"))
        command = object_id + ImtqActionIds.read_self_test_results
        command = PusTelecommand(service=8, subservice=128, ssc=35, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Request dataset with positive z self test results"))
        sid = make_sid(object_id, ImtqSetIds.POSITIVE_Y_TEST)
        command = generate_one_hk_command(sid, 36)
        tc_queue.appendleft(command.pack_command_tuple())

    if op_code == "0" or op_code == "6":
        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Perform negative z self test"))
        command = object_id + ImtqActionIds.perform_negative_z_test
        command = PusTelecommand(service=8, subservice=128, ssc=35, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Initiate reading of negative z self test results"))
        command = object_id + ImtqActionIds.read_self_test_results
        command = PusTelecommand(service=8, subservice=128, ssc=36, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Request dataset with negative z self test results"))
        sid = make_sid(object_id, ImtqSetIds.NEGATIVE_Z_TEST)
        command = generate_one_hk_command(sid, 37)
        tc_queue.appendleft(command.pack_command_tuple())

    if op_code == "0" or op_code == "7":
        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Commanding dipole"))
        x_dipole = 0
        y_dipole = 0
        z_dipole = 0
        duration = 0    # ms
        command = pack_dipole_command(object_id, x_dipole, y_dipole, z_dipole, duration)
        command = PusTelecommand(service=8, subservice=128, ssc=20, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

    if op_code == "0" or op_code == "8":
        tc_queue.appendleft((QueueCommands.PRINT, "IMTQ: Get commanded dipole"))
        command = object_id + ImtqActionIds.get_commanded_dipole
        command = PusTelecommand(service=8, subservice=128, ssc=21, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

    return tc_queue


def pack_dipole_command(object_id: bytearray, x_dipole: int, y_dipole: int, z_dipole: int, duration: int) -> bytearray:
    """ This function packs the command causing the ISIS IMTQ to generate a dipole.
    @param object_id    The object id of the IMTQ handler.
    @param x_dipole   The dipole of the x coil in 10^-4*Am^2 (max. 2000)
    @param y_dipole   The dipole of the y coil in 10^-4*Am^2 (max. 2000)
    @param z_dipole   The dipole of the z coil in 10^-4*Am^2 (max. 2000)
    @param duration  The duration in milliseconds the dipole will be generated by the coils.
                     When set to 0, the dipole will be generated until a new dipole actuation
                     command is sent.
    """
    action_id = ImtqActionIds.start_actuation_dipole
    command = bytearray()
    command = object_id + action_id
    command.extend(x_dipole.to_bytes(length=2, byteorder='big'))
    command.extend(y_dipole.to_bytes(length=2, byteorder='big'))
    command.extend(z_dipole.to_bytes(length=2, byteorder='big'))
    command.extend(duration.to_bytes(length=2, byteorder='big'))
    return command