###########################################################################
#
#  Copyright (c) 2020-2020 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 accelerometer.py
#
#  @date 28-Jul-2020
#  @author S. Nash
#
#  @brief
#  Provides access to accelerometer readings and commands to override the
#  accelerometer sensor readings.
#
############################################################################
import struct
from ..protocols.CAN import (DenaliMessage,
                             DenaliChannels)
from ..utils.conversions import integer_to_bytearray, float_to_bytearray
from .constants import RESET, NO_RESET
from ..utils.base import _AbstractSubSystem, _publish
from logging import Logger

class HDAccelerometer(_AbstractSubSystem):
    """
    Hemodialysis Delivery (HD) Dialin API sub-class for accelerometer related commands.
    """

    # Accelerometer message IDs
    MSG_ID_HD_ACCELEROMETER_DATA = 0x0033
    MSG_ID_HD_ACCEL_OVERRIDE = 0x8027
    MSG_ID_HD_ACCEL_MAX_OVERRIDE = 0x8028
    MSG_ID_HD_ACCEL_SEND_INTERVAL_OVERRIDE = 0x8029
    MSG_ID_HD_ACCEL_SET_CALIBRATION = 0x802A

    # Vector axes
    class AccelerometerVector:
        def __init__(self, x=0.0, y=0.0, z=0.0):
            """
            HDAccelerometer constructor

            """
            self.x = x
            self.y = y
            self.z = z

    # Vector axes
    VECTOR_AXIS_X = 0
    VECTOR_AXIS_Y = 1
    VECTOR_AXIS_Z = 2

    # Pressure/Occlusion broadcast message field positions
    START_POS_X = DenaliMessage.PAYLOAD_START_INDEX
    END_POS_X = START_POS_X + 4
    START_POS_Y = END_POS_X
    END_POS_Y = START_POS_Y + 4
    START_POS_Z = END_POS_Y
    END_POS_Z = START_POS_Z + 4
    START_POS_MAX_X = END_POS_Z
    END_POS_MAX_X = START_POS_MAX_X + 4
    START_POS_MAX_Y = END_POS_MAX_X
    END_POS_MAX_Y = START_POS_MAX_Y + 4
    START_POS_MAX_Z = END_POS_MAX_Y
    END_POS_MAX_Z = START_POS_MAX_Z + 4
    START_POS_TILT_X = END_POS_MAX_Z
    END_POS_TILT_X = START_POS_TILT_X + 4
    START_POS_TILT_Y = END_POS_TILT_X
    END_POS_TILT_Y = START_POS_TILT_Y + 4
    START_POS_TILT_Z = END_POS_TILT_Y
    END_POS_TILT_Z = START_POS_TILT_Z + 4

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

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

        if self.can_interface is not None:
            channel_id = DenaliChannels.hd_sync_broadcast_ch_id
            msg_id = self.MSG_ID_HD_ACCELEROMETER_DATA
            self.can_interface.register_receiving_publication_function(channel_id, msg_id,
                                                                       self._handler_accelerometer_sync)

        self.vector = self.AccelerometerVector()
        self.vector_max = self.AccelerometerVector()
        self.tilts = self.AccelerometerVector()

    def get_accel_vector(self):
        """
        Gets the accelerometer vector.
        @return: (vector) The vector from the accelerometer
        """
        return self.vector

    def get_accel_max_vector(self):
        """
        Gets the accelerometer maximum vector.
        @return: (vector) The max. vector from the accelerometer
        """
        return self.vector_max

    def get_accel_tilts(self):
        """
        Gets the tilt angles from the accelerometer.
        @return: (vector) The X, Y, and Z tilt angles.
        """
        return self.tilts

    @_publish([
        "vector",
        "vector_max",
        "tilts"
    ])
    def _handler_accelerometer_sync(self, message):
        """
        Handles published accelerometer data messages.  Accelerometer data are captured
        for reference.

        @param message:  published accelerometer data message
        @return: none
        """

        x = struct.unpack('f', bytearray(message['message'][self.START_POS_X:self.END_POS_X]))
        y = struct.unpack('f', bytearray(message['message'][self.START_POS_Y:self.END_POS_Y]))
        z = struct.unpack('f', bytearray(message['message'][self.START_POS_Z:self.END_POS_Z]))
        self.vector = self.AccelerometerVector( x[0], y[0], z[0] )

        x = struct.unpack('f', bytearray(message['message'][self.START_POS_MAX_X:self.END_POS_MAX_X]))
        y = struct.unpack('f', bytearray(message['message'][self.START_POS_MAX_Y:self.END_POS_MAX_Y]))
        z = struct.unpack('f', bytearray(message['message'][self.START_POS_MAX_Z:self.END_POS_MAX_Z]))
        self.vector_max = self.AccelerometerVector( x[0], y[0], z[0] )

        x = struct.unpack('f', bytearray(message['message'][self.START_POS_TILT_X:self.END_POS_TILT_X]))
        y = struct.unpack('f', bytearray(message['message'][self.START_POS_TILT_Y:self.END_POS_TILT_Y]))
        z = struct.unpack('f', bytearray(message['message'][self.START_POS_TILT_Z:self.END_POS_TILT_Z]))
        self.tilts = self.AccelerometerVector( x[0], y[0], z[0] )

    def cmd_accel_vector_override(self, mag, axis, reset=NO_RESET):
        """
        Constructs and sends the accelerometer vector override command
        Constraints:
            Must be logged into HD.

        @param mag:  float - axis magnitude (in g) to override with
        @param axis:  integer - accelerometer axis to override
        @param reset:  integer - 1 to reset a previous override, 0 to override
        @return: 1 if successful, zero otherwise
        """

        rst = integer_to_bytearray(reset)
        sta = float_to_bytearray(mag)
        idx = integer_to_bytearray(axis)
        payload = rst + sta + idx

        message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_hd_ch_id,
                                              message_id=self.MSG_ID_HD_ACCEL_OVERRIDE,
                                              payload=payload)

        self.logger.debug("override HD accelerometer axis magnitude")

        # 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(mag) + " g.  "
            self.logger.debug("Accelerometer axis " + str(axis) + " 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_accel_max_vector_override(self, mag, axis, reset=NO_RESET):
        """
        Constructs and sends the accelerometer maximum vector override command
        Constraints:
            Must be logged into HD.

        @param mag:  float - axis magnitude (in g) to override with
        @param axis:  integer - accelerometer axis to override
        @param reset:  integer - 1 to reset a previous override, 0 to override
        @return: 1 if successful, zero otherwise
        """

        rst = integer_to_bytearray(reset)
        sta = float_to_bytearray(mag)
        idx = integer_to_bytearray(axis)
        payload = rst + sta + idx

        message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_hd_ch_id,
                                              message_id=self.MSG_ID_HD_ACCEL_MAX_OVERRIDE,
                                              payload=payload)

        self.logger.debug("override HD accelerometer axis maximum magnitude")

        # 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(mag) + " g.  "
            self.logger.debug("Accelerometer max. axis " + str(axis) + " 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_accel_broadcast_interval_override(self, ms, reset=NO_RESET):
        """
        Constructs and sends the accelerometer broadcast interval override command
        Constraints:
            Must be logged into HD.
            Given interval must be non-zero and a multiple of the HD priority task interval (10 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
        """

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

        message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_hd_ch_id,
                                              message_id=self.MSG_ID_HD_ACCEL_SEND_INTERVAL_OVERRIDE,
                                              payload=payload)

        self.logger.debug("override HD accelerometer 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("Accelerometer 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_set_accel_calibration(self, x_offset, y_offset, z_offset):
        """
            Constructs and sends the HD accelerometer axis calibration offsets message to the HD.

            @param x_offset:  float - calibration offset for X axis
            @param y_offset:  float - calibration offset for Y axis
            @param z_offset:  float - calibration offset for Z axis

            @return: 1 if successful, zero otherwise
        """
        x = float_to_bytearray(x_offset)
        y = float_to_bytearray(y_offset)
        z = float_to_bytearray(z_offset)
        payload = x + y + z

        message = DenaliMessage.build_message(channel_id=DenaliChannels.dialin_to_hd_ch_id,
                                              message_id=self.MSG_ID_HD_ACCEL_SET_CALIBRATION,
                                              payload=payload)

        self.logger.debug("Setting HD accelerometer axis calibration offsets")

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

        # If there is content...
        if received_message is not None:
            self.logger.debug(received_message)
            # str_res = str(flow)
            self.logger.debug("HD accelerometer calibration axis offsets set to : " + str(x_offset) + ", " + str(y_offset) + ", " +
                  str(z_offset) + ".  " + 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