#!/usr/bin/env python
'''
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020, QinetiQ, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of QinetiQ nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
'''

# Original version: Christopher Scianna Christopher.Scianna@us.QinetiQ.com

from Queue import Queue, Empty

import serial
import serial.threaded

import diagnostic_tools
import diagnostic_updater
import rospy

from health_monitor.msg import ReportFault
from sensor_msgs.msg import FluidPressure
from sensor_msgs.msg import Temperature


def bytes_to_float(input_bytes):
    """
    Reinterpret 4 bytes as a 32 bit IEEE 754 floating point number.
    """
    assert len(input_bytes) == 4
    sign = input_bytes[0] >> 7
    extra_bit = input_bytes[1] >> 7
    exponent = ((input_bytes[0] - 128 * sign) << 1) | extra_bit
    mantissa = ((input_bytes[1] - 128 * extra_bit) <<
                16) | (input_bytes[2] << 8) | input_bytes[3]
    return ((-1)**sign) * (1.0 + int(mantissa) / 8388608.0) * (2**(exponent - 127))


class ConnectionTimeoutError(Exception):
    """
    Raised on send/receive timeout over current connection.
    """
    pass


class ConnectionLostError(Exception):
    """
    Raised when current connection is lost.
    """
    pass


class Keller9lxPressureSensorProtocol(serial.threaded.Protocol):
    """
    Serial protocol for Keller 9Lx pressure sensors.
    See https://www.kelleramerica.com/manuals-and-software/manuals/4ld_9ld_protocol_e.pdf.
    """

    def __init__(self, *args, **kwargs):
        super(Keller9lxPressureSensorProtocol, self).__init__(*args, **kwargs)
        self.__receive_buffer = bytearray()
        self.__receive_queue = Queue()
        self.__transport = None

    def connection_made(self, transport):
        rospy.loginfo('Pressure sensor connected')
        self.__transport = transport

    def data_received(self, data):
        rospy.logdebug('Data received from pressure sensor: %s', data)
        self.__receive_buffer += data
        while len(self.__receive_buffer) >= 14:
            # TODO(hidmic): perform CRC16 check
            self.__receive_queue.put(self.__receive_buffer[:14])
            self.__receive_buffer = self.__receive_buffer[14:]

    def send_receive_raw(self, request, timeout=None):
        """
        Send a raw request and wait for a raw response, up to given timeout seconds.
        """
        if not self.__transport.alive:
            raise ConnectionLostError()
        self.__transport.write(request)
        rospy.logdebug('Sent %s to pressure sensor', request)
        try:
            response = self.__receive_queue.get(timeout=timeout)
        except Empty:
            raise ConnectionTimeoutError()
        if not response:
            self.__receive_queue.put(None)  # unblock one, if any
            raise ConnectionLostError()
        rospy.logdebug('Pressure sensor replied %s', response)
        self.__receive_queue.task_done()
        return response

    def connection_lost(self, exception):
        if exception:
            rospy.logerr('Lost connection due to %s', str(exception))
        self.__receive_queue.put(None)  # unblock one, if any
        rospy.loginfo('Pressure sensor disconnected')


