from nbus_slave import *


class ImuSlave(NbusSlave):

    def __init__(self, module, comm_port):
        NbusSlave.__init__(self, module, comm_port)

    def evaluate_param_value(self, sensor, param, val):
        value = -1
        if param == PARAM_RANGE:
            value = 2 ** (val + 1)  # +/- 2, 4, 8, 16g
        if param == PARAM_SAMPLERATE:
            if val == 1:
                value = 562.5
            if val == 3:
                value = 281.3
            if val == 5:
                value = 187.5
            if val == 7:
                value = 140.6
            if val == 10:
                value = 102.3
            if val == 15:
                value = 70.3
            if val == 22:
                value = 48.9
            if val == 31:
                value = 32.2
            if val == 63:
                value = 17.6
            if val == 127:
                value = 8.8
            if val == 255:
                value = 4.4
            if val == 513:
                value = 2.2
            if val == 1022:
                value = 1.1
            if val == 2044:
                value = 0.55
            if val == 4095:
                value = 0.27

        if param == PARAM_FILTER:
            value = (val - 1)

        self.slave_params[sensor][param] = value

        return value

    def cmd_sensor_get_data_IMU(self, sensor_index):
        [x, y, z] = self._cmd_sensor_get_data(sensor_index)
        z = z - 16384 / (self.slave_params[sensor_index][PARAM_RANGE]/2)
        if x > 32768:
            x = x - 65535
        if y > 32768:
            y = y - 65535
        if z > 32768:
            z = z - 65535
        x = x/16384 * (self.slave_params[sensor_index][PARAM_RANGE]/2)
        y = y/16384 * (self.slave_params[sensor_index][PARAM_RANGE]/2)
        z = z/16384 * (self.slave_params[sensor_index][PARAM_RANGE]/2)
        return [x, y, z]