/***
 * Warthog Robotics
 * University of Sao Paulo (USP) at Sao Carlos
 * http://www.warthog.sc.usp.br/
 *
 * This file is part of WRJoy project.
 *
 * 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.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 ***/

// Includes class header
#include <WRJoy/joystick.hh>
#include <iostream>

// Includes WRJoy
#include <WRJoy/wrjoy.hh>


// Includes Qt library
#include <QtCore/QtCore>
#include <QtWidgets/QtWidgets>
#include <QtXml/QtXml>

// Includes SDL library
#include <SDL2/SDL.h>

// Includes GEARSystem
#include <GEARSystem/gearsystem.hh>


// Selects WRJoy namespace
using namespace WRJoy;


WRJoy::Joystick::Joystick(QString name, bool createGUI) {
	_team = 0;
	_player = 0;
    _kickEnabled = true;
    _chipKickEnabled = true;

    // Sets joystick name
    _name = name;

    // Initializes run-time variables
    _keepRunning      = false;
    _isOpened         = false;
    _sdlJoystickIndex = -1;
    _sdlJoystick      = 0;

    // Initializes advanced variables
    _runSleepTime = 16.0;
    _deadZoneStandardMaxZero = 5000;
    _deadZoneStandardMinOne  = 32767;
    _deadZonePositiveOnlyMaxZero = 0;
    _deadZonePositiveOnlyMinOne  = 65535;

    // Initializes SDL's Joystick module
    if (SDL_WasInit(SDL_INIT_JOYSTICK) == 0) {
        SDL_Init(SDL_INIT_JOYSTICK);
    }

    // GAMBIARRA!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
    // Apenas para facilitar os testes inciais
    this->initializeAxesConfiguration();
    this->initializeButtonsConfiguration();

    // Creates GUI elements if requested
    if (createGUI) {
        JoystickGUI::JoystickGUI* gui = new JoystickGUI::JoystickGUI(this);
        _widget = gui;
        _hasGUI = true;
    }

    // Sets null elements otherwise
    else {
        _widget = 0;
        _hasGUI = false;
    }

    // Configures the signals connections
    this->configureConnections();

    _controller = new GEARSystem::Controller();
    _controller->connect(QString::fromUtf8("localhost"), 0);
}


WRJoy::Joystick::~Joystick() {
    // Closes the joystick before destroying
    this->close();

    // Quit SDL
    SDL_Quit();

    // Deletes the GUI
    if (_hasGUI) {
        if (_widget->parent() == 0) {
            delete(_widget);
        }
    }
}


QString WRJoy::Joystick::name() {
    // Returns the joystick name
    return(_name);
}


QWidget* WRJoy::Joystick::widget() {
    // Returns the joystick GUI
    return(_widget);
}


void WRJoy::Joystick::configureConnections() {
    QObject::connect(this, &WRJoy::Joystick::stateUpdated, this, &WRJoy::Joystick::processState);

    QObject::connect(this, &WRJoy::Joystick::setSpeedCommand, this, &WRJoy::Joystick::executeSetSpeedCommand);
    QObject::connect(this, &WRJoy::Joystick::holdBallCommand, this, &WRJoy::Joystick::executeHoldBallCommand);
    QObject::connect(this, &WRJoy::Joystick::kickCommand, this, &WRJoy::Joystick::executeKickCommand);
    QObject::connect(this, &WRJoy::Joystick::chipKickCommand, this, &WRJoy::Joystick::executeChipKickCommand);
    QObject::connect(this, &WRJoy::Joystick::kickOnTouchCommand, this, &WRJoy::Joystick::executeKickOnTouchCommand);
    QObject::connect(this, &WRJoy::Joystick::chipKickOnTouchCommand, this, &WRJoy::Joystick::executeChipKickOnTouchCommand);
}


void WRJoy::Joystick::initializeAxesConfiguration() {
    _moveXAnalogAxis = 0;
    _moveYAnalogAxis = 1;

    _turnClockwiseAnalogAxis        = 5;
    _turnCounterClockwiseAnalogAxis = 2;
}

