# nxt.sensor.mindsensors module -- Classes implementing Mindsensors sensors
# Copyright (C) 2006,2007  Douglas P Lau
# Copyright (C) 2009  Marcus Wanner, Paulo Vieira, rhn
# Copyright (C) 2010  Marcus Wanner, MindSensors
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.

import logging

from nxt.sensor import Mode, Type
from nxt.sensor.analog import BaseAnalogSensor
from nxt.sensor.digital import BaseDigitalSensor

logger = logging.getLogger(__name__)


class SumoEyes(BaseAnalogSensor):
    """The class to control Mindsensors Sumo sensor. Warning: long range not
    working for my sensor.
    """

    # range: 5-10cm
    class Reading:
        """Contains the reading of SumoEyes sensor. left and right can be True or
        False. If True, then there is something there, if False, then it's empty
        there.
        """

        def __init__(self, raw_reading):
            self.raw = raw_reading
            val = raw_reading.normalized_value  # FIXME: make it rely on raw_value
            right = 600 < val < 700
            both = 700 <= val < 900
            left = 300 < val < 400
            self.left = left or both
            self.right = right or both

        def __str__(self):
            return "(left: " + str(self.left) + ", right: " + str(self.right) + ")"

    def __init__(self, brick, port, long_range=False):
        super().__init__(brick, port)
        self.set_long_range(long_range)

    def set_long_range(self, val):
        """Sets if the sensor should operate in long range mode (12 inches) or
        the short range mode (6 in). val should be True or False.
        """
        if val:
            type_ = Type.LIGHT_INACTIVE
        else:
            type_ = Type.LIGHT_ACTIVE
        self.set_input_mode(type_, Mode.RAW)

    def get_sample(self):
        """Returns the processed meaningful values of the sensor"""
        return self.Reading(self.get_valid_input_values())


class Compassv2(BaseDigitalSensor):
    """Class for the now-discontinued CMPS-Nx sensor. Also works with v1.1 sensors.
    Note that when using a v1.x sensor, some of the commands are not supported!
    To determine your sensor's version, use get_sensor_info().version"""

    I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
    I2C_ADDRESS.update(
        {
            "command": (0x41, "<B"),
            "heading": (0x42, "<H"),
            "x_offset": (0x44, "<h"),  # unsure about signedness for this one
            "y_offset": (0x46, "<h"),  # and this one
            "x_range": (0x48, "<H"),
            "y_range": (0x4A, "<H"),
            "x_raw": (0x4C, "<H"),  # and this one
            "y_raw": (0x4E, "<H"),  # and this one
        }
    )

    class Commands:
        AUTO_TRIG_ON = "A"
        AUTO_TRIG_OFF = "S"
        MAP_HEADING_BYTE = "B"  # map heading to 0-255 range
        MAP_HEADING_INTEGER = "I"  # map heading to 0-36000 (or 3600) range
        SAMPLING_50_HZ = "E"  # set sampling frequency to 50 Hz
        SAMPLING_60_HZ = "U"  # set sampling frequency to 60 Hz
        SET_ADPA_MODE_ON = "N"  # set ADPA mode on
        SET_ADPA_MODE_OFF = "O"  # set ADPA mode off
        BEGIN_CALIBRATION = "C"  # begin calibration
        DONE_CALIBRATION = "D"  # done with calibration
        LOAD_USER_CALIBRATION = "L"  # load user calibration value

    def __init__(self, brick, port, check_compatible=True):
        super().__init__(brick, port, check_compatible)
        self.command(self.Commands.MAP_HEADING_INTEGER)

    def command(self, command):
        value = ord(command)
        self.write_value("command", (value,))

    def get_heading(self):
        return self.read_value("heading")[0]

    get_sample = get_heading


Compassv2.add_compatible_sensor(None, "mndsnsrs", "CMPS")


