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

from spacepackets.ecss.tc import PusTelecommand
from tmtccmd.tc import DefaultPusQueueHelper
from tmtccmd.tc.pus_3_fsfw_hk import (
    make_sid,
    generate_one_diag_command,
    generate_one_hk_command,
)
from tmtccmd.tc.pus_200_fsfw_modes import pack_mode_data, Modes
from tmtccmd.util import ObjectIdU32


class OpCodes:
    ON = ["on"]
    NORMAL = ["normal"]
    OFF = ["off"]


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: ObjectIdU32, q: DefaultPusQueueHelper, op_code: str):
    q.add_log_cmd(
        f"Testing ISIS IMTQ handler with object id: {object_id.as_hex_string}"
    )

    if op_code in OpCodes.OFF:
        q.add_log_cmd("IMTQ: Set mode off")
        command = pack_mode_data(object_id.as_bytes, Modes.OFF, 0)
        q.add_pus_tc(PusTelecommand(service=200, subservice=1, app_data=command))
    if op_code in OpCodes.ON:
        q.add_log_cmd("IMTQ: Set mode on")
        command = pack_mode_data(object_id.as_bytes, Modes.ON, 0)
        q.add_pus_tc(PusTelecommand(service=200, subservice=1, app_data=command))
    if op_code in OpCodes.NORMAL:
        q.add_log_cmd("IMTQ: Mode Normal")
        command = pack_mode_data(object_id.as_bytes, Modes.NORMAL, 0)
        q.add_pus_tc(PusTelecommand(service=200, subservice=1, app_data=command))
    if op_code == "3":
        q.add_log_cmd("IMTQ: Perform positive x self test")
        command = object_id.as_bytes + ImtqActionIds.perform_positive_x_test
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))

        q.add_log_cmd("IMTQ: Initiate reading of positive x self test results")
        command = object_id.as_bytes + ImtqActionIds.read_self_test_results
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))

        q.add_log_cmd("IMTQ: Request dataset with positive x self test results")
        sid = make_sid(object_id.as_bytes, ImtqSetIds.POSITIVE_X_TEST)
        q.add_pus_tc(generate_one_hk_command(sid))

    if op_code == "4":
        q.add_log_cmd("IMTQ: Perform negative x self test")
        command = object_id.as_bytes + ImtqActionIds.perform_negative_x_test
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
        q.add_log_cmd("IMTQ: Initiate reading of negative x self test results")
        command = object_id.as_bytes + ImtqActionIds.read_self_test_results
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
        q.add_log_cmd("IMTQ: Request dataset with negative x self test results")
        sid = make_sid(object_id.as_bytes, ImtqSetIds.NEGATIVE_X_TEST)
        q.add_pus_tc(generate_one_hk_command(sid))

    if op_code == "5":
        q.add_log_cmd("IMTQ: Perform positive y self test")
        command = object_id.as_bytes + ImtqActionIds.perform_positive_y_test
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
        q.add_log_cmd("IMTQ: Initiate reading of positive y self test results")
        command = object_id.as_bytes + ImtqActionIds.read_self_test_results
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))

        q.add_log_cmd("IMTQ: Request dataset with positive y self test results")
        sid = make_sid(object_id.as_bytes, ImtqSetIds.POSITIVE_Y_TEST)
        q.add_pus_tc(generate_one_hk_command(sid))

    if op_code == "6":
        q.add_log_cmd("IMTQ: Perform negative y self test")
        command = object_id.as_bytes + ImtqActionIds.perform_negative_y_test
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))

        q.add_log_cmd("IMTQ: Initiate reading of negative y self test results")
        command = object_id.as_bytes + ImtqActionIds.read_self_test_results
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))

        q.add_log_cmd("IMTQ: Request dataset with negative y self test results")
        sid = make_sid(object_id.as_bytes, ImtqSetIds.NEGATIVE_Y_TEST)
        q.add_pus_tc(generate_one_hk_command(sid))

    if op_code == "7":
        q.add_log_cmd("IMTQ: Perform positive z self test")
        command = object_id.as_bytes + ImtqActionIds.perform_positive_z_test
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))

        q.add_log_cmd("IMTQ: Initiate reading of positive z self test results")
        command = object_id.as_bytes + ImtqActionIds.read_self_test_results
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))

        q.add_log_cmd("IMTQ: Request dataset with positive z self test results")
        sid = make_sid(object_id.as_bytes, ImtqSetIds.POSITIVE_Y_TEST)
        q.add_pus_tc(generate_one_hk_command(sid))

    if op_code == "8":
        q.add_log_cmd("IMTQ: Perform negative z self test")
        command = object_id.as_bytes + ImtqActionIds.perform_negative_z_test
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
        q.add_log_cmd("IMTQ: Initiate reading of negative z self test results")
        command = object_id.as_bytes + ImtqActionIds.read_self_test_results
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))
        q.add_log_cmd("IMTQ: Request dataset with negative z self test results")
        sid = make_sid(object_id.as_bytes, ImtqSetIds.NEGATIVE_Z_TEST)
        q.add_pus_tc(generate_one_hk_command(sid))

    if op_code == "9":
        q.add_log_cmd("IMTQ: Commanding dipole")

        x_dipole = int(input("Specify X dipole [range [0, 2000] * 10^-4 * Am^2]: "))
        y_dipole = int(input("Specify Y dipole [range [0, 2000] * 10^-4 * Am^2]: "))
        z_dipole = int(input("Specify Z dipole [range [0, 2000] * 10^-4 * Am^2]: "))
        duration = int(
            input(
                f"Specify torque duration [range [0, {pow(2, 16) - 1}, "
                f"0 for continuous generation until update]: "
            )
        )
        q.add_pus_tc(
            pack_dipole_command(
                object_id.as_bytes, x_dipole, y_dipole, z_dipole, duration
            )
        )

    if op_code == "10":
        q.add_log_cmd("IMTQ: Get commanded dipole")
        command = object_id.as_bytes + ImtqActionIds.get_commanded_dipole
        q.add_pus_tc(PusTelecommand(service=8, subservice=128, app_data=command))

    if op_code == "11":
        q.add_log_cmd("IMTQ: Get engineering hk set")
        q.add_pus_tc(
            generate_one_diag_command(
                sid=make_sid(object_id=object_id.as_bytes, set_id=ImtqSetIds.ENG_HK_SET)
            )
        )

    if op_code == "12":
        q.add_log_cmd("IMTQ: Get calibrated MTM hk set")
        q.add_pus_tc(
            generate_one_diag_command(
                sid=make_sid(
                    object_id=object_id.as_bytes, set_id=ImtqSetIds.CAL_MTM_SET
                )
            )
        )

    if op_code == "13":
        q.add_log_cmd("IMTQ: Get raw MTM hk set")
        q.add_pus_tc(
            generate_one_diag_command(
                sid=make_sid(
                    object_id=object_id.as_bytes, set_id=ImtqSetIds.RAW_MTM_SET
                )
            )
        )


