# -*- coding: utf-8 -*-
"""reaction_wheels.py
@brief  Tests for the reaction wheel handler
@author J. Meier
@date   20.06.2021
"""
import struct

from tmtccmd.config import TmtcDefinitionWrapper, OpCodeEntry
from tmtccmd.config.tmtc import tmtc_definitions_provider
from tmtccmd.tc import DefaultPusQueueHelper
from tmtccmd.tc.pus_3_fsfw_hk import (
    generate_one_hk_command,
    generate_one_diag_command,
    make_sid,
)
from spacepackets.ecss.tc import PusTelecommand
from tmtccmd.tc.pus_200_fsfw_modes import pack_mode_data, Modes, Subservices
from config.definitions import CustomServiceList


class OpCodesDevs:
    SPEED = ["0", "speed"]
    ON = ["1", "on"]
    NML = ["2", "nml"]
    OFF = ["3", "off"]
    GET_STATUS = ["4", "status"]
    GET_TM = ["5", "tm"]


class InfoDevs:
    SPEED = "Set speed"
    ON = "Set On"
    NML = "Set Normal"
    OFF = "Set Off"
    GET_STATUS = "Get Status HK"
    GET_TM = "Get TM HK"


class OpCodesAss:
    ON = ["0", "on"]
    NML = ["1", "nml"]
    OFF = ["2", "off"]


class InfoAss:
    ON = "Mode On: 3/4 RWs min. on"
    NML = "Mode Normal: 3/4 RWs min. normal"
    OFF = "Mode Off: All RWs off"


class RwSetIds:
    STATUS_SET_ID = 4
    TEMPERATURE_SET_ID = 8
    LAST_RESET = 2
    TM_SET = 9


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])
    INIT_RW_CONTROLLER = bytearray([0x0, 0x0, 0x0, 0x05])
    SET_SPEED = bytearray([0x0, 0x0, 0x0, 0x06])
    # Reads temperature from reaction wheel into dataset with id 8
    GET_TEMPERATURE = bytearray([0x0, 0x0, 0x0, 0x08])
    GET_TM = bytearray([0x0, 0x0, 0x0, 0x09])


class SpeedDefinitions:
    RPM_100 = 1000
    RPM_5000 = 5000


class RampTime:
    MS_1000 = 1000


@tmtc_definitions_provider
def add_rw_cmds(defs: TmtcDefinitionWrapper):
    oce = OpCodeEntry()
    oce.add(info=InfoDevs.SPEED, keys=OpCodesDevs.SPEED)
    oce.add(info=InfoDevs.ON, keys=OpCodesDevs.ON)
    oce.add(info=InfoDevs.OFF, keys=OpCodesDevs.OFF)
    oce.add(info=InfoDevs.NML, keys=OpCodesDevs.NML)
    oce.add(info=InfoDevs.GET_STATUS, keys=OpCodesDevs.GET_STATUS)
    oce.add(info=InfoDevs.GET_TM, keys=OpCodesDevs.GET_TM)
    defs.add_service(
        name=CustomServiceList.REACTION_WHEEL_1.value,
        info="Reaction Wheel 1",
        op_code_entry=oce,
    )
    defs.add_service(
        name=CustomServiceList.REACTION_WHEEL_2.value,
        info="Reaction Wheel 2",
        op_code_entry=oce,
    )
    defs.add_service(
        name=CustomServiceList.REACTION_WHEEL_3.value,
        info="Reaction Wheel 3",
        op_code_entry=oce,
    )
    defs.add_service(
        name=CustomServiceList.REACTION_WHEEL_4.value,
        info="Reaction Wheel 4",
        op_code_entry=oce,
    )
    oce = OpCodeEntry()
    oce.add(info=InfoAss.ON, keys=OpCodesAss.ON)
    oce.add(info=InfoAss.NML, keys=OpCodesAss.NML)
    oce.add(info=InfoAss.OFF, keys=OpCodesAss.OFF)
    defs.add_service(
        name=CustomServiceList.RW_ASSEMBLY.value,
        info="Reaction Wheel Assembly",
        op_code_entry=oce,
    )


