import logging; logger = logging.getLogger("morse." + __name__)
import bge
import math
import morse.core.sensor

class AccelerometerClass(morse.core.sensor.MorseSensorClass):
    """ Accelerometer sensor """

    def __init__(self, obj, parent=None):
        """ Constructor method.

        Receives the reference to the Blender object.
        The second parameter should be the name of the object's parent.
        """
        logger.info('%s initialization' % obj.name)
        # Call the constructor of the parent class
        super(self.__class__,self).__init__(obj, parent)

        # Variables to store the previous position
        self.ppx = 0.0
        self.ppy = 0.0
        self.ppz = 0.0
        # Variables to store the previous velocity
        self.pvx = 0.0
        self.pvy = 0.0
        self.pvz = 0.0
        # Make a new reference to the sensor position
        self.p = self.blender_obj.position
        self.v = [0.0, 0.0, 0.0]            # Velocity
        self.pv = [0.0, 0.0, 0.0]           # Previous Velocity
        self.a = [0.0, 0.0, 0.0]            # Acceleration

        self.local_data['distance'] = 0.0
        self.local_data['velocity'] = [0.0, 0.0, 0.0]
        self.local_data['acceleration'] = [0.0, 0.0, 0.0]

        logger.info('Component initialized')


    def default_action(self):
        """ Compute the speed and accleration of the robot

        The speed and acceleration are computed using the blender tics
        to measure time.
        When computing velocity as v = d / t, and t = 1 / frequency, then
        v = d * frequency
        where frequency is computed from the blender tics and number of skipped
        logic steps for this sensor.
        """
        # Compute the difference in positions with the previous loop
        dx = self.p[0] - self.ppx
        dy = self.p[1] - self.ppy
        dz = self.p[2] - self.ppz
        self.local_data['distance'] = math.sqrt(dx**2 + dy**2 + dz**2)
        logger.debug("DISTANCE: %.4f" % self.local_data['distance'])

        # Store the position in this instant
        self.ppx = self.p[0]
        self.ppy = self.p[1]
        self.ppz = self.p[2]

        # Scale the speeds to the time used by Blender
        self.v[0] = dx * self.frequency
        self.v[1] = dy * self.frequency
        self.v[2] = dz * self.frequency
        logger.debug("SPEED: (%.4f, %.4f, %.4f)" % (self.v[0], self.v[1], self.v[2]))

        self.a[0] = (self.v[0] - self.pvx) * self.frequency
        self.a[1] = (self.v[1] - self.pvy) * self.frequency
        self.a[2] = (self.v[2] - self.pvz) * self.frequency
        logger.debug("ACCELERATION: (%.4f, %.4f, %.4f)" % (self.a[0], self.a[1], self.a[2]))

        # Update the data for the velocity
        self.pvx = self.v[0]
        self.pvy = self.v[1]
        self.pvz = self.v[2]

        # Store the important data
        self.local_data['velocity'] = self.v
        self.local_data['acceleration'] = self.a