class DIST(BaseDigitalSensor):
    """Class for the Distance Infrared Sensor"""

    I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
    I2C_ADDRESS.update(
        {
            "command": (0x41, "<B"),
            "distance": (0x42, "<H"),
            "voltage": (0x44, "<H"),
            "type": (0x50, "<B"),
            "no_of_data_points": (0x51, "<B"),
            "min_distance": (0x52, "<H"),
            "max_distance": (0x54, "<H"),
        }
    )

    class Commands:
        TYPE_GP2D12 = "1"  # GP2D12 sensor Module
        TYPE_GP2D120 = "2"  # Short range sensor Module
        TYPE_GP2Y0A21YK = "3"  # Medium range sensor Module
        TYPE_GP2Y0A02YK = "4"  # Long range sensor Module
        TYPE_CUSTOM = "5"  # Custom sensor Module
        POWER_ON = "E"  # Sensor module power on
        POWER_OFF = "D"  # Sensor module power offset
        ADPA_ON = "N"  # ADPA mode on
        ADPA_OFF = "O"  # ADPA mode off (default)

    def command(self, command):
        value = ord(command)
        self.write_value("command", (value,))

    def get_distance(self):
        return self.read_value("distance")[0]

    get_sample = get_distance

    def get_type(self):
        return self.read_value("type")[0]

    def get_voltage(self):
        return self.read_value("voltage")[0]

    def get_min_distance(self):
        return self.read_value("min_distance")[0]

    def get_max_distance(self):
        return self.read_value("max_distance")[0]


DIST.add_compatible_sensor(None, "mndsnsrs", "DIST")


class RTC(BaseDigitalSensor):
    """Class for the RealTime Clock sensor (DS1307)"""

    # TODO: Create a function to set the clock
    # Has no identification
    I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
    I2C_ADDRESS.update(
        {
            "seconds": (0x00, "<B"),
            "minutes": (0x01, "<B"),
            "hours": (0x02, "<B"),
            "day": (0x03, "<B"),
            "date": (0x04, "<B"),
            "month": (0x05, "<B"),
            "year": (0x06, "<B"),
        }
    )
    I2C_DEV = 0xD0

    def __init__(self, brick, port):
        super().__init__(brick, port, check_compatible=False)

    def get_seconds(self):
        gs = self.read_value("seconds")[0]
        gs2 = gs & 0xF  # bitmasks
        gs3 = gs & 0x70
        gs3 = gs3 >> 4
        return str(gs3) + str(gs2)

    def get_minutes(self):
        gm = self.read_value("minutes")[0]
        gm2 = gm & 0xF
        gm3 = gm & 0x70
        gm3 = gm3 >> 4
        return str(gm3) + str(gm2)

    def get_hours(self):
        gh = self.read_value("hours")[0]
        gh2 = gh & 0xF
        gh3 = gh & 0x30
        gh3 = gh3 >> 4
        return str(gh3) + str(gh2)

    def get_day(self):
        gwd = self.read_value("day")[0]
        gwd = gwd & 0x07
        return gwd

    def get_month(self):
        gmo = self.read_value("month")[0]
        gmo2 = gmo & 0xF
        gmo3 = gmo & 0x10
        gmo3 = gmo3 >> 4
        return str(gmo3) + str(gmo2)

    def get_year(self):
        """Last two digits (10 for 2010)"""
        gy = self.read_value("year")[0]
        gy2 = gy & 0xF
        gy3 = gy & 0xF0
        gy3 = gy3 >> 4
        return str(gy3) + str(gy2)

    def get_date(self):
        gd = self.read_value("date")[0]
        gd2 = gd & 0xF
        gd3 = gd & 0x60
        gd3 = gd3 >> 4
        return str(gd3) + str(gd2)

    def hour_mode(self, mode):
        """Writes mode bit and re-enters hours, which is required"""
        if mode == 12 or 24:
            hm = self.read_value("hours")[0]
            hm2 = hm & 0x40
            hm2 = hm2 >> 6
            if mode == 12 and hm2 == 0:  # 12_HOUR = 1
                hm3 = hm + 64
                self.write_value("hours", (hm3,))
            elif mode == 24 and hm2 == 1:  # 24_HOUR = 0
                hm3 = hm - 64
                self.write_value("hours", (hm3,))
        else:
            raise ValueError("Must be 12 or 24!")

    def get_mer(self):
        mer = self.read_value("hours")[0]
        mer2 = mer & 0x40
        mer2 = mer2 >> 6
        if mer2 == 1:
            mer3 = mer & 0x20
            mer3 = mer3 >> 0x10
            return mer3
        else:
            logger.error("cannot get mer in 24-hour mode")

    def get_sample(self):
        """Returns a struct_time() tuple which can be processed by the time module."""
        import time

        return time.struct_time(
            (
                int(self.get_year()) + 2000,
                int(self.get_month()),
                int(self.get_date()),
                int(self.get_hours()),
                int(self.get_minutes()),
                int(self.get_seconds()),
                int(self.get_day()),
                0,  # Should be the Julian Day, but computing that is hard.
                0,  # No daylight savings time to worry about here.
            )
        )


