#!/usr/bin/env python
#_*_ coding:utf-8 _*_

"""
    Sensor class for the arudino_python package
    History:
        20200330:增加获取电池剩余电量函数.
        20200423:增加获取电流函数
"""
import roslib; roslib.load_manifest('ros_arduino_python')
import rospy
import numpy as np
from decimal import Decimal
from sensor_msgs.msg import Range
from ros_arduino_msgs.msg import *


LOW = 0
HIGH = 1

INPUT = 0
OUTPUT = 1

class MessageType:
    ANALOG     = 0
    DIGITAL    = 1
    INFRARED   = 2
    RANGE      = 3
    FLOAT      = 4
    INT        = 5
    BOOL       = 6
    ULTRASONIC = 7

class Sensor(object):
    def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs):
        self.controller = controller
        self.name = name
        self.pin = pin
        self.rate = rate
        self.direction = direction

        self.frame_id = frame_id
        self.value = None

        self.t_delta = rospy.Duration(1.0 / self.rate)
        self.t_next = rospy.Time.now() + self.t_delta


    def poll(self):
        now = rospy.Time.now()
        if now > self.t_next:
            if self.direction == "input":
                try:
                    self.value = self.read_value()
                    #rospy.loginfo("read value: "+str(self.value))
                except:
                    try:
                        # try again
                        rospy.logwarn("Failed to read sensor values, try again. ")
                        self.ack = self.read_value()
                    except:
                        rospy.logerr("Sensor read value error !")
                        return
            else:
                try:
                    self.ack = self.write_value()
                except:
                    rospy.logerr("Sensor write value error !")
                    return

            # For range sensors, assign the value to the range message field
            if self.message_type == MessageType.RANGE:
                self.msg.range = self.value
            elif self.message_type == MessageType.INFRARED:
                self.msg.front_dist = self.value[0]
                self.msg.left_dist  = self.value[1]
                self.msg.right_dist = self.value[2]
            elif self.message_type == MessageType.ULTRASONIC:
                self.msg.front_dist = self.value[0]
                self.msg.left_dist  = self.value[1]
                self.msg.right_dist = self.value[2]
            else:
                self.msg.value = self.value

            # Add a timestamp and publish the message
            self.msg.header.stamp = rospy.Time.now()
            self.pub.publish(self.msg)

            self.t_next = now + self.t_delta

class AnalogSensor(Sensor):
    def __init__(self, *args, **kwargs):
        super(AnalogSensor, self).__init__(*args, **kwargs)

        self.message_type = MessageType.ANALOG
        self.msg = Analog()
        self.msg.header.frame_id = self.frame_id

        self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5)
        self.value = LOW

    def read_value(self):
        return self.controller.analog_read(self.pin)

    def write_value(self, value):
        return self.controller.analog_write(self.pin, value)

class AnalogFloatSensor(Sensor):
    def __init__(self, *args, **kwargs):
        super(AnalogFloatSensor, self).__init__(*args, **kwargs)

        self.message_type = MessageType.ANALOG

        self.msg = AnalogFloat()
        self.msg.header.frame_id = self.frame_id

        self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5)

        if self.direction == "output":
            self.controller.pin_mode(self.pin, OUTPUT)
        else:
            self.controller.pin_mode(self.pin, INPUT)

        self.value = LOW

    def read_value(self):
        return self.controller.analog_read(self.pin)

    def write_value(self, value):
        return self.controller.analog_write(self.pin, value)

class DigitalSensor(Sensor):
    def __init__(self, *args, **kwargs):
        super(DigitalSensor, self).__init__(*args, **kwargs)

        self.message_type = MessageType.DIGITAL
        self.msg = Digital()
        self.msg.header.frame_id = self.frame_id

        self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5)
        self.value = LOW

    def read_value(self):
        return self.controller.digital_read(self.pin)

    def write_value(self):
        # Alternate HIGH/LOW when writing at a fixed rate
        self.value = not self.value
        return self.controller.digital_write(self.pin, self.value)

class RangeSensor(Sensor):
    def __init__(self, *args, **kwargs):
        super(RangeSensor, self).__init__(*args, **kwargs)

        self.message_type = MessageType.RANGE
        self.msg = Range()
        self.msg.header.frame_id = self.frame_id
        self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5)

    def read_value(self):
        self.msg.header.stamp = rospy.Time.now()

