#include "GyverEncoder.h"
#include "mbed.h"
#include <debounce.h>

#if defined(PRECISE_ALGORITHM)
const int8_t KNOBDIR[] = {
    0,
    -1,
    1,
    0,
    1,
    0,
    0,
    -1,
    -1,
    0,
    0,
    1,
    0,
    1,
    -1,
    0,
};
int8_t encPos = 0;
#endif

// ================= CONSTRUCTOR =================
Encoder::Encoder()
{
    flags.use_button = true;
}

Encoder::Encoder(PinName clk, PinName dt, PinName sw, bool type)
{

    if (sw != NC) {
        _SW = new DigitalIn(sw, PullUp);
        flags.use_button = true;
    } else {
        flags.use_button = false;
    }
    flags.enc_type = type;

    _CLK = new DigitalIn(clk, PullUp);
    _DT = new DigitalIn(dt, PullUp);

    flags.invBtn = (DEFAULT_BTN_PULL == HIGH_PULL) ? true : false;

#if defined(FAST_ALGORITHM)
    prevState = digitalRead(_CLK);
#else
    prevState = (_CLK->read() | _DT->read() << 1);
#endif

    flags.isDouble_f = false;
    flags.isPress_f = false;
    flags.isRelease_f = false;
    flags.isHolded_f = false;
    flags.butt_flag = false;
    flags.hold_flag = false;

    debounce_timer = Kernel::Clock::now();

    encThread = new Thread(osPriorityNormal, 1024, NULL, "Encoder Thread");
}

void Encoder::start()
{
    //    _CLK->fall(callback(this, &Encoder::encoderThread));
    //    _CLK->rise(callback(this, &Encoder::encoderThread));

    //    _SW->fall(callback(this, &Encoder::encoderThread));
    //    _SW->rise(callback(this, &Encoder::encoderThread));
    encThread->start(callback(this, &Encoder::encoderThread));
}

void Encoder::stop()
{
    encThread->terminate();
}

void Encoder::encoderThread()
{

    for (;;) {
        this->tick();

        ThisThread::sleep_for(1ms);
    }
}

// ================= SET =================
void Encoder::setDirection(bool direction)
{
    if (direction) {
        // uint8_t buf = _CLK;
        // _CLK = _DT;
        // _DT = buf;
    }
}
void Encoder::setPinMode(bool mode)
{
    _CLK->mode((mode) ? PullDown : PullUp);
    _DT->mode((mode) ? PullDown : PullUp);
}
void Encoder::setBtnPinMode(bool mode)
{

    _SW->mode((mode) ? PullDown : PullUp);
    flags.invBtn = (mode) ? 0 : 1;
}
void Encoder::setType(bool type)
{
    flags.enc_type = type;
}
void Encoder::setTickMode(bool tickMode)
{
    flags.enc_tick_mode = tickMode;
}
void Encoder::setFastTimeout(Kernel::Clock::duration timeout)
{
    _fast_timeout = timeout;
}

// ================= IS =================
// повороты
bool Encoder::isTurn()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    if (flags.isTurn_f) {
        flags.isTurn_f = false;
        return true;
    } else
        return false;
}
bool Encoder::isRight()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    if (encState == 2) {
        encState = 0;
        return true;
    } else
        return false;
}
bool Encoder::isLeft()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    if (encState == 1) {
        encState = 0;
        return true;
    } else
        return false;
}
bool Encoder::isRightH()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    if (encState == 4) {
        encState = 0;
        return true;
    } else
        return false;
}
bool Encoder::isLeftH()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    if (encState == 3) {
        encState = 0;
        return true;
    } else
        return false;
}
bool Encoder::isFastR()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    if (flags.isFastR_f) {
        flags.isFastR_f = false;
        return true;
    } else
        return false;
}
bool Encoder::isFastL()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    if (flags.isFastL_f) {
        flags.isFastL_f = false;
        return true;
    } else
        return false;
}