class ACCL(BaseDigitalSensor):
    """Class for Accelerometer sensor"""

    I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
    I2C_ADDRESS.update(
        {
            "sensitivity": (0x19, "B"),
            "command": (0x41, "B"),
            "x_tilt": (0x42, "b"),
            "y_tilt": (0x43, "b"),
            "z_tilt": (0x44, "b"),
            "all_tilt": (0x42, "3b"),
            "x_accel": (0x45, "<h"),
            "y_accel": (0x47, "<h"),
            "z_accel": (0x49, "<h"),
            "all_accel": (0x45, "<3h"),
            "x_offset": (0x4B, "<h"),
            "x_range": (0x4D, "<h"),
            "y_offset": (0x4F, "<h"),
            "y_range": (0x51, "<h"),
            "z_offset": (0x53, "<h"),
            "z_range": (0x55, "<h"),
        }
    )

    class Commands:
        SENS_15G = "1"  # that's 1.5...Alt. 2.5G (sensors older than V3.20)
        SENS_2G = "2"  # Alt .3.3G
        SENS_4G = "3"  # Alt. 6.7G
        SENS_6G = "4"  # Alt. 10G
        X_CALIBRATION = "X"  # Acquire X point calibration
        X_CAL_AND_END = "x"  # X point calibration and end calibration
        Y_CALIBRATION = "Y"  # Acquire Y point calibration
        Y_CAL_AND_END = "y"  # Y point calibration and end calibration
        Z_CALIBRATION = "Z"  # Acquire Z point calibration
        Z_CAL_AND_END = "z"  # Z point calibration and end calibration
        CAL_RESET = "R"  # Reset to factory set calibration
        ADPA_ON = "N"  # Set ADPA mode On
        ADPA_OFF = "O"  # Set ADPA mode Off (default)

    def __init__(self, brick, port, check_compatible=True):
        super().__init__(brick, port, check_compatible)

    def command(self, command):
        value = ord(command)
        self.write_value("command", (value,))

    def get_sensitivity(self):
        return chr(self.read_value("sensitivity")[0])

    def get_tilt(self, axis):
        xyz = str(axis) + "_tilt"
        return self.read_value(xyz)[0]

    def get_all_tilt(self):
        return self.read_value("all_tilt")

    def get_accel(self, axis):
        xyz = str(axis) + "_accel"
        return self.read_value(xyz)[0]

    def get_all_accel(self):
        return self.read_value("all_accel")

    get_sample = get_all_accel

    def get_offset(self, axis):
        xyz = str(axis) + "_offset"
        return self.read_value(xyz)[0]

    def get_range(self, axis):
        xyz = str(axis) + "_range"
        return self.read_value(xyz)[0]

    def set_offset(self, axis, value):
        xyz = str(axis) + "_offset"
        self.write_value(xyz, (value,))

    def set_range(self, axis, value):
        xyz = str(axis) + "_range"
        self.write_value(xyz, (value,))


ACCL.add_compatible_sensor(None, "mndsnsrs", "ACCL-NX")  # Tested with version 'V3.20'


class MTRMUX(BaseDigitalSensor):
    """Class for RCX Motor Multiplexer sensor"""

    I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
    I2C_ADDRESS.update(
        {
            "command": (0x41, "<B"),
            "direction_m1": (0x42, "<B"),
            "speed_m1": (0x43, "<B"),
            "direction_m2": (0x44, "<B"),
            "speed_m2": (0x45, "<B"),
            "direction_m3": (0x46, "<B"),
            "speed_m3": (0x47, "<B"),
            "direction_m4": (0x48, "<B"),
            "speed_m4": (0x49, "<B"),
        }
    )
    I2C_DEV = 0xB4

    class Commands:
        FLOAT = 0x00
        FORWARD = 0x01
        REVERSE = 0x02
        BRAKE = 0x03

    def __init__(self, brick, port, check_compatible=True):
        super().__init__(brick, port, check_compatible)

    def command(self, command):
        self.write_value("command", (command,))

    def set_direction(self, number, value):
        addressname = "direction_m" + str(number)
        self.write_value(addressname, (value,))

    def set_speed(self, number, value):
        addressname = "speed_m" + str(number)
        self.write_value(addressname, (value,))

    def get_direction(self, number):
        addressname = "direction_m" + str(number)
        return self.read_value(addressname)[0]

    def get_speed(self, number):
        addressname = "speed_m" + str(number)
        return self.read_value(addressname)[0]