void WRJoy::Joystick::initializeButtonsConfiguration() {
    _movePositiveXDigitalButton = 12;
    _moveNegativeXDigitalButton = 11;
    _movePositiveYDigitalButton = 13;
    _moveNegativeYDigitalButton = 14;
    _turnClockwiseDigitalButton = -1;
    _turnCounterClockwiseDigitalButton = -1;

    _runButton = 5;

    _holdBallButton = 4;

    _kickButton = -1;
    _passButton = -1;
    _chipKickButton = -1;
    _chipPassButton = -1;

    _kickOnTouchButton = 0;
    _passOnTouchButton = 2;
    _chipKickOnTouchButton = -1;
    _chipPassOnTouchButton = -1;

    _playerDownButton = 1;
    _playerUpButton = 3;
    _lastPlayerDownButton = false;
    _lastPlayerUpButton = false;

    _moveAnalogMaxSpeed = 2.0;
    _moveRunAnalogMaxSpeed = 3.0;

    _turnAnalogMaxSpeed = 3.0*3.1416;
    _turnRunAnalogMaxSpeed = 6.0*9.4248;

    _moveDigitalSpeed = 1.0;
    _moveRunDigitalSpeed = 2.0;

    _turnDigitalSpeed = 3.1416;
    _turnRunDigitalSpeed = 6.2832;

    _kickSpeed = 8.0;
    _passSpeed = 3.0;

    _chipKickSpeed = 8.0;
    _chipPassSpeed = 3.0;
}


void WRJoy::Joystick::run() {
    _keepRunning = true;

    // Verifies the keep running flag
    bool runAgain;
    _runLock.lock();
    runAgain = _keepRunning;
    _runLock.unlock();

    // Thread loop
    while (runAgain) {
        // Updates joystick state and emits a signal
        SDL_JoystickUpdate();
        emit stateUpdated();

        // Sleeps for some time
        this->usleep(_runSleepTime*1E3);

        // Verifies the keep running flag
        _runLock.lock();
        runAgain = _keepRunning;
        _runLock.unlock();
    }
}


void WRJoy::Joystick::stop() {
    // Gets run-time mutex lock
    QMutexLocker mutexLocker(&_runLock);

    // Marks the thread to return from its loop
    _keepRunning = false;
}


void WRJoy::Joystick::open(qint8 sdlIndex) {
    // Gets SDL mutex lock
    _sdlLock.lock();

    // Stops the thread
    if (this->isRunning()) {
        this->stop();
        this->wait();
    }

    // Closes previous joystick at SDL
    if (_isOpened) {
        SDL_JoystickClose(_sdlJoystick);
    }

    // Opens the joystick at SDL
    _sdlJoystick = SDL_JoystickOpen(sdlIndex);

    // Sets appropriate flags and emits an 'open' signal if successful
    if (_sdlJoystick != 0) {
        _sdlJoystickIndex = sdlIndex;
        _isOpened = true;

        // Creates axes type configuration list
        qint8 i;
        for (i = 0; i < SDL_JoystickNumAxes(_sdlJoystick); i++) {
            _axesTypes << WRJoy::Joystick::Standard;
        }

        // Releases SDL mutex lock
        _sdlLock.unlock();


        // GAMBIARRA!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        // Configuracao para o controle XBox 360 para testes
        this->setAxisType(0, WRJoy::Joystick::Standard);
        this->setAxisType(1, WRJoy::Joystick::StandardInverse);
        this->setAxisType(2, WRJoy::Joystick::PositiveOnly);
        this->setAxisType(5, WRJoy::Joystick::PositiveOnly);


        emit opened();
    }

    // Sets null elements and emits an 'openFail' signal otherwise
    else {
        _sdlJoystickIndex = -1;
        _isOpened = false;

        // Releases SDL mutex lock
        _sdlLock.unlock();

        emit openFail(QString::fromUtf8(SDL_GetError()));
    }
}