// кнопка
bool Encoder::isPress()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    if (flags.isPress_f) {
        flags.isPress_f = false;
        return true;
    } else
        return false;
}
bool Encoder::isRelease()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    if (flags.isRelease_f) {
        flags.isRelease_f = false;
        return true;
    } else
        return false;
}
bool Encoder::isClick()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    if (flags.isRelease_f) {
        flags.isRelease_f = false;
        return true;
    } else
        return false;
}
bool Encoder::isHolded()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    if (flags.hold_flag && flags.isHolded_f) {
        flags.isHolded_f = false;
        return true;
    } else
        return false;
}
bool Encoder::isSingle()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    if (flags.isSingle_f) {
        flags.isSingle_f = false;
        flags.isDouble_f = false;
        return true;
    } else
        return false;
}
bool Encoder::isDouble()
{
    // if (flags.enc_tick_mode)
    // 	Encoder::tick();
    // if (flags.isDouble_f)
    // {
    // 	flags.isDouble_f = false;
    // 	flags.isSingle_f = false;
    // 	return true;
    // }
    // else
    // 	return false;
    if (flags.isDouble_f) {
        flags.isDouble_f = false;
        return true;
    }
    return false;
}
bool Encoder::isHold()
{
    if (flags.enc_tick_mode)
        Encoder::tick();
    return (SW_state);
}

// ================= TICK =================
void Encoder::tick(bool clk, bool dt, bool sw)
{
    extTick = true;
    flags.extCLK = clk;
    flags.extDT = dt;
    flags.extSW = sw;
    Encoder::tick();
    extTick = false;
}

void Encoder::tick()
{
    thisMls = Kernel::Clock::now();
    debounceDelta = thisMls - debounce_timer;
    // int butstate;

#ifdef ENC_WITH_BUTTON
    //    static etl::debounce<DEBOUNCE_COUNT, HOLD_COUNT, REPEAT_COUNT> key_state;
    //    static etl::debounce<DEBOUNCE_COUNT, HOLD_COUNT, REPEAT_COUNT> key_invState;
    //    // butstate = *_SW;

    //    if (key_state.add(*_SW == 0)) {
    //        if (key_state.is_set() && flags.isRelease_f == true) {
    //            flags.isPress_f = true;
    //            flags.butt_flag = true;
    //            flags.isRelease_f = false;
    //        }
    //        if (key_state.is_held()) {
    //            flags.isHolded_f = true;
    //            flags.butt_flag = false;

    //        } else

    //            if (key_state.is_repeating()) {
    //            flags.isDouble_f = true;
    //        }
    //    }
    //    //    if (flags.butt_flag) {
    //    if (key_invState.add(*_SW == 1)) {
    //        if (key_invState.is_set()) {
    //            flags.isPress_f = false;
    //            flags.isRelease_f = true;
    //            flags.butt_flag = false;
    //            //            }
    //        }
    //    }

    if (flags.use_button) {
        if (!extTick)
            //            if (_SW->read() == 0) { // читаем состояние кнопки SW
            SW_state = _SW->read() ^ flags.invBtn;
        //            } else {
        //                SW_state = false ^ flags.invBtn;
        //            }
        else
            SW_state = flags.extSW;

        if (SW_state && !flags.butt_flag && (debounceDelta > ENC_DEBOUNCE_BUTTON)) {
            flags.butt_flag = true;
            flags.turn_flag = false;
            debounce_timer = thisMls;
            debounceDelta = 0ms;
            flags.isPress_f = true;
            flags.isHolded_f = true;
            flags.doubleAllow = true;
        }
        if (!SW_state && flags.butt_flag && (debounceDelta > ENC_DEBOUNCE_BUTTON)) {
            if (!flags.turn_flag && !flags.hold_flag) { // если кнопка отпущена и ручка не поворачивалась
                flags.turn_flag = false;
                flags.isRelease_f = true;
            }
            if (debounceDelta > ENC_HOLD_TIMEOUT)
                flags.isReleaseHold_f = true;
            flags.butt_flag = false;
            debounce_timer = thisMls;
            debounceDelta = 0ms;
            flags.hold_flag = false;

            if (flags.doubleAllow && !flags.doubleFlag) {
                flags.doubleFlag = true;
                flags.countFlag = false;
            } else {
                flags.countFlag = true;
            }
        }
        if (flags.doubleFlag && debounceDelta > ENC_DOUBLE_TIMEOUT) {
            if (!flags.turn_flag) {
                if (!flags.countFlag)
                    flags.isSingle_f = true;
                else
                    flags.isDouble_f = true;
            }
            flags.doubleFlag = false;
        }
        if (flags.butt_flag && debounceDelta > ENC_HOLD_TIMEOUT && !flags.turn_flag) {
            if (SW_state) {
                flags.hold_flag = true;
                flags.doubleAllow = false;
            } else {
                flags.butt_flag = false;
                flags.hold_flag = false;
                debounce_timer = thisMls;
                debounceDelta = 0ms;
            }
        }
    }
#endif

#if defined(FAST_ALGORITHM)
    uint8_t curState = (extTick) ? (flags.extCLK) : (digitalRead(_CLK));

    if (curState != prevState
#if (ENC_DEBOUNCE_TURN > 0)
        && (debounceDelta > ENC_DEBOUNCE_TURN)
#endif
    ) {
        encState = 0;
        turnFlag = !turnFlag;
        if (turnFlag || !flags.enc_type) {
            if (((extTick) ? (flags.extDT) : digitalRead(_DT)) != prevState) {
                encState = 1;
            } else {
                encState = 2;
            }
        }

#elif defined(BINARY_ALGORITHM)
    uint8_t curState = (extTick) ? (flags.extCLK | (flags.extDT << 1)) : (_CLK->read() | (_DT->read() << 1));

    if (curState != prevState
        //#if (ENC_DEBOUNCE_TURN > 0)
        && (debounceDelta > ENC_DEBOUNCE_TURN)
        //#endif
    ) {
        encState = 0;
        if (curState == 0b11) {
            switch (prevState) {
            case 0b10:
                encState = 1;
                break;
            case 0b01:
                encState = 2;
                break;
            }
        } else if (curState == 0b00 && !flags.enc_type) {
            switch (prevState) {
            case 0b01:
                encState = 1;
                break;
            case 0b10:
                encState = 2;
                break;
            }
        }

#elif defined(PRECISE_ALGORITHM)
    uint8_t curState = (extTick) ? (flags.extCLK | (flags.extDT << 1)) : (_CLK->read() | (_DT->read() << 1));

    if (prevState != curState
        //#if (ENC_DEBOUNCE_TURN > 0)
        //        && (debounceDelta > ENC_DEBOUNCE_TURN)
        //#endif
    ) {
        encState = 0;
        encPos += KNOBDIR[curState | (prevState << 2)];
        if (flags.enc_type) {
            if (curState == 0x3 && encPos != 0) {
                encState = (encPos == 4) ? 1 : 2;
                encPos = 0;
            }
        } else {
            if ((curState == 0x3 || !curState) && encPos != 0) {
                encState = (encPos == 2) ? 1 : 2;
                encPos = 0;
            }
        }
#endif

        if (encState != 0) {
            flags.isTurn_f = true;
            if (thisMls - fast_timer < _fast_timeout) {
                if (encState == 1)
                    flags.isFastL_f = true;
                else if (encState == 2)
                    flags.isFastR_f = true;
                fast_timer = thisMls;
            } else
                fast_timer = thisMls;
#ifdef ENC_WITH_BUTTON
                // if (flags.use_button)
                // 	if (SW_state)
                // 		encState += 2;
#endif
        }
        prevState = curState;
        flags.turn_flag = true;
        debounce_timer = thisMls;
        debounceDelta = 0ms;
    }
}