MTRMUX.add_compatible_sensor(None, "mndsnsrs", "MTRMUX")  # Tested with version 'V2.11'


class LineLeader(BaseDigitalSensor):
    """Class for Line Sensor Array"""

    I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
    I2C_ADDRESS.update(
        {
            "command": (0x41, "<B"),
            "steering": (0x42, "<b"),
            "average": (0x43, "<B"),
            "result": (0x44, "<B"),
            "set_point": (0x45, "<B"),
            "kp": (0x46, "<B"),
            "ki": (0x47, "<B"),
            "kd": (0x48, "<B"),
            "kp_divisor": (0x61, "<B"),
            "ki_divisor": (0x62, "<B"),
            "kd_divisor": (0x63, "<B"),
            # One byte for each sensor, so byte# = sensor#
            "calibrated_reading_byte1": (0x49, "<B"),
            "calibrated_reading_byte2": (0x4A, "<B"),
            "calibrated_reading_byte3": (0x4B, "<B"),
            "calibrated_reading_byte4": (0x4C, "<B"),
            "calibrated_reading_byte5": (0x4D, "<B"),
            "calibrated_reading_byte6": (0x4E, "<B"),
            "calibrated_reading_byte7": (0x4F, "<B"),
            "calibrated_reading_byte8": (0x50, "<B"),
            "all_calibrated_readings": (0x49, "<8B"),
            "w_read_limit": (0x51, "<H"),
            "b_read_limit": (0x59, "<B"),
            "w_cal_data1": (0x64, "<B"),
            "b_cal_data": (0x6C, "<B"),
            "uncal_sensor1_voltage_byte1": (0x74, "<B"),
            "uncal_sensor2_voltage_byte1": (0x76, "<B"),
            "uncal_sensor3_voltage_byte1": (0x78, "<B"),
            "uncal_sensor4_voltage_byte1": (0x7A, "<B"),
            "uncal_sensor5_voltage_byte1": (0x7C, "<B"),
            "uncal_sensor6_voltage_byte1": (0x7E, "<B"),
            "uncal_sensor7_voltage_byte1": (0x80, "<B"),
            "uncal_sensor8_voltage_byte1": (0x82, "<B"),
            "all_uncal_readings": (0x74, "<8B"),
        }
    )

    class Commands:
        CALIBRATE_WHITE = "W"
        CALIBRATE_BLACK = "B"
        SENSOR_SLEEP = "D"
        US_CONFIG = "A"
        EU_CONFIG = "E"
        UNI_CONFIG = "U"
        SENSOR_WAKE = "P"
        COLOR_INVERT = "I"
        COLOR_INVERT_REVERSE = "R"
        SNAPSHOT = "S"

    def __init__(self, brick, port, check_compatible=True):
        super().__init__(brick, port, check_compatible)

    def command(self, command):
        value = ord(command)
        self.write_value("command", (value,))

    def get_steering(self):
        "Value to add to the left and subtract from the right motor's power."
        return self.read_value("steering")[0]

    def get_average(self):
        "Weighted average; greater as line is closer to right edge. 0 for no line."
        return self.read_value("average")[0]

    def get_result(self):
        "Bitmap, one bit for each sensor"
        return self.read_value("result")[0]

    def set_set_point(self, value):
        "Average value for steering to gravitate to. 10 (left) to 80 (right)."
        self.write_value("set_point", (value,))

    def set_pid(self, pid, value):
        addressname = "k" + str(pid)
        self.write_value(addressname, (value,))

    def set_pid_divisor(self, pid, value):
        addressname = "k" + str(pid) + "_divisor"
        self.write_value(addressname, (value,))

    def get_reading(self, number):
        addressname = "calibrated_reading_byte" + str(number)
        return self.read_value(addressname)[0]

    def get_reading_all(self):
        return self.read_value("all_calibrated_readings")

    get_sample = get_reading_all

    def get_uncal_reading(self, number):
        addressname = "uncal_sensor" + str(number) + "_voltage_byte1"
        return self.read_value(addressname)[0]

    def get_uncal_all(self):
        return self.read_value("all_uncal_readings")