def pack_dipole_command(
    object_id: bytes, x_dipole: int, y_dipole: int, z_dipole: int, duration: int
) -> PusTelecommand:
    """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 = object_id + action_id
    x_dipole = int(round(x_dipole))
    y_dipole = int(round(y_dipole))
    z_dipole = int(round(z_dipole))
    if x_dipole < -2000 or x_dipole > 2000:
        raise_dipole_error("X dipole", x_dipole)
    if y_dipole < -2000 or y_dipole > 2000:
        raise_dipole_error("Y dipole", y_dipole)
    if z_dipole < -2000 or z_dipole > 2000:
        raise_dipole_error("Z dipole", z_dipole)
    duration = int(round(duration))
    if duration < 0 or duration > pow(2, 16) - 1:
        raise ValueError(
            f"Duration in ms of {duration} smaller than 0 or larger than allowed {pow(2, 16) - 1}"
        )
    command += struct.pack("!h", x_dipole)
    command += struct.pack("!h", y_dipole)
    command += struct.pack("!h", z_dipole)
    command += struct.pack("!H", duration)
    command = PusTelecommand(service=8, subservice=128, app_data=command)
    return command


def raise_dipole_error(dipole_str: str, value: int):
    raise ValueError(
        f"{dipole_str} {value} negative or larger than maximum allowed 2000 * 10^-4*Am^2"
    )