###########################################################################
#
# Copyright (c) 2022-2024 Diality Inc. - All Rights Reserved.
#
# THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN
# WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER.
#
# @file    ejector.py
#
# @author  (last)      Micahel Garthwaite
# @date    (last)      10-Mar-2023
# @author  (original)  Micahel Garthwaite
# @date    (original)  01-Nov-2024
#
############################################################################

import struct
from logging import Logger

from .constants import RESET, NO_RESET
from leahi_dialin.common.msg_defs import MsgIds, MsgFieldPositions
from leahi_dialin.common.td_defs import EjectorNames, EjectorAttributes
from leahi_dialin.protocols.CAN import DenaliMessage, DenaliChannels
from leahi_dialin.utils.base import AbstractSubSystem, publish
from leahi_dialin.utils.checks import check_broadcast_interval_override_ms
from leahi_dialin.utils.conversions import integer_to_bytearray, float_to_bytearray


class TDEjector(AbstractSubSystem):
    """
    TDEjector

    Treatment Delivery (TD) Dialin API sub-class for ejector motor related commands.
    """

    def __init__(self, can_interface, logger: Logger):
        """

        @param can_interface: Denali Can Messenger object
        """
        super().__init__()
        self.can_interface = can_interface
        self.logger = logger

        if self.can_interface is not None:
            channel_id = DenaliChannels.td_sync_broadcast_ch_id
            msg_id = MsgIds.MSG_ID_TD_EJECTOR_DATA.value
            self.can_interface.register_receiving_publication_function(channel_id, msg_id,
                                                                       self._handler_ejector_sync)

        self.h5_motor = {
            EjectorNames.H5_MOTOR.name: {
                EjectorAttributes.STATE.name: 0,
                EjectorAttributes.SET_SPEED.name: 0.0
            }
        }
        self.td_ejector_timestamp = 0

    @publish(["td_ejector_timestamp", "h5_motor"])
    def _handler_ejector_sync(self, message, timestamp=0.0):
        """
        Handles published ejector data messages.

        @param message:  published ejector data message as: ejector state
        @return: None
        """
        self.h5_motor[EjectorNames.H5_MOTOR.name][EjectorAttributes.STATE.name] = struct.unpack('i', bytearray(
            message['message'][MsgFieldPositions.START_POS_FIELD_1:MsgFieldPositions.END_POS_FIELD_1]))[0]
        self.h5_motor[EjectorNames.H5_MOTOR.name][EjectorAttributes.SET_SPEED.name] = struct.unpack('f', bytearray(
            message['message'][MsgFieldPositions.START_POS_FIELD_2:MsgFieldPositions.END_POS_FIELD_2]))[0]

        self.td_ejector_timestamp = timestamp

    def cmd_ejector_data_broadcast_interval_override(self, ms: int, reset: int = NO_RESET) -> int:
        """
        Constructs and sends the ejector motor data broadcast interval override command
        Constraints:
            Must be logged into TD.
            Given interval must be non-zero and a multiple of the TD general task interval (50 ms).

        @param ms:  integer - interval (in ms) to override with
        @param reset:  integer - 1 to reset a previous override, 0 to override
        @return: 1 if successful, zero otherwise
        """

        if not check_broadcast_interval_override_ms(ms):
            return False

        rst = integer_to_bytearray(reset)
        mis = integer_to_bytearray(ms)
        payload = rst + mis

        message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_td_ch_id,
                                              message_id=MsgIds.MSG_ID_TD_EJECTOR_PUBLISH_INTERVAL_OVERRIDE_REQUEST.value,
                                              payload=payload)

        self.logger.debug("override TD ejector data broadcast interval")

        # Send message
        received_message = self.can_interface.send(message)

        # If there is content...
        if received_message is not None:
            if reset == RESET:
                str_res = "reset back to normal: "
            else:
                str_res = str(ms) + " ms: "
            self.logger.debug("ejector data broadcast interval overridden to " + str_res +
                              str(received_message['message'][DenaliMessage.PAYLOAD_START_INDEX]))
            # response payload is OK or not OK
            return received_message['message'][DenaliMessage.PAYLOAD_START_INDEX]
        else:
            self.logger.debug("Timeout!!!!")
            return False

    def cmd_ejector_request_command(self, cmd: int) -> int:
        """
        Constructs and sends the ejector request command.
            EJECTOR_OPERATION_HOME = 0,          ///< Ejector home operation
            EJECTOR_OPERATION_RETRACT,           ///< Ejector retract operation
            EJECTOR_OPERATION_EXTEND,            ///< Ejector extend operation

        Constraints:
            Must be logged into TD.
        @param cmd:  integer - ID of an ejector operation that is being requested
        @return: 1 if successful, zero otherwise
        """

        idx = integer_to_bytearray(cmd)
        payload = idx

        message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_td_ch_id,
                                              message_id=MsgIds.MSG_ID_TD_EJECTOR_COMMAND.value,
                                              payload=payload)

        self.logger.debug("setting ejector state to" + str(cmd))

        # Send message
        received_message = self.can_interface.send(message)

        # If there is content...
        if received_message is not None:
            # response payload is OK or not OK
            return received_message['message'][DenaliMessage.PAYLOAD_START_INDEX]
        else:
            self.logger.debug("Timeout!!!!")
            return False

    def cmd_ejector_set_motor_speed_request(self, speed: int) -> int:
        """
        Constructs and sends the ejector set speed command. Negative values imply reverse ( retract ) direction.

        Constraints:
            Must be logged into TD.
        @param speed:  float - value to set the motor speed in RPM.

        @return: 1 if successful, zero otherwise
        """

        spd = float_to_bytearray(speed)
        payload = spd

        message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_td_ch_id,
                                              message_id=MsgIds.MSG_ID_TD_EJECTOR_MOTOR_SET_SPEED_REQUEST.value,
                                              payload=payload)

        self.logger.debug("setting ejector set speed to" + str(spd))

        # Send message
        received_message = self.can_interface.send(message)

        # If there is content...
        if received_message is not None:
            # response payload is OK or not OK
            return received_message['message'][DenaliMessage.PAYLOAD_START_INDEX]
        else:
            self.logger.debug("Timeout!!!!")
            return False