LineLeader.add_compatible_sensor(
    None, "mndsnsrs", "LineLdr"
)  # Tested with version 'V1.16'


class Servo(BaseDigitalSensor):
    """Class for Servo sensors"""

    I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
    I2C_ADDRESS.update(
        {
            "command": (0x41, "<B"),
            "servo_1_pos": (0x42, "<H"),
            "servo_2_pos": (0x44, "<H"),
            "servo_3_pos": (0x46, "<H"),
            "servo_4_pos": (0x48, "<H"),
            "servo_5_pos": (0x4A, "<H"),
            "servo_6_pos": (0x4C, "<H"),
            "servo_7_pos": (0x4E, "<H"),
            "servo_8_pos": (0x50, "<H"),
            "servo_1_speed": (0x52, "<B"),
            "servo_2_speed": (0x53, "<B"),
            "servo_3_speed": (0x54, "<B"),
            "servo_4_speed": (0x55, "<B"),
            "servo_5_speed": (0x56, "<B"),
            "servo_6_speed": (0x57, "<B"),
            "servo_7_speed": (0x58, "<B"),
            "servo_8_speed": (0x59, "<B"),
            "servo_1_quick": (0x5A, "<B"),
            "servo_2_quick": (0x5B, "<B"),
            "servo_3_quick": (0x5C, "<B"),
            "servo_4_quick": (0x5D, "<B"),
            "servo_5_quick": (0x5E, "<B"),
            "servo_6_quick": (0x5F, "<B"),
            "servo_7_quick": (0x60, "<B"),
            "servo_8_quick": (0x61, "<B"),
        }
    )
    I2C_DEV = 0xB0

    COMMANDVALUES = {
        "R": (0x52),  # Resume macro execution
        "S": (0x53),  # reset initial position and speed
        "I1": (0x4931),  # store initial position motor 1
        "I2": (0x4932),  # store initial position motor 2
        "I3": (0x4933),  # etc...
        "I4": (0x4934),
        "I5": (0x4935),
        "I6": (0x4936),
        "I7": (0x4937),
        "I8": (0x4938),
        "H": (0x48),  # Halt macro
        "Gx": (0x4778),  # not going to work yet x = variable
        "EM": (0x454D),  # Edit Macro
        "P": (0x50),  # Pause Macro
    }

    class Commands:
        RESUME_MACRO = "R"
        RESET_POS_SPEED = "S"
        STORE_MOTOR_POS_1 = "I1"
        STORE_MOTOR_POS_2 = "I2"
        STORE_MOTOR_POS_3 = "I3"
        STORE_MOTOR_POS_4 = "I4"
        STORE_MOTOR_POS_5 = "I5"
        STORE_MOTOR_POS_6 = "I6"
        STORE_MOTOR_POS_7 = "I7"
        STORE_MOTOR_POS_8 = "I8"
        HALT_MACRO = "H"
        X_TO_VAR = "Gx"  # not going to work yet
        EDIT_MACRO = "EM"
        PAUSE_MACRO = "P"

    def __init__(self, brick, port, check_compatible=True):
        super().__init__(brick, port, check_compatible)

    def command(self, command):
        value = self.COMMANDVALUES[command]
        self.write_value("command", (value,))

    def get_bat_level(self):
        return self.read_value("command")[0]

    def set_position(self, number, value):
        addressname = "servo_" + str(number) + "_pos"
        self.write_value(addressname, (value,))

    def get_position(self, number):
        return self.read_value("servo_" + str(number) + "_pos")[0]

    def set_speed(self, number, value):
        addressname = "servo_" + str(number) + "_speed"
        self.write_value(addressname, (value,))

    def get_speed(self, number):
        return self.read_value("servo_" + str(number) + "_speed")[0]

    def set_quick(self, number, value):
        addressname = "servo_" + str(number) + "_quick"
        self.write_value(addressname, (value,))