void WRJoy::Joystick::close() {
    // Gets SDL mutex lock
    _sdlLock.lock();

    // Stops the thread
    if (this->isRunning()) {
        this->stop();
        this->wait();
    }

    // Closes the joystick at SDL and sets appropriate flags
    if (_isOpened) {
        SDL_JoystickClose(_sdlJoystick);
        _sdlJoystick      = 0;
        _sdlJoystickIndex = -1;
        _isOpened         = false;
        _axesTypes.clear();
    }

    // Releases SDL mutex lock
    _sdlLock.unlock();

    // Emits a 'closed' signal
    emit closed();
}


bool WRJoy::Joystick::isOpened() {
    return(_isOpened);
}

void WRJoy::Joystick::setSleepTime(double sleepTime) {
    // Sets the thread sleep time
    _runSleepTime = sleepTime;
}

double WRJoy::Joystick::sleepTime() {
    // Returns the thread sleep time
    return(_runSleepTime);
}


void WRJoy::Joystick::setDeadZoneStandardMaxZero(qint16 maxZero) {
    // Sets the dead zone maximum zero value
    _deadZoneStandardMaxZero = maxZero;
}

qint16 WRJoy::Joystick::deadZoneStandardMaxZero() {
    // Returns the dead zone maximum zero value
    return(_deadZoneStandardMaxZero);
}


void WRJoy::Joystick::setDeadZoneStandardMinOne(qint16 minOne) {
    // Sets the dead zone minimum one value
    _deadZoneStandardMinOne = minOne;
}

qint16 WRJoy::Joystick::deadZoneStandardMinOne() {
    // Returns the dead zone minimum one value
    return(_deadZoneStandardMinOne);
}


void WRJoy::Joystick::setDeadZonePositiveOnlyMaxZero(quint16 maxZero) {
    // Sets the dead zone maximum zero value
    _deadZonePositiveOnlyMaxZero = maxZero;
}

quint16 WRJoy::Joystick::deadZonePositiveOnlyMaxZero() {
    // Returns the dead zone maximum zero value
    return(_deadZonePositiveOnlyMaxZero);
}


void WRJoy::Joystick::setDeadZonePositiveOnlyMinOne(quint16 minOne) {
    // Sets the dead zone minimum one value
    _deadZonePositiveOnlyMinOne = minOne;
}

quint16 WRJoy::Joystick::deadZonePositiveOnlyMinOne() {
    // Returns the dead zone minimum one value
    return(_deadZonePositiveOnlyMinOne);
}


QString WRJoy::Joystick::sdlJoystickName() {
    // Gets SDL mutex lock
    QMutexLocker mutexLocker(&_sdlLock);

    // Returns SDL index
    return(QString::fromUtf8(SDL_JoystickName(_sdlJoystick)));
}


qint8 WRJoy::Joystick::sdlJoystickIndex() {
    // Gets SDL mutex lock
    QMutexLocker mutexLocker(&_sdlLock);

    // Returns SDL index
    return(_sdlJoystickIndex);
}


qint8 WRJoy::Joystick::numberAxes() {
    // Gets SDL mutex lock
    QMutexLocker mutexLocker(&_sdlLock);

    // Returns the number of axes
    return(SDL_JoystickNumAxes(_sdlJoystick));
}


qint8 WRJoy::Joystick::numberButtons() {
    // Gets SDL mutex lock
    QMutexLocker mutexLocker(&_sdlLock);

    // Returns the number of buttons
    return(SDL_JoystickNumButtons(_sdlJoystick));
}


double WRJoy::Joystick::axisState(quint8 axis) {
    // Gets SDL mutex lock
    QMutexLocker mutexLocker(&_sdlLock);

    // Conditionates the state
    double state;
	if(_axesTypes.size() < axis)
		return 0.0;

    state = this->conditionSignal(SDL_JoystickGetAxis(_sdlJoystick, axis), _axesTypes.at(axis));

    // Returns the axis state
    return(state);
}


quint8 WRJoy::Joystick::buttonState(quint8 button) {
    // Gets SDL mutex lock
    QMutexLocker mutexLocker(&_sdlLock);

    // Returns the button state
    return(SDL_JoystickGetButton(_sdlJoystick, button));
}


void WRJoy::Joystick::setAxisType(quint8 axis, AxisType type) {
    // Gets SDL mutex lock
    QMutexLocker mutexLocker(&_sdlLock);

    // Sets the axis type to the configuration list
    if (axis < _axesTypes.size()) {
        _axesTypes.replace(axis, type);
    }
}