def pack_single_rw_test_into(
    object_id: bytes, rw_idx: int, q: DefaultPusQueueHelper, op_code: str
):
    if op_code in OpCodesDevs.SPEED:
        speed = int(input("Specify speed [0.1 RPM]: "))
        ramp_time = int(input("Specify ramp time [ms]: "))
        q.add_log_cmd(f"RW {rw_idx}: {InfoDevs.SPEED}")
        q.add_pus_tc(pack_set_speed_command(object_id, speed, ramp_time))

    if op_code in OpCodesDevs.ON:
        q.add_log_cmd(f"RW {rw_idx}: {InfoDevs.ON}")
        mode_data = pack_mode_data(object_id, Modes.ON, 0)
        q.add_pus_tc(PusTelecommand(service=200, subservice=1, app_data=mode_data))

    if op_code in OpCodesDevs.NML:
        q.add_log_cmd(f"RW {rw_idx}: {InfoDevs.NML}")
        mode_data = pack_mode_data(object_id, Modes.NORMAL, 0)
        q.add_pus_tc(PusTelecommand(service=200, subservice=1, app_data=mode_data))

    if op_code in OpCodesDevs.OFF:
        q.add_log_cmd(f"RW {rw_idx}: {InfoDevs.OFF}")
        mode_data = pack_mode_data(object_id, Modes.OFF, 0)
        q.add_pus_tc(PusTelecommand(service=200, subservice=1, app_data=mode_data))

    if op_code in OpCodesDevs.GET_TM:
        q.add_log_cmd(f"RW {rw_idx}: {InfoDevs.GET_TM}")
        q.add_pus_tc(
            generate_one_hk_command(
                sid=make_sid(object_id=object_id, set_id=RwSetIds.TM_SET)
            )
        )
    if op_code in OpCodesDevs.GET_STATUS:
        q.add_log_cmd(f"RW {rw_idx}: {InfoDevs.GET_STATUS}")
        q.add_pus_tc(
            generate_one_diag_command(
                sid=make_sid(object_id=object_id, set_id=RwSetIds.STATUS_SET_ID)
            )
        )


def pack_rw_ass_cmds(q: DefaultPusQueueHelper, object_id: bytes, op_code: str):
    if op_code in OpCodesAss.OFF:
        data = pack_mode_data(object_id=object_id, mode=Modes.OFF, submode=0)
        q.add_pus_tc(
            PusTelecommand(
                service=200, subservice=Subservices.TC_MODE_COMMAND, app_data=data
            )
        )
    if op_code in OpCodesAss.ON:
        data = pack_mode_data(object_id=object_id, mode=Modes.ON, submode=0)
        q.add_pus_tc(
            PusTelecommand(
                service=200, subservice=Subservices.TC_MODE_COMMAND, app_data=data
            )
        )
    if op_code in OpCodesAss.NML:
        data = pack_mode_data(object_id=object_id, mode=Modes.NORMAL, submode=0)
        q.add_pus_tc(
            PusTelecommand(
                service=200, subservice=Subservices.TC_MODE_COMMAND, app_data=data
            )
        )


def pack_set_speed_command(
    object_id: bytes, speed: int, ramp_time_ms: int
) -> PusTelecommand:
    """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_ms: The time after which the reaction wheel will reach the commanded speed.
        Valid times are 10 - 10000 ms
    """
    if speed > 0:
        if speed < 1000 or speed > 65000:
            raise ValueError(
                "Invalid RW speed specified. "
                "Allowed range is [1000, 65000] 0.1 * RPM"
            )
    elif speed < 0:
        if speed < -65000 or speed > -1000:
            raise ValueError(
                "Invalid RW speed specified. "
                "Allowed range is [-65000, -1000] 0.1 * RPM"
            )
    else:
        # Speed is 0
        pass

    if ramp_time_ms < 0 or (
        ramp_time_ms > 0 and (ramp_time_ms > 10000 or ramp_time_ms < 10)
    ):
        raise ValueError("Invalid Ramp Speed time. Allowed range is [10-10000] ms")
    command_id = RwCommandIds.SET_SPEED
    command = bytearray()
    command += object_id + command_id
    command = command + struct.pack("!i", speed)
    command = command + ramp_time_ms.to_bytes(length=2, byteorder="big")
    command = PusTelecommand(service=8, subservice=128, app_data=command)
    return command