Servo.add_compatible_sensor(None, "mndsnsrs", "NXTServo")  # Tested with version 'V1.20'


class MMX(BaseDigitalSensor):
    """Class for MMX sensors"""

    I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
    I2C_ADDRESS.update(
        {
            "command": (0x41, "<B"),
            # Motor Writes
            "encoder_1_target": (0x42, "<l"),
            "speed_1": (0x46, "<B"),
            "seconds_to_run_1": (0x47, "<B"),
            "command_b_1": (0x48, "<B"),
            "command_a_1": (0x49, "<B"),
            "encoder_2_target": (0x4A, "<l"),
            "speed_2": (0x4E, "<B"),
            "seconds_to_run_2": (0x4F, "<B"),
            "command_b_2": (0x50, "<B"),
            "command_a_2": (0x51, "<B"),
            # Motor reads
            "encoder_1_pos": (0x62, "<H"),
            "encoder_2_pos": (0x66, "<H"),
            "status_m1": (0x72, "<B"),
            "status_m2": (0x73, "<B"),
            "tasks_running_m1": (0x76, "<H"),
            "tasks_running_m2": (0x77, "<H"),
            # PID Control
            "p_encoder": (0x7A, "<H"),
            "i_encoder": (0x7C, "<H"),
            "d_encoder": (0x7E, "<H"),
            "p_speed": (0x80, "<H"),
            "i_speed": (0x82, "<H"),
            "d_speed": (0x84, "<H"),
            "pass_count": (0x86, "<B"),
            "tolerance": (0x87, "<B"),
        }
    )
    I2C_DEV = 0x06

    class Commands:
        RESET_PARAMS_ENCODERS = "R"
        ISSUE_SYNCED_COMMANDS = "S"
        MOTOR_1_FLOAT_STOP = "a"
        MOTOR_2_FLOAT_STOP = "b"
        BOTH_FLOAT_STOP = "c"
        MOTOR_1_BRAKE_STOP = "A"
        MOTOR_2_BRAKE_STOP = "B"
        BOTH_BRAKE_STOP = "C"
        MOTOR_1_ENC_RESET = "r"
        MOTOR_2_ENC_RESET = "s"

    def __init__(self, brick, port, check_compatible=True):
        super().__init__(brick, port, check_compatible)

    def command(self, command):
        value = ord(command)
        self.write_value("command", (value,))

    def get_bat_level(self):
        return self.read_value("command")[0]

    def set_encoder_target(self, motor_number, value):
        addressname = "encoder_" + str(motor_number) + "_target"
        self.write_value(addressname, (value,))

    def set_speed(self, motor_number, value):
        addressname = "speed_" + str(motor_number)
        self.write_value(addressname, (value,))

    def set_time_run(self, motor_number, seconds):
        addressname = "seconds_to_run_" + str(motor_number)
        self.write_value(addressname, (seconds,))

    def command_b(self, motor_number, value):
        addressname = "command_b_" + str(motor_number)
        self.write_value(addressname, (value,))

    def command_a(self, motor_number, bit_num, bit_val):
        addressname = "command_a_" + str(motor_number)
        s = self.read_value(addressname)[0]
        # I feel like there must be an easier way to write one bit...
        val = bit_val << bit_num
        if bit_val == 1:
            value = val | s
            self.write_value(addressname, (value,))
            return value  # testing purposes
        elif bit_val == 0:
            val = 1
            val = val << bit_num
            val = val ^ 0xFF
            value = val & s
            self.write_value(addressname, (value,))
            return value

    def get_encoder_pos(self, motor_number):
        addressname = "encoder_" + str(motor_number) + "_pos"
        return self.read_value(addressname)[0]

    def get_motor_status(self, motor_number, bit_num):
        addressname = "status_m" + str(motor_number)
        s = self.read_value(addressname)[0]
        x = 1
        x = x << bit_num
        value = x & s
        value = value >> bit_num
        return value

    def get_tasks(self, motor_number):
        addressname = "tasks_running_m" + str(motor_number)
        return self.read_value(addressname)[0]

    def set_pid(self, pid, target, value):
        addressname = str(pid) + "_" + str(target)
        self.write_value(addressname, (value,))

    def set_pass_count(self, value):
        self.write_value("pass_count", (value,))

    def set_tolerance(self, value):
        self.write_value("tolerance", (value,))