WRJoy::Joystick::AxisType WRJoy::Joystick::axisType(quint8 axis) {
    // Gets SDL mutex lock
    QMutexLocker mutexLocker(&_sdlLock);

    // Returns the axis type
    AxisType axisType = Standard;

    if (axis < _axesTypes.size()) {
        axisType = _axesTypes.at(axis);
    }

    return(axisType);
}


double WRJoy::Joystick::conditionSignal(qint16 signal, AxisType axisType) {
    // Variables
    double newSignal;

    switch (axisType) {
        case StandardInverse: {
            // Inverts standard axis
            newSignal = -1.0*this->conditionSignal(signal, Standard);
            break;
        }

        case PositiveOnlyInverse: {
            // Inverts positive only axis
            newSignal = -1.0*this->conditionSignal(signal, PositiveOnly);
            break;
        }

        case PositiveOnly: {
            // Gets the positive valued signal
            quint16 positiveSignal = signal + 32768;

            // Subratcts the maximum zero value
            newSignal  = positiveSignal - _deadZonePositiveOnlyMaxZero;
            newSignal /= _deadZonePositiveOnlyMinOne - _deadZonePositiveOnlyMaxZero;

            // Bouns the signal
            newSignal = qBound(0.0, newSignal, 1.0);

            break;
        }

        default:
        case Standard: {
            // Subratcts the maximum zero value
            newSignal  = abs(signal) - _deadZoneStandardMaxZero;
            newSignal /= _deadZoneStandardMinOne - _deadZoneStandardMaxZero;

            // Bouns the signal
            newSignal = qBound(0.0, newSignal, 1.0);

            // Sets the negative signal if needed
            if (signal < 0) {
                newSignal *= -1.0;
            }

            break;
        }
    }

    // Returns the signal
    return(newSignal);
}

