# -*- coding: utf-8 -*-

########################################
# HXbot controller module              #
# EoF 2016 EoF@itphx.ru                #
########################################

import HX
import threading
import array
import struct
import select
import time
from fcntl import ioctl


class HXjoystickClass():

    def __init__(self):
        super(HXjoystickClass, self).__init__()

        self.__e = None
        self.__t = None
        self.ready = False

        self.device = open(HX.JOYSTICK_PATH, 'rb')

        self.name = ""
        self.axis_states = {}
        self.button_states = {}
        self.axis_map = []
        self.button_map = []
        self.axis_names = HX.JOYSTICK_AXIS_NAMES
        self.button_names = HX.JOYSTICK_BUTTON_NAMES

        self.online = False
        self.is_running = False
        self.last_command_time = 0.0

        # Get the device name.
        buf = array.array('B', [0] * 64)
        # JSIOCGNAME(len)
        ioctl(self.device, 0x80006a13 + (0x10000 * len(buf)), buf)
        self.name = buf.tostring()
        #print('Device name: %s' % js_name)

        # Get number of axes and buttons.
        buf = array.array('B', [0])
        ioctl(self.device, 0x80016a11, buf)  # JSIOCGAXES
        num_axes = buf[0]

        buf = array.array('B', [0])
        ioctl(self.device, 0x80016a12, buf)  # JSIOCGBUTTONS
        num_buttons = buf[0]

        # Get the axis map.
        buf = array.array('B', [0] * 0x40)
        ioctl(self.device, 0x80406a32, buf)  # JSIOCGAXMAP

        for axis in buf[:num_axes]:
            axis_name = self.axis_names.get(axis, 'unknown(0x%02x)' % axis)
            self.axis_map.append(axis_name)
            self.axis_states[axis_name] = 0.0

        # Get the button map.
        buf = array.array('H', [0] * 200)
        ioctl(self.device, 0x80406a34, buf)  # JSIOCGBTNMAP

        for btn in buf[:num_buttons]:
            btn_name = self.button_names.get(btn, 'unknown(0x%03x)' % btn)
            self.button_map.append(btn_name)
            self.button_states[btn_name] = 0

        #print('%d axes found: %s' % (num_axes, ', '.join(self.axis_map)))
        #print('%d btns found: %s' % (num_buttons, ', '.join(self.button_map)))

    def start(self):
        self.__e = threading.Event()
        self.__t = threading.Thread(target=self.__process, args=(self.__e, ))

        self.__t.start()
        self.is_running = True

    def stop(self):
        self.__e.set()
        self.__t.join()
        self.is_running = False

        self.__e = None
        self.__t = None

    def __process(self, stopRequest):
        while not stopRequest.is_set():
            r, w, e = select.select([self.device], [], [], 0.8)
            if self.device in r:
                try:
                    evbuf = self.device.read(8)
                    self.online = True
                except OSError:
                    self.online = False
                    self.ready = False
                    continue

                if evbuf:
                    time_, value, type, number = struct.unpack('IhBB', evbuf)

                    # Save last command time
                    self.last_command_time = time.clock()

                    #if type & 0x80:
                        #print("(initial)")

                    if type & 0x01:
                        button = self.button_map[number]
                        if button:
                            self.button_states[button] = value
                            #if value:
                                #print("%s pressed" % (button))
                            #else:
                                #print("%s released" % (button))

                    if type & 0x02:
                        axis = self.axis_map[number]
                        if axis:
                            fvalue = value / 32767.0
                            self.axis_states[axis] = fvalue
                            #print("%s: %.3f" % (axis, fvalue))

                # Switch ready property
                if (self.button_states[HX.JOYSTICK_START_BTN_1] and
                    self.button_states[HX.JOYSTICK_START_BTN_2]):
                    if self.ready:
                        self.ready = False
                    else:
                        self.ready = True

            # Timeout? Relax...
            elif time.clock() - self.last_command_time >= \
                HX.JOYSTICK_READY_TIMEOUT / 100:
                self.ready = False

    def get_m(self):
        axis_move = self.axis_states[HX.JOYSTICK_MOVE_AXIS]
        axis_steer = self.axis_states[HX.JOYSTICK_STEER_AXIS]

        axis_head_v = self.axis_states[HX.JOYSTICK_HEAD_V_AXIS]
        axis_head_h = self.axis_states[HX.JOYSTICK_HEAD_H_AXIS]

        # STOP (FLOAT)
        if (axis_move >= -1 / 8) and (axis_move <= 1 / 8):
            move = HX.MOVE_FLOAT

        # MOVE FORWARD
        elif (axis_move >= -2 / 8) and (axis_move < -1 / 8):
            move = HX.MOVE_FWD1
        elif (axis_move >= -3 / 8) and (axis_move < -2 / 8):
            move = HX.MOVE_FWD2
        elif (axis_move >= -4 / 8) and (axis_move < -3 / 8):
            move = HX.MOVE_FWD3
        elif (axis_move >= -5 / 8) and (axis_move < -4 / 8):
            move = HX.MOVE_FWD4
        elif (axis_move >= -6 / 8) and (axis_move < -5 / 8):
            move = HX.MOVE_FWD5
        elif (axis_move >= -7 / 8) and (axis_move < -6 / 8):
            move = HX.MOVE_FWD6
        elif (axis_move >= -1) and (axis_move < -7 / 8):
            move = HX.MOVE_FWD7

        # MOVE BACKWARD
        elif (axis_move > 1 / 8) and (axis_move <= 2 / 8):
            move = HX.MOVE_REV1
        elif (axis_move > 2 / 8) and (axis_move <= 3 / 8):
            move = HX.MOVE_REV2
        elif (axis_move > 3 / 8) and (axis_move <= 4 / 8):
            move = HX.MOVE_REV3
        elif (axis_move > 4 / 8) and (axis_move <= 5 / 8):
            move = HX.MOVE_REV4
        elif (axis_move > 5 / 8) and (axis_move <= 6 / 8):
            move = HX.MOVE_REV5
        elif (axis_move > 6 / 8) and (axis_move <= 7 / 8):
            move = HX.MOVE_REV6
        elif (axis_move > 7 / 8) and (axis_move <= 1):
            move = HX.MOVE_REV7

        # CENTER (FLOAT)
        if (axis_steer >= -1 / 8) and (axis_steer <= 1 / 8):
            steer = HX.STEER_CENTER

        # STEER LEFT
        elif (axis_steer >= -2 / 8) and (axis_steer < -1 / 8):
            steer = HX.STEER_LEFT1
        elif (axis_steer >= -3 / 8) and (axis_steer < -2 / 8):
            steer = HX.STEER_LEFT2
        elif (axis_steer >= -4 / 8) and (axis_steer < -3 / 8):
            steer = HX.STEER_LEFT3
        elif (axis_steer >= -5 / 8) and (axis_steer < -4 / 8):
            steer = HX.STEER_LEFT4
        elif (axis_steer >= -6 / 8) and (axis_steer < -5 / 8):
            steer = HX.STEER_LEFT5
        elif (axis_steer >= -7 / 8) and (axis_steer < -6 / 8):
            steer = HX.STEER_LEFT6
        elif (axis_steer >= -1) and (axis_steer < -7 / 8):
            steer = HX.STEER_LEFT7

        # STEER RIGHT
        elif (axis_steer > 1 / 8) and (axis_steer <= 2 / 8):
            steer = HX.STEER_RIGHT1
        elif (axis_steer > 2 / 8) and (axis_steer <= 3 / 8):
            steer = HX.STEER_RIGHT2
        elif (axis_steer > 3 / 8) and (axis_steer <= 4 / 8):
            steer = HX.STEER_RIGHT3
        elif (axis_steer > 4 / 8) and (axis_steer <= 5 / 8):
            steer = HX.STEER_RIGHT4
        elif (axis_steer > 5 / 8) and (axis_steer <= 6 / 8):
            steer = HX.STEER_RIGHT5
        elif (axis_steer > 6 / 8) and (axis_steer <= 7 / 8):
            steer = HX.STEER_RIGHT6
        elif (axis_steer > 7 / 8) and (axis_steer <= 1):
            steer = HX.STEER_RIGHT7

        # Head V
        if axis_head_v > 0:
            head_v = int(HX.HEAD_V_CENTER - HX.HEAD_V_UP_RANGE * axis_head_v)
        else:
            head_v = int(HX.HEAD_V_CENTER - HX.HEAD_V_DOWN_RANGE * axis_head_v)

        # Head H
        if axis_head_h > 0:
            head_h = int(HX.HEAD_H_CENTER - HX.HEAD_H_R_RANGE * axis_head_h)
        else:
            head_h = int(HX.HEAD_H_CENTER - HX.HEAD_H_L_RANGE * axis_head_h)

        # Return
        return (True, move, steer, head_v, head_h)