MMX.add_compatible_sensor(None, "mndsnsrs", "NxTMMX")  # Tested with version 'V1.01'


class HID(BaseDigitalSensor):
    """Class for Human Interface Device sensors.
    These are connected to a computer and look like a keyboard to it."""

    I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
    I2C_ADDRESS.update(
        {
            "command": (0x41, "<B"),
            "modifier": (0x42, "<B"),
            "keyboard_data": (0x43, "<B"),
        }
    )
    I2C_DEV = 0x04

    class Commands:
        TRANSMIT = "T"
        ASCII_MODE = "A"
        DIRECT_MODE = "D"

    def __init__(self, brick, port, check_compatible=True):
        super().__init__(brick, port, check_compatible)

    def command(self, command):
        value = ord(command)
        self.write_value("command", (value,))

    def set_modifier(self, mod):
        self.write_value("modifier", (mod,))

    def write_data(self, data):
        data = ord(data)
        self.write_value("keyboard_data", (data,))


HID.add_compatible_sensor(None, "mndsnsrs", "NXTHID")  # Tested with version 'V1.02'


class PS2(BaseDigitalSensor):
    I2C_ADDRESS = BaseDigitalSensor.I2C_ADDRESS.copy()
    I2C_ADDRESS.update(
        {
            "command": (0x41, "<B"),
            "button_set_1": (0x42, "<B"),
            "button_set_2": (0x43, "<B"),
            "x_left_joystick": (0x44, "<b"),
            "y_left_joystick": (0x45, "<b"),
            "x_right_joystick": (0x46, "<b"),
            "y_right_joystick": (0x47, "<b"),
        }
    )

    class ControllerState:
        class Buttons:
            (
                left,
                down,
                right,
                up,
                square,
                cross,
                circle,
                triangle,
                r1,
                r2,
                r3,
                l1,
                l2,
                l3,
            ) = (
                0 for i in range(14)
            )  # 14 zeros

        def __init__(self, buttons_1, buttons_2, left_x, left_y, right_x, right_y):
            self.leftstick = (left_x, left_y)
            self.rightstick = (right_x, right_y)
            buttons_1 = ~buttons_1
            buttons_2 = ~buttons_2
            self.buttons = self.Buttons()
            self.buttons.left = bool(buttons_1 & 0x80)
            self.buttons.down = bool(buttons_1 & 0x40)
            self.buttons.right = bool(buttons_1 & 0x20)
            self.buttons.up = bool(buttons_1 & 0x10)
            self.buttons.square = bool(buttons_2 & 0x80)
            self.buttons.cross = bool(buttons_2 & 0x40)
            self.buttons.circle = bool(buttons_2 & 0x20)
            self.buttons.triangle = bool(buttons_2 & 0x10)
            self.buttons.r1 = bool(buttons_2 & 0x08)
            self.buttons.r2 = bool(buttons_2 & 0x02)
            self.buttons.r3 = bool(buttons_1 & 0x04)
            self.buttons.l1 = bool(buttons_2 & 0x04)
            self.buttons.l2 = bool(buttons_2 & 0x01)
            self.buttons.l3 = bool(buttons_1 & 0x02)

    class Commands:
        POWER_ON = "E"
        POWER_OFF = "D"
        DIGITAL_MODE = "A"
        ANALOG_MODE = "s"
        ADPA_ON = "N"
        ADPA_OFF = "O"

    def __init__(self, brick, port, check_compatible=True):
        super().__init__(brick, port, check_compatible)

    def command(self, command):
        value = ord(command)
        self.write_value("command", (value,))

    def get_joystick(self, xy, lr):
        addressname = str(xy) + "_" + str(lr) + "_joystick"
        return self.read_value(addressname)[0]

    def get_buttons(self, setnum):
        addressname = "button_set_" + str(setnum)
        return self.read_value(addressname)[0]

    def get_sample(self):
        return self.ControllerState(
            self.get_buttons(1),
            self.get_buttons(2),
            self.get_joystick("x", "left"),
            self.get_joystick("y", "left"),
            self.get_joystick("x", "right"),
            self.get_joystick("y", "right"),
        )


PS2.add_compatible_sensor(None, "mndsnsrs", "PSPNX")  # Tested with version 'V2.00'
