/**
 * @file controlPad.cpp
 * @author Alexander Klein (alex@kleiax.de)
 * @brief 
 * @version 0.1
 * @date 2023-03-30
 * 
 * @copyright Copyright (c) 2023
 * 
 */

#include "controlPad.h"

ControlPad::ControlPad() {
    this->loopDelay = 5;
}

void ControlPad::run() {
    if(!this->connected)
        return;

    if (millis() - this->lastMessageReceive > this->disconnectTime) {
        this->connected = false;
        this->controlInput.buttons = 0;
        this->controlInput.x = 127;
        this->controlInput.y = 127;
        return;
    }

    if (this->lastButtons != controlInput.buttons)
        this->updated = true;

    if (this->menuControl && this->controlInput.buttons > 0 && this->updated) {
        if (this->firstButtonPress) {
            this->menuControl->printMenu();
            this->firstButtonPress = false;
            std::cout << "ControlPad::loop - First menu print" << std::endl;
            return;
        }

        if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Left))
            this->menuControl->left();
        else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Right))
            this->menuControl->right();
        else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Up))
            this->menuControl->up();
        else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Down))
            this->menuControl->down();
        else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Yes))
            this->menuControl->yes();
        else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::No))
            this->menuControl->no();

        this->updated = false;
        this->lastButtons = controlInput.buttons;
    }
    return;
}

void ControlPad::insertData(const uint8_t *data) {
    this->connected = true;
    this->lastMessageReceive = millis();

    uint8_t lastCount = this->controlInput.counter + 1;
    memcpy(&(this->controlInput), data, sizeof(this->controlInput));
    if (lastCount != this->controlInput.counter)
        std::cout << "ControlPad::insertData counter wrong value" << std::endl;

    if (this->controlInput.x > 127 - this->deadZoneX 
        && this->controlInput.x < 127 + this->deadZoneX)
        this->controlInput.x = 127;

    if (this->controlInput.y > 127 - this->deadZoneY 
        && this->controlInput.y < 127 + this->deadZoneY)
        this->controlInput.y = 127;
}