class MyRangeSensor(Sensor):
    def __init__(self, *args, **kwargs):
        super(MyRangeSensor, self).__init__(*args, **kwargs)

        self.message_type = MessageType.INFRARED
        self.msg = InfraredSensors()
        self.msg.header.frame_id = self.frame_id

        self.pub = rospy.Publisher("~sensor/" + self.name, InfraredSensors, queue_size=2)

    def read_value(self):
        self.msg.header.stamp = rospy.Time.now()

class MyUltrasonicSensor(Sensor):
    def __init__(self, *args, **kwargs):
        super(MyUltrasonicSensor, self).__init__(*args, **kwargs)

        self.message_type = MessageType.ULTRASONIC
        self.msg = UltrasonicSensors()
        self.msg.header.frame_id = self.frame_id

        self.pub = rospy.Publisher("~sensor/" + self.name, UltrasonicSensors, queue_size=2)

    def read_value(self):
        self.msg.header.stamp = rospy.Time.now()

class SonarSensor(RangeSensor):
    def __init__(self, *args, **kwargs):
        super(SonarSensor, self).__init__(*args, **kwargs)
        self.msg.radiation_type = Range.ULTRASOUND

class MySonarSensor(MyUltrasonicSensor):
    def __init__(self, *args, **kwargs):
        super(MySonarSensor, self).__init__(*args, **kwargs)
        self.msg.radiation_type = Range.ULTRASOUND

class IRSensor(RangeSensor):
    def __init__(self, *args, **kwargs):
        super(IRSensor, self).__init__(*args, **kwargs)
        self.msg.radiation_type = Range.INFRARED

class MyIRSensor(MyRangeSensor):
    def __init__(self, *args, **kwargs):
        super(MyIRSensor, self).__init__(*args, **kwargs)
        self.msg.radiation_type = Range.INFRARED

class GP2Y0A41(MyIRSensor):
    def __init__(self, *args, **kwargs):
        super(GP2Y0A41, self).__init__(*args, **kwargs)

        self.msg.field_of_view = 0.01
        self.msg.min_range = 0.04
        self.msg.max_range = 0.30

    def read_value(self):
        values = self.controller.detect_distance()
        distances = np.array([values[0], values[1], values[2]])
        return distances/100.0

class IR2Y0A02(IRSensor):
    def __init__(self, *args, **kwargs):
        super(IR2Y0A02, self).__init__(*args, **kwargs)

        self.msg.field_of_view = 0.001
        self.msg.min_range = 0.200
        self.msg.max_range = 1.500

    def read_value(self):
        value = self.controller.analog_read(self.pin)

        try:
            volts = value*0.0048828125;
            distance = 65*pow(volts, -1.10)
        except:
            return self.msg.min_range

        # Convert to meters
        distance /= 100.000
        dist = round(float(distance), 3)

        # If we get a spurious reading, set it to the max_range
        if dist > self.msg.max_range: dist = self.msg.max_range
        if dist < self.msg.min_range: dist = self.msg.min_range

        return dist

class US025(MySonarSensor):
    def __init__(self, *args, **kwargs):
        super(US025, self).__init__(*args, **kwargs)

        self.msg.field_of_view = 0.01
        self.msg.min_range = 0.03
        self.msg.max_range = 6

    def read_value(self):
        values = self.controller.detect_ultrasonic_distance()
        distances = np.array([values[0], values[1], values[2]])
        return distances/100.0

class MotorTotalCurrent(AnalogFloatSensor):
    def __init__(self, *args, **kwargs):
        super(MotorTotalCurrent, self).__init__(*args, **kwargs)

    def read_value(self):
        midVal = mylist[15]
        result = (midVal/1024.0*4523.00 - 4523.00/2)/100
        return Decimal(result).quantize(Decimal('0.00'))

class BatPercent(DigitalSensor):
    def __init__(self, *args, **kwargs):
        super(BatPercent, self).__init__(*args, **kwargs)

    def read_value(self):
        percent = self.controller.getBatPercent()
        return int(percent)

class CurrentValue(DigitalSensor):
    def __init__(self, *args, **kwargs):
        super(CurrentValue, self).__init__(*args, **kwargs)

    def read_value(self):
        currentvalue = self.controller.getCurrentValue()
        #rospy.loginfo("read currentvalue: "+currentvalue)
        return float(currentvalue)


if __name__ == '__main__':
    rospy.loginf("arduino sensor main function ...")