void WRJoy::Joystick::processState() {

    // Process team decrement command
    bool playerDown = (this->buttonState(_playerDownButton)) ? true : false;
    if(_lastPlayerDownButton==false && playerDown) {
        if(_player>0) {
            _player--;
            emit playerChanged(_player);
        }
    }
    _lastPlayerDownButton = playerDown;

    // Process the player up button
    bool playerUp = (this->buttonState(_playerUpButton)) ? true : false;
    if(_lastPlayerUpButton==false && playerUp) {
        if(_player<11) {
            _player++;
            emit playerChanged(_player);
        }
    }
    _lastPlayerUpButton = playerUp;

    // Processes the hold ball command
    bool holdBall = (this->buttonState(_holdBallButton)) ? true : false;

    // Processes the kick command
    bool kick = (this->buttonState(_kickButton) || this->buttonState(_passButton)) ? true : false;
    double kickSpeed = 0.0;
    if(kick)
        kickSpeed = (this->buttonState(_kickButton)) ? _kickSpeed : _passSpeed;

    // Processes the chip-kick command
    bool chipKick = (this->buttonState(_chipKickButton) || this->buttonState(_chipPassButton)) ? true : false;
    double chipKickSpeed = 0.0;
    if(chipKick)
        chipKickSpeed = (this->buttonState(_chipKickButton)) ? _chipKickSpeed : _chipPassSpeed;

    // Processes the kick on touch commands
    bool kickOnTouch = (this->buttonState(_kickOnTouchButton) || this->buttonState(_passOnTouchButton)) ? true : false;
    double kickOnTouchSpeed = 0.0;
    if(kickOnTouch)
        kickOnTouchSpeed = (this->buttonState(_kickOnTouchButton)) ? _kickSpeed : _passSpeed;

    // Processes the chip-kick on touch command
    bool chipKickOnTouch = (this->buttonState(_chipKickOnTouchButton) || this->buttonState(_chipPassOnTouchButton)) ? true : false;
    double chipKickOnTouchSpeed = 0.0;
    if(chipKickOnTouch)
        chipKickOnTouchSpeed = (this->buttonState(_chipKickOnTouchButton)) ? _chipKickSpeed : _chipPassSpeed;

    // Processes movement commands
    bool mustRun = (this->buttonState(_runButton)) ? true : false;

    // X, Y
    double xSpeed = 0.0;
    double ySpeed = 0.0;

    double analogXSignal = this->axisState(_moveXAnalogAxis);
    double analogYSignal = this->axisState(_moveYAnalogAxis);

    bool   analogMovement = (!qFuzzyIsNull(analogXSignal) || !qFuzzyIsNull(analogYSignal)) ? true : false;
    if(analogMovement) {
        xSpeed = analogXSignal;
        ySpeed = analogYSignal;
    } else {
        xSpeed = this->buttonState(_movePositiveXDigitalButton) - this->buttonState(_moveNegativeXDigitalButton);
        ySpeed = this->buttonState(_movePositiveYDigitalButton) - this->buttonState(_moveNegativeYDigitalButton);
    }

    xSpeed *= (mustRun) ? _moveRunAnalogMaxSpeed : _moveAnalogMaxSpeed;
    ySpeed *= (mustRun) ? _moveRunAnalogMaxSpeed : _moveAnalogMaxSpeed;

    // Theta
    double thetaSpeed = this->axisState(_turnCounterClockwiseAnalogAxis) - this->axisState(_turnClockwiseAnalogAxis);
    thetaSpeed *= (mustRun) ? _turnRunAnalogMaxSpeed : _turnAnalogMaxSpeed;

    // Emits the commands signals
    emit setSpeedCommand(_team, _player, xSpeed, ySpeed, thetaSpeed);

    emit holdBallCommand(_team, _player, holdBall);

    if(kick && _kickEnabled)
        emit kickCommand(_team, _player, kickSpeed);
    if(kickOnTouch && _kickEnabled)
        emit kickOnTouchCommand(_team, _player, kickOnTouch, kickOnTouchSpeed);

    if(!kick && !kickOnTouch)
        emit kickOnTouchCommand(_team, _player, false, 0.0);

    if(chipKick && _chipKickEnabled)
        emit chipKickCommand(_team, _player, chipKickSpeed);
    if(chipKickOnTouch && _chipKickEnabled)
        emit chipKickOnTouchCommand(_team, _player, chipKickOnTouch, chipKickOnTouchSpeed);
}

void WRJoy::Joystick::executeSetSpeedCommand(quint8 team, quint8 player, double xSpeed, double ySpeed, double thetaSpeed) {
    _controller->setSpeed(team, player, xSpeed, ySpeed, thetaSpeed);
	}

void WRJoy::Joystick::executeHoldBallCommand(quint8 team, quint8 player, bool enable) {
	_controller->holdBall(team, player, enable);
}

void WRJoy::Joystick::executeKickCommand(quint8 team, quint8 player, double speed) {
	_controller->kick(team, player, speed);
}

void WRJoy::Joystick::executeChipKickCommand(quint8 team, quint8 player, double speed) {
	_controller->chipKick(team, player, speed);
}

void WRJoy::Joystick::executeKickOnTouchCommand(quint8 team, quint8 player, bool enable, double speed) {
	_controller->kickOnTouch(team, player, enable, speed);
}

void WRJoy::Joystick::executeChipKickOnTouchCommand(quint8 team, quint8 player, bool enable, double speed) {
	_controller->chipKickOnTouch(team, player, enable, speed);
}

void WRJoy::Joystick::setTeam(quint8 team) {
    _team = team;
}

void WRJoy::Joystick::setPlayer(quint8 player) {
    _player = player;
}

void WRJoy::Joystick::setKickEnable(bool state){
    _kickEnabled = state;
}

void WRJoy::Joystick::setKickChipEnable(bool state){
    _chipKickEnabled = state;
}

void WRJoy::Joystick::setSpeed(double speed)
{
    _moveAnalogMaxSpeed = speed;
}

bool WRJoy::Joystick::getKickEnable()
{
    return _kickEnabled;
}

bool WRJoy::Joystick::getChipKickEnable()
{
    return _chipKickEnabled;
}

double WRJoy::Joystick::getMaxAnalogSpeed()
{
    return _moveAnalogMaxSpeed;
}