class Keller9lxPressureSensorDriverNode(object):
    """
    Driver ROS node for Keller 9Lx pressure sensors.

    Re-publishes pressure and temperature readings.
    """

    VERSION = "3.0x"

    # Function 48: Initialise and Release
    # Request frame: [DevAddress, 48, Check Sum H, Check Sum L]
    FUNCTION48_REQUEST = bytearray([250, 48, 4, 67])

    # Function 73: Request Floating Point Data
    # Request Frame: [DevAddress, 73, Channel, Check Sum H, Check Sum L]
    # where Channel may be:
    #   1: Pressure Sensor 1
    #   2: Pressure Sensor 2,
    #   3: Additional Temp Sensor,
    #   4:Temp Sensor 1,
    #   5:Temp Sensor 2
    FUNCTION73_PRESSURE_REQUEST = bytearray([250, 73, 1, 161, 167])
    FUNCTION73_TEMPERATURE_REQUEST = bytearray([250, 73, 4, 162, 103])

    def __init__(self, name='pressure_sensor', anonymous=False):
        rospy.init_node(name, anonymous=anonymous)
        rospy.set_param('/version_numbers/pressure_sensor', self.VERSION)
        rospy.loginfo('Starting Pressure Sensor Version: [%s]', self.VERSION)

        self.__sensor_port = rospy.get_param('~port', '/dev/ttyUSB0')
        self.__sensor_baud_rate = rospy.get_param('~baudRate', 9600)
        self.__rate = rospy.get_param('~rate', 1.0)

        # Create publishers
        self.__temperature_pub = rospy.Publisher(
            '~temperature', Temperature, queue_size=1
        )

        self.__pressure_pub = diagnostic_tools.DiagnosedPublisher(
            '~pressure', FluidPressure, queue_size=1
        )

        # Create diagnostics updater
        self.__diagnostics_updater = diagnostic_updater.Updater()
        self.__diagnostics_updater.setHardwareID('pressure_sensor')

        # Add pressure range check
        lower_pressure_bound = rospy.get_param('~lower_pressure_bound', 0.)
        upper_pressure_bound = rospy.get_param('~upper_pressure_bound', 100e3)

        def is_pressure_within_range(pressure):
            """
            Check if pressure in Pascals is within configured range.
            """
            within_range = lower_pressure_bound <= pressure
            within_range = within_range and pressure <= upper_pressure_bound
            if not within_range:
                return (
                    diagnostic_tools.Diagnostic.ERROR,
                    '{} Pa outside [{} Pa, {} Pa] range'.format(
                        pressure,
                        lower_pressure_bound,
                        upper_pressure_bound,
                    ),
                    ReportFault.PRESSURE_THRESHOLD_REACHED
                )
            return diagnostic_tools.Diagnostic.OK

        self.__pressure_range_check = diagnostic_tools.HealthCheck(
            name='Pressure within range', test_fn=is_pressure_within_range
        )
        self.__diagnostics_updater.add(self.__pressure_range_check)

        # Add pressure rate check
        min_rate = rospy.get_param('~min_rate', self.__rate / 2)
        max_rate = rospy.get_param('~max_rate', self.__rate * 2)
        check_configuration = diagnostic_tools.PeriodicMessageStatus.configure(
            min_acceptable_period=(1.0 / max_rate),  # in seconds
            max_acceptable_period=(1.0 / min_rate),  # in seconds
            abnormal_diagnostic=diagnostic_tools.Diagnostic(
                diagnostic_tools.Diagnostic.WARN
            ),
            stale_diagnostic=diagnostic_tools.Diagnostic(
                diagnostic_tools.Diagnostic.STALE,
                code=ReportFault.PRESSURE_DATA_STALE
            )
        )
        check = self.__pressure_pub.add_check(
            diagnostic_tools.PeriodicMessageStatus(
                config=check_configuration
            )
        )
        self.__diagnostics_updater.add(check)

        # Add pressure stagnation check
        check_configuration = diagnostic_tools.MessageStagnationCheck.configure(
            stagnation_diagnostic=diagnostic_tools.Diagnostic(
                diagnostic_tools.Diagnostic.ERROR,
                code=ReportFault.PRESSURE_DATA_STAGNATED
            )
        )
        pressure_steady_band = rospy.get_param(
            '~pressure_data_steady_band', 0.0)

        def equal_pressure(first, second):
            """
            Check if two fluid pressure messages are equal.
            """
            pressure_delta = first.fluid_pressure - second.fluid_pressure
            return abs(pressure_delta) < pressure_steady_band

        check = self.__pressure_pub.add_check(
            diagnostic_tools.MessageStagnationCheck(
                equal_op=equal_pressure,
                config=check_configuration
            )
        )
        self.__diagnostics_updater.add(check)

        self.__diagnostics_timer = rospy.Timer(
            rospy.Duration.from_sec(self.__diagnostics_updater.period),
            lambda ev: self.__diagnostics_updater.update()
        )

    MAX_RECONNECTION_ATTEMPTS = 5
    RECONNECTION_CLEARING_PERIOD = 10  # in seconds

    def spin(self):
        """
        Spin driver indefinetely.

        May attempt reconnections.
        """
        reconnection_attempts = 0
        time_of_last_reconnection = None
        while not rospy.is_shutdown():
            try:
                port = serial.Serial(
                    port=self.__sensor_port,
                    baudrate=self.__sensor_baud_rate,
                    parity=serial.PARITY_NONE,
                    stopbits=serial.STOPBITS_ONE,
                    bytesize=serial.EIGHTBITS
                )
                with port:
                    transport = serial.threaded.ReaderThread(
                        port, Keller9lxPressureSensorProtocol
                    )
                    with transport:
                        self.loop(transport)
            except (ConnectionTimeoutError, ConnectionLostError):
                now = rospy.get_time()
                if time_of_last_reconnection:
                    time_between_reconnections = now - time_of_last_reconnection
                    if time_between_reconnections > self.RECONNECTION_CLEARING_PERIOD:
                        reconnection_attempts = 0
                if reconnection_attempts >= self.MAX_RECONNECTION_ATTEMPTS:
                    raise
                reconnection_attempts += 1
                time_of_last_reconnection = now

    def loop(self, transport):
        """
        Main driver loop.
        """
        rospy.loginfo('Initializing pressure sensor')
        transport.protocol.send_receive_raw(
            request=self.FUNCTION48_REQUEST, timeout=10
        )
        rospy.loginfo('Pressure sensor initialized')
        rospy.loginfo('Starting pressure sensor polling loop')
        timeout = 1. / (2 * self.__rate)
        rate = rospy.Rate(self.__rate)
        while not rospy.is_shutdown():
            rospy.logdebug('Reading temperature from pressure sensor')
            response = transport.protocol.send_receive_raw(
                request=self.FUNCTION73_TEMPERATURE_REQUEST,
                timeout=timeout
            )
            response = response[5:]  # skip command echo i.e. first 5 bytes
            temperature_in_celsius = bytes_to_float(response[2:6])
            temperature_in_fahrenheit = temperature_in_celsius * 9 / 5 + 32
            rospy.logdebug('Temperature is %d F', temperature_in_fahrenheit)
            temperature_message = Temperature()
            temperature_message.header.stamp = rospy.Time.now()
            temperature_message.temperature = temperature_in_celsius
            self.__temperature_pub.publish(temperature_message)

            rospy.logdebug('Reading pressure from pressure sensor')
            response = transport.protocol.send_receive_raw(
                request=self.FUNCTION73_PRESSURE_REQUEST,
                timeout=timeout
            )
            response = response[5:]  # skip command echo i.e. first 5 bytes
            pressure_in_bars = bytes_to_float(response[2:6])
            pressure_in_absolute_bars = pressure_in_bars + 1.01325
            pressure_in_pascals = pressure_in_absolute_bars * 100000
            rospy.logdebug('Pressure is %d bar', pressure_in_bars)
            self.__pressure_range_check.test(pressure_in_pascals)
            pressure_message = FluidPressure()
            pressure_message.header.stamp = rospy.Time.now()
            pressure_message.fluid_pressure = pressure_in_pascals
            self.__pressure_pub.publish(pressure_message)

            rate.sleep()


