###########################################################################
#
# Copyright (c) 2020-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    pressure_sensors.py
#
# @author  (last)      Micahel Garthwaite
# @date    (last)      07-Mar-2023
# @author  (original)  Sean
# @date    (original)  14-Apr-2020
#
############################################################################
import struct
from enum import unique
from logging import Logger

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


class DDPressureSensors(AbstractSubSystem):
    """
    DD interface containing pressure related commands.
    """


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

        @param can_interface: The DenaliCANMessenger object
        """

        super().__init__()
        self.can_interface = can_interface
        self.logger = logger

        if self.can_interface is not None:
            channel_id = DenaliChannels.dd_sync_broadcast_ch_id
            self.msg_id_dd_pressures_data = MsgIds.MSG_ID_DD_PRESSURES_DATA.value
            self.can_interface.register_receiving_publication_function(channel_id, self.msg_id_dd_pressures_data,
                                                                       self._handler_pressures_sync)

        self.dd_pressures = {
            DDPressureNames.D9_PRES.name: 0.0,
            DDPressureNames.D66_PRES.name: 0.0,
            DDPressureNames.D51_PRES.name: 0.0,
            DDPressureNames.D18_PRES.name: 0.0,
            DDPressureNames.D41_PRES.name: 0.0
        }

        self.dd_pressures_timestamp = 0

    @publish(["msg_id_dd_pressures_data", "dd_pressures", "dd_pressures_timestamp"])
    def _handler_pressures_sync(self, message,timestamp=0.0):
        """
        Handles published DD pressure data messages. DD Pressure data are captured
        for reference.

        @param message: published DD pressure data message
        @return: none
        """
        self.dd_pressures[DDPressureNames.D9_PRES.name] = struct.unpack('f', bytearray(
            message['message'][MsgFieldPositions.START_POS_FIELD_1:MsgFieldPositions.END_POS_FIELD_1]))[0]
        self.dd_pressures[DDPressureNames.D66_PRES.name] = struct.unpack('f', bytearray(
            message['message'][MsgFieldPositions.START_POS_FIELD_2:MsgFieldPositions.END_POS_FIELD_2]))[0]
        self.dd_pressures[DDPressureNames.D51_PRES.name] = struct.unpack('f', bytearray(
            message['message'][MsgFieldPositions.START_POS_FIELD_3:MsgFieldPositions.END_POS_FIELD_3]))[0]
        self.dd_pressures[DDPressureNames.D18_PRES.name] = struct.unpack('f', bytearray(
            message['message'][MsgFieldPositions.START_POS_FIELD_4:MsgFieldPositions.END_POS_FIELD_4]))[0]
        self.dd_pressures[DDPressureNames.D41_PRES.name] = struct.unpack('f', bytearray(
            message['message'][MsgFieldPositions.START_POS_FIELD_5:MsgFieldPositions.END_POS_FIELD_5]))[0]

        self.dd_pressures_timestamp = timestamp

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

        @param ms:  unsigned int - broadcast interval (in ms)
        @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)
        ivl = integer_to_bytearray(ms)
        payload = rst + ivl

        message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_dd_ch_id,
                                              message_id=MsgIds.MSG_ID_DD_PRESSURE_SENSOR_PUBLISH_INTERVAL_OVERRIDE_REQUEST.value,
                                              payload=payload)

        self.logger.debug("override pressure data broadcast interval")

        # 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_pressure_readings_override(self, sensor: int, pressure: float, reset: int = NO_RESET) -> int:
        """
        Constructs and sends the pressure readings override command.
        Constraints:
            Must be logged into DD.

        @param sensor:  unsigned int - sensor ID
        @param pressure:  float - pressure (in PSI)
        @param reset:  integer - 1 to reset a previous override, 0 to override
        @return: 1 if successful, zero otherwise
        """

        rst = integer_to_bytearray(reset)
        prs = float_to_bytearray(pressure)
        idx = integer_to_bytearray(sensor)
        payload = rst + prs + idx

        message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_dd_ch_id,
                                              message_id=MsgIds.MSG_ID_DD_PRESSURE_SENSOR_READINGS_OVERRIDE_REQUEST.value,
                                              payload=payload)

        self.logger.debug("override pressure reading")

        # 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_pressure_sensor_read_counter_override(self, sensor: int, counter: int, reset: int = NO_RESET) -> int:
        """
        Constructs and sends the pressure sensor read counter override command
        Constraints:
            Must be logged into DD.

        @param sensor: unsigned int - sensor ID
        @param counter:  integer - read count to override sensor with
        @param reset:  integer - 1 to reset a previous override, 0 to override
        @return: 1 if successful, zero otherwise

        """

        reset_byte_array = integer_to_bytearray(reset)
        read_byte_array = integer_to_bytearray(counter)
        sensor_byte_array = integer_to_bytearray(sensor)
        payload = reset_byte_array + read_byte_array + sensor_byte_array

        message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_dd_ch_id,
                                              message_id=MsgIds.MSG_ID_DD_PRESSURE_SENSOR_READ_COUNTER_OVERRIDE_REQUEST.value,
                                              payload=payload)

        if reset == RESET:
            str_res = "reset back to normal"
        else:
            str_res = str(sensor) + " psi"
        self.logger.debug("override read counter value for pressure sensor " + str(sensor) + ": " + str_res)

        # 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.error("Timeout!!!!")
            return False

    def cmd_pressure_sensor_error_counter_override(self, sensor: int, counter: int, reset: int = NO_RESET) -> int:
        """
        Constructs and sends the pressure sensor error counter override command.
        Constraints:
            Must be logged into DD.

        @param sensor: unsigned int - sensor ID
        @param counter:  integer - error count to override sensor with
        @param reset:  integer - 1 to reset a previous override, 0 to override
        @return: 1 if successful, zero otherwise

        """

        reset_byte_array = integer_to_bytearray(reset)
        error_byte_array = integer_to_bytearray(counter)
        sensor_byte_array = integer_to_bytearray(sensor)
        payload = reset_byte_array + error_byte_array + sensor_byte_array

        message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_dd_ch_id,
                                              message_id=MsgIds.MSG_ID_DD_PRESSURE_SENSOR_ERROR_COUNTER_OVERRIDE_REQUEST.value,
                                              payload=payload)

        if reset == RESET:
            str_res = "reset back to normal"
        else:
            str_res = str(sensor) + " psi"
        self.logger.debug("override error counter value for pressure sensor " + str(sensor) + ": " + str_res)

        # 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.error("Timeout!!!!")
            return False

    def cmd_pressure_filtered_readings_override(self, sensor: int, pressure: float, reset: int = NO_RESET) -> int:
        """
        Constructs and sends the pressure filtered readings override command.
        Constraints:
            Must be logged into DD.

        @param sensor:  unsigned int - sensor ID
        @param pressure: float - pressure (in PSI)
        @param reset:  integer - 1 to reset a previous override, 0 to override
        @return: 1 if successful, zero otherwise
        """

        rst = integer_to_bytearray(reset)
        prs = float_to_bytearray(pressure)
        idx = integer_to_bytearray(sensor)
        payload = rst + prs + idx

        message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_dd_ch_id,
                                              message_id=MsgIds.MSG_ID_DD_PRESSURE_SENSOR_FILTER_READINGS_OVERRIDE_REQUEST.value,
                                              payload=payload)

        self.logger.debug("override pressure reading")

        # 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