import math
from WonderPy.core.wwConstants import WWRobotConstants
from .wwSensorBase import WWSensorBase
from WonderPy.util import wwMath

_rcv = WWRobotConstants.RobotComponentValues
_expected_json_fields = (
    _rcv.WW_SENSOR_VALUE_AXIS_X,
    _rcv.WW_SENSOR_VALUE_AXIS_Y,
    _rcv.WW_SENSOR_VALUE_AXIS_Z,
)


class WWSensorAccelerometer(WWSensorBase):
    """
    Measures linear acceleration along the 3 primary axes. Units are in gravities (9.8m/s/s).
    A robot sitting flat and at rest will have an acceleration of positive 1 on the Z axis.
    A robot being pushed suddenly forward will register a positive acceleration on the Y axis.
    (followed by a negative acceleration when it stops accelerating)

    This sensor can be fairly noisy, and naturally includes the 'acceleration' due to gravity.
    Extensions to this might include low-pass filtering to smooth the data,
    and possibly subtracting out the low-pass value to subtract out gravity.
    """

    def __init__(self, robot):
        super(WWSensorAccelerometer, self).__init__(robot)
        self._x = 0
        self._y = 0
        self._z = 0

    def _important_field_names(self):
        return 'x', 'y', 'z'

    @property
    def x(self):
        return self._x

    @property
    def y(self):
        return self._y

    @property
    def z(self):
        return self._z

    @staticmethod
    def one_gravity_cm_s_s():
        return 980.665

    def parse(self, single_component_dictionary):
        if not self.check_fields_exist(single_component_dictionary, _expected_json_fields):
            return

        x = single_component_dictionary[_rcv.WW_SENSOR_VALUE_AXIS_X]
        y = single_component_dictionary[_rcv.WW_SENSOR_VALUE_AXIS_Y]
        z = single_component_dictionary[_rcv.WW_SENSOR_VALUE_AXIS_Z]

        x, y = wwMath.coords_json_to_api_pos(x, y)

        self._x = x
        self._y = y
        self._z = z

        self._valid = True

    def degrees_z_yz(self):
        """
        degrees away from z axis in the yz plane. note undefined if yz is horizontal
        """
        return math.atan2(self.y, self.z) * math.degrees(1)

    def degrees_y_yz(self):
        """
        degrees away from y axis in the yz plane. note undefined if yz is horizontal
        """
        return math.atan2(self.z, self.y) * math.degrees(1)

    def degrees_z_xz(self):
        """
        degrees away from z axis in the xz plane. note undefined if xz is horizontal
        """
        return math.atan2(self.x, self.z) * math.degrees(1)

    def degrees_x_xz(self):
        """
        degrees away from x axis in the xz plane. note undefined if xz is horizontal
        """
        return math.atan2(self.z, self.x) * math.degrees(1)

    def degrees_y_xy(self):
        """
        degrees away from y axis in the xy plane. note undefined if xy is horizontal
        """
        return math.atan2(self.x, self.y) * math.degrees(1)

    def degrees_x_xy(self):
        """
        degrees away from y axis in the xy plane. note undefined if xy is horizontal
        """
        return math.atan2(self.y, self.x) * math.degrees(1)
