# -*- 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.pus_tc.packer import TcQueueT
from tmtccmd.ecss.tc import PusTelecommand
from tmtccmd.pus_tc.service_3_housekeeping import make_sid, generate_one_hk_command


class RwSetIds:
    STATUS_SET_ID = 4
    TEMPERATURE_SET_ID = 8


class RwCommandIds:
    RESET_MCU = bytearray([0x0, 0x0, 0x0, 0x01])
    # Reads status information from reaction wheel into dataset with id 4
    GET_RW_STATUS = bytearray([0x0, 0x0, 0x0, 0x04])
    SET_SPEED = bytearray([0x0, 0x0, 0x0, 0x06])
    # Reads temperature from reaction wheel into dataset with id 8
    GET_TEMPERATURE = bytearray([0x0, 0x0, 0x0, 0x08])


class SpeedDefinitions:
    RPM_100 = 1000
    RPM_5000 = 5000


class RampTime:
    MS_100 = 100


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

    if op_code == "0" or op_code == "1":
        tc_queue.appendleft((QueueCommands.PRINT, "Reaction Wheel: Set speed"))
        command = pack_set_speed_command(SpeedDefinitions.RPM_100, RampTime.MS_100)
        command = PusTelecommand(service=8, subservice=128, ssc=40, app_data=command)
        tc_queue.appendleft(command.pack_command_tuple())

    return tc_queue


def pack_set_speed_command(object_id: bytearray, speed: int, ramp_time: int) -> bytearray:
    """ With this function a command is packed to set the speed of a reaction wheel
    @param object_id    The object id of the reaction wheel handler.
    @param speed   Valid speeds are [-65000, -1000] and [1000, 65000]. Values are specified in 0.1 * RPM
    @param ramp_time   The time after which the reaction wheel will reached the commanded speed. Valid times are
                       10 - 10000 ms
    """
    command_id = RwCommandIds.SET_SPEED
    command = bytearray()
    command = object_id + command_id
    command.extend(speed.to_bytes(length=4, byteorder='big'))
    command.extend(ramp_time.to_bytes(length=2, byteorder='big'))
    return command