def main():
    """
    Main driver application.
    """
    driver_node = Keller9lxPressureSensorDriverNode()
    try:
        driver_node.spin()
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    main()

#    Open serial connection to sensor
#       baudrate=9600
#       parity=none
#       stopbits=1
#       bytesize=8
#       stopbits=1
#       bytesize=8
#    On start up initialize
#       code 48 setup [DevAddress, 48, Check Sum H, Check Sum L] ex:[250,48,4,67]
#       single unit device adress is 250
#       read data response. Should start [250,48,...]
#    Requesting data
#       code 73: Request Data
#       code 73 setup [DevAddress, 73, Channel*,Check Sum H, Check Sum L]
#       ****Channels:
#        1:Pressure Sensor 1,
#        2:Pressure Sensor 2,
#        3:Additional Temp Sensor,
#        4:Temp Sensor 1,
#        5:Temp Sensor 2)
#       ex: Pressure Request: 	[250,73,1,161,167]
#       ex: Temperature Request:[250,73,4,162,103]
#    Processing Data
#       returned message will be in the form [DevAddress,73,B3,B2,B1,B0,Stat,Check Sum H, Check Sum L]
#       The most significant bit of B3 is the sign
#       The 7 least significant of B3 concatinated with the most significant of B2 is the exponent
#       and the remaining 23 bits of B2,B1,B0 make up the Mantis
#       To convert to decimal for the following equation is used:
