#include <Vector.h>
#include <Streaming.h>
#include <Arduino.h>
#include "app.h"
#include "features.h"

// #define HAS_SHRED_DEBUG

#ifdef HAS_SHRED_DEBUG
#define SHRED_DEBUG(A) Serial.println(A);
#else
#define SHRED_DEBUG(A)
#endif

#define MANUAL_ABORT                             \
    if (app->opModeSwitch->value() == OP_MANUAL) \
    {                                            \
        return;                                  \
    }

short App::plungerCB(short val)
{
    mLoad->loop();
    if (mLoad->jammed() || overloaded || isAutoReversing())
    {
        return false;
    }
    return 1;
}

ushort App::loop_auto_reverse()
{
    switch (shredState)
    {
    case JAMMED:
    {
        this->shredState = FORWARDING;
        this->vfd->rev(true);
        jamCounter++;
        timer.in(
            AR_REVERSE_TIME, [](App *app) -> void
            {
                app->vfd->stop();
                app->shredState = REVERSED;
            },
            this);
        break;
    }
    case STOPPING:
    {
        this->shredState = FORWARDING;
        timer.in(
            AR_FORWARD_WAIT_TIME, [](App *app) -> void
            { app->shredState = REVERSED; },
            this);
        break;
    }
    case REVERSED:
    {
        if (this->mLoad->jammed())
        {
            shredState = STUCK;
            break;
        }
        this->vfd->fwd(true);
        this->plunger->reset();
        this->shredState = FORWARDING;
        timer.in(
            AR_FORWARDING_TIME, [](App *app) -> void
            {
                // MANUAL_ABORT;
                if (app->mLoad->jammed())
                {
                    app->shredState = JAMMED;
                    SHRED_DEBUG("Stll jammed");
                }
                else
                {
                    SHRED_DEBUG(app->shredStateLast);
                    app->shredState = app->shredStateLast;
                }
            },
            this);
        break;
    }
    }
    return E_OK;
}
short App::shred(short value)
{
    shredState = INIT;
    shredStateLast = 0;
    _state = SHREDDING;
    shredStart = millis();
    statusLightError.status_blink(true);
}
bool App::isAutoReversing()
{
    return (shredState == App::SHRED_STATE::JAMMED ||
            shredState == App::SHRED_STATE::REVERSING ||
            shredState == App::SHRED_STATE::REVERSED ||
            shredState == App::SHRED_STATE::STOPPING ||
            shredState == App::SHRED_STATE::FORWARDING) ||
           _state == JAMMED;
}
short App::setShredState(short newState)
{

    if (shredState == App::SHRED_STATE::CANCELLING)
    {
        return App::SHRED_STATE::CANCELLING;
    }

    if (isAutoReversing())
    {
        return App::SHRED_STATE::JAMMED;
    }

    if (shredState != App::SHRED_STATE::WAITING && shredState != App::SHRED_STATE::FORWARDING)
    {
        shredStateLast = shredState;
    }
    shredState = newState;

    return shredState;
}

void App::loopShredCancel()
{

    switch (shredCancelState)
    {
    case INIT:
    {
        plunger->reset();
        if (vfd->direction != VFD::DIRECTION::STOP)
        {
            vfd->stop();
            timer.in(
                SWT_UNPOWERED, [](App *app) -> void
                { app->shredCancelState = SHREDDED; },
                this);
        }
        else
        {
            shredCancelState = SHREDDED;
        }
        break;
    }

    case SHREDDED:
    {
        shredCancelState = WAITING;
        plunger->home();
        timer.in(
            SWT_SHREDDED, [](App *app) -> void
            {
                if (app->plunger->home())
                {
                    app->shredCancelState = DONE;
                }
                else
                {
                    if (app->shredCancelState != DONE)
                    {
                        app->shredCancelState = SHREDDED;
                    }
                }
            },
            this);
        break;
    }
    case DONE:
    {
        SHRED_DEBUG("CANCELLED SHREDDING ----------------------------------------------------");
        shredCancelState = DONE;
        shredState = CANCELLED;
        plunger->reset();
        break;
    }
    }
}

ushort App::loopShred()
{
    if (shredState == STUCK)
    {
        SHRED_DEBUG("stuck");
        return;
    }

    if (shredState == CANCELLING)
    {
        loopShredCancel();
        return E_OK;
    }

    if (this->isAutoReversing())
    {
        return this->loop_auto_reverse();
    }

    if (this->mLoad->jammed())
    {
        if (!this->isAutoReversing())
        {
            SHRED_DEBUG("jammed");
            this->plunger->pause();
            this->setShredState(JAMMED);
            this->loop_auto_reverse();
        }
        return E_OK;
    }

    short mLoadError = this->mLoad->ok();
    /*
    if (mLoadError != E_OK)
    {
        shredState == CANCELLING;
        vfd->stop();
        Serial.println("cancel!");
    }*/

    if (_state != App::APP_STATE::SHREDDING && analogRead(SHRED_START_PIN) > 700)
    {
        shred();
        return;
    }

    switch (shredState)
    {
    case CANCELLING:
    {
        loopShredCancel();
        break;
    }
    case INIT:
    {
        this->setShredState(WAITING);
        this->plunger->reset();
        timer.in(
            SWT_INIT, [](App *app) -> void
            {
                MANUAL_ABORT;
                app->setShredState(POWERED);
            },
            this);

        break;
    }
    case POWERED:
    {
        this->setShredState(WAITING);
        this->plunger->home(false);
        timer.in(
            SWT_POWERED, [](App *app) -> void
            {
                MANUAL_ABORT;
                if (app->plunger->home(false))
                {
                    app->setShredState(HOMED);
                }
                else
                {
                    app->setShredState(POWERED);
                }
            },
            this);
        break;
    }
    case HOMED:
    {
        this->setShredState(WAITING);
        this->vfd->fwd(true);
        timer.in(
            SWT_STARTED, [](App *app) -> void
            {
                MANUAL_ABORT;
                app->setShredState(STARTED);
            },
            this);
        break;
    }
    case PLUNGED_SHREDDING:
    {
        this->setShredState(WAITING);
        if (this->mLoad->shredding())
        {
            timer.in(
                1000, [](App *app) -> void
                {
                    MANUAL_ABORT;
                    app->setShredState(PLUNGED_SHREDDING);
                },
                this);
        }
        else
        {
            if (this->opModeSwitch->value() == OP_MANUAL)
            {
                return;
            }
            if (now - this->mLoad->lastLoad > 2000)
            {
                this->setShredState(UNPOWERED);
            }
            else
            {
                this->setShredState(PLUNGED_SHREDDING);
            }
        }
        break;
    }
    case STARTED:
    {
        this->setShredState(WAITING);
        this->plunger->plunge();
        this->mLoad->lastLoad = millis();
        timer.in(
            SWT_PLUNGED, [](App *app) -> void
            {
                MANUAL_ABORT;
                if (app->isAutoReversing())
                {
                    return;
                }
                if (app->plunger->plunge())
                {
                    app->setShredState(PLUNGED_SHREDDING);
                }
                else
                {
                    app->setShredState(STARTED);
                }
            },
            this);
        break;
    }

    case PLUNGED:
    {
        this->setShredState(WAITING);
        timer.in(
            SWT_PLUNGED, [](App *app) -> void
            {
                MANUAL_ABORT;
                if (app->mLoad->shredding())
                {
                    app->setShredState(PLUNGED);
                }
                else
                {
                    app->setShredState(UNPOWERED);
                }
            },
            this);
        break;
    }
    case UNPOWERED:
    {
        this->setShredState(WAITING);
        this->vfd->stop();
        timer.in(
            SWT_UNPOWERED, [](App *app) -> void
            {
                MANUAL_ABORT;
                app->setShredState(SHREDDED);
            },
            this);
        break;
    }
    case SHREDDED:
    {
        this->setShredState(WAITING);
        this->plunger->home();
        timer.in(
            SWT_SHREDDED, [](App *app) -> void
            {
                MANUAL_ABORT;
                if (app->plunger->home())
                {
                    app->setShredState(DONE);
                }
                else
                {

                    if (app->shredState == DONE || app->shredState == RESET)
                    {
                        SHRED_DEBUG(app->shredState);
                    }
                    app->setShredState(SHREDDED);
                }
            },
            this);
        break;
    }
    case DONE:
    {
        timer.in(
            SWT_SHREDDED_POWER_OFF, [](App *app) -> void
            { MANUAL_ABORT; },
            this);
        this->plunger->reset();
        this->_state = App::APP_STATE::RESET;
        this->shredStateLast = 0;
        this->shredCancelState = 0;
        this->shredState = WAITING;
        this->statusLightError.status_blink(false);
        break;
    }
    default:
        break;
    }
}