#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

short App::setOverload(short val)
{
    return;
    overloaded = val;
    if (!overloaded)
    {
        shredState = shredStateLast;
    }
    return val;
}

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

ushort App::loop_auto_reverse()
{

#ifdef HAS_POWER
    if (!powerSwitch->isOn(POWER_PRIMARY))
    {
        return E_POWER;
    }
#endif

    switch (shredState)
    {
    case JAMMED:
    {
        shredState = FORWARDING;
        vfd->rev(true);
        jamCounter++;
        SHRED_DEBUG("jammed: reversing");
        timer.in(
            AR_REVERSE_TIME, [](App *app) -> void {
                app->vfd->stop();
                app->shredState = STOPPING;
                SHRED_DEBUG("jammed: stopped");
            },
            this);
        break;
    }
    case STOPPING:
    {
        shredState = FORWARDING;
        SHRED_DEBUG("jammed: stopping");
        timer.in(
            AR_FORWARD_WAIT_TIME, [](App *app) -> void {
                app->shredState = REVERSED;
                SHRED_DEBUG("jammed: stopped");
            },
            this);
        break;
    }
    case REVERSED:
    {
        shredState = FORWARDING;
        if (mLoad->jammed())
        {
            SHRED_DEBUG("reversed: stuck!");
            shredState = STUCK;
            break;
        }
        vfd->fwd(true);
        plunger->reset();
        SHRED_DEBUG("jammed: forward");
        timer.in(
            AR_FORWARDING_TIME, [](App *app) -> void {
                if (app->mLoad->jammed())
                {
                    SHRED_DEBUG("jammed: still jammed, redo autoreverse");
                    app->shredState = JAMMED;
                }
                else
                {
                    SHRED_DEBUG("jammed: continue with");
                    SHRED_DEBUG(app->shredStateLast);
                    SHRED_DEBUG(app->shredState);
                    if (app->shredStateLast && app->shredStateLast != WAITING)
                    {
                        app->shredState = app->shredStateLast;
                    }
                    else
                    {
                        if ((millis() - app->shredStart) / 1000 > 60)
                        {
                            SHRED_DEBUG("jammed: invalid state abort");
                            app->shredState = CANCELLING;
                            return;
                        }
                        SHRED_DEBUG("jammed: invalid state continue with");

                        app->shredState = JAMMED;
                        SHRED_DEBUG(app->shredStateLast);
                        SHRED_DEBUG(app->shredState);
                        SHRED_DEBUG((millis() - app->shredStart) / 1000);
                        app->vfd->stop();
                    }
                    app->plunger->reset();
                    app->plunger->plunge();
                }
            },
            this);
        break;
    }
    }
    return E_OK;
}
short App::shred(short value)
{
    shredState = INIT;
    shredStateLast = 0;
    _state = SHREDDING;
    // {"0":25,"1":1,"2":1,"3":0,"4":0,"5":3478}
    SHRED_DEBUG("START SHREDDING --------------------------");
    powerSwitch->on(POWER_PRIMARY);
    shredStart = millis();
}
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 (newState != WAITING && newState != FORWARDING)
    {
        shredStateLast = shredState;
    }
    shredState = newState;
    Serial.print("Did set new shred state : ");
    Serial.println(shredState);
    return shredState;
}

void App::loopShredCancel()
{

    switch (shredCancelState)
    {
    case INIT:
    {
        SHRED_DEBUG("CANCEL STOP SHREDDER");
        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:
    {
        SHRED_DEBUG("CANCEL : SHREDDED");
        shredCancelState = WAITING;
        plunger->home();
        timer.in(
            SWT_SHREDDED, [](App *app) -> void {
                if (app->plunger->home())
                {
                    SHRED_DEBUG("\t cancel back homed, done");
                    app->shredCancelState = DONE;
                }
                else
                {
                    if (app->shredCancelState != DONE)
                    {
                        SHRED_DEBUG("\t cancel not back homed , try again");
                        app->shredCancelState = SHREDDED;
                    }
                }
            },
            this);
        break;
    }
    case DONE:
    {
        SHRED_DEBUG("CANCELLED SHREDDING ----------------------------------------------------");
        powerSwitch->off(POWER_PRIMARY);
        powerSwitch->off(POWER_SECONDARY);
        shredCancelState = DONE;
        shredState = CANCELLED;
        plunger->reset();
        break;
    }
    }
}

ushort App::loopShred()
{
#ifdef HAS_POWER
    if (_state == SHREDDING)
    {
        if (!powerSwitch->isOn(POWER_PRIMARY))
        {
            Serial.println("got no power");
            return E_POWER;
        }
    }
#endif

    if (shredState == STUCK)
    {
        Serial.println("stuck");
        return;
    }

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

    if (isAutoReversing())
    {
        return loop_auto_reverse();
    }

    if (mLoad->jammed())
    {
        if (!isAutoReversing())
        {
            SHRED_DEBUG("SET JAMMED !!! ");
            plunger->pause();
            shredStateLast = shredState;
            shredState = JAMMED;
            loop_auto_reverse();
        }
        return E_OK;
    }

    short mLoadError = mLoad->ok();

    if (mLoadError != E_OK)
    {
        shredState == CANCELLING;
        vfd->stop();
        Serial.println("cancel!");
    }

    if (_state != App::APP_STATE::SHREDDING && analogRead(CONTROLLINO_A15) < 500)
    {
        Serial.println("shred");
        shred();
        return;
    }

    if (_state != App::APP_STATE::SHREDDING)
    {
        // delay(10);
        // Serial.println("abort, not shredding");
        return;
    }

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

        break;
    }
    case POWERED:
    {
        setShredState(WAITING);
        SHRED_DEBUG("POWERED : homing");
        plunger->home(false);
        timer.in(
            SWT_POWERED, [](App *app) -> void {
                if (app->plunger->home(false))
                {
                    app->setShredState(HOMED);
                    SHRED_DEBUG("\t homed!");
                }
                else
                {
                    SHRED_DEBUG("\t not homed, try again");
                    app->setShredState(POWERED);
                }
            },
            this);
        break;
    }
    case HOMED:
    {
        setShredState(WAITING);
        SHRED_DEBUG("HOMED");
        vfd->fwd(true);
        timer.in(
            SWT_STARTED, [](App *app) -> void {
                app->setShredState(STARTED);
            },
            this);
        break;
    }
    case PLUNGED_SHREDDING:
    {
        setShredState(WAITING);
        if (mLoad->shredding())
        {
            timer.in(
                1000, [](App *app) -> void {
                    SHRED_DEBUG("PLUNGED_SHREDDING : still shredding");
                    app->setShredState(PLUNGED_SHREDDING);
                },
                this);
        }
        else
        {
            if (millis() - mLoad->lastLoad > 2000)
            {
                SHRED_DEBUG("PLUNGED_SHREDDING : seems idle, move on to homing");
                setShredState(UNPOWERED);
            }
            else
            {
                setShredState(PLUNGED_SHREDDING);
            }
        }
        break;
    }
    case STARTED:
    {
        setShredState(WAITING);
        SHRED_DEBUG("STARTED");
        plunger->plunge();
        mLoad->lastLoad = millis();
        SHRED_DEBUG("STARTED : plunging");
        timer.in(
            SWT_PLUNGED, [](App *app) -> void {
                if (app->plunger->plunge())
                {
                    SHRED_DEBUG("STARTED : plunged");
                    app->setShredState(PLUNGED_SHREDDING);
                }
                else
                {
                    SHRED_DEBUG("STARTED : not plunged");
                    app->setShredState(STARTED);
                }
            },
            this);
        break;
    }

    case PLUNGED:
    {
        setShredState(WAITING);
        SHRED_DEBUG("PLUNGED");
        timer.in(
            SWT_PLUNGED, [](App *app) -> void {
                if (app->mLoad->shredding())
                {
                    SHRED_DEBUG("STILL SHREDDING");
                    app->setShredState(PLUNGED);
                }
                else
                {
                    SHRED_DEBUG("SEEMS DONE SHREDDING");
                    app->setShredState(UNPOWERED);
                }
            },
            this);
        break;
    }
    case UNPOWERED:
    {
        setShredState(WAITING);
        SHRED_DEBUG("STOP SHREDDER");
        vfd->stop();
        timer.in(
            SWT_UNPOWERED, [](App *app) -> void {
                app->setShredState(SHREDDED);
            },
            this);
        break;
    }
    case SHREDDED:
    {
        setShredState(WAITING);
        SHRED_DEBUG("SHREDDED : homing");
        plunger->home();
        timer.in(
            SWT_SHREDDED, [](App *app) -> void {
                if (app->plunger->home())
                {
                    SHRED_DEBUG("\t back homed, done");
                    app->setShredState(DONE);
                }
                else
                {

                    if (app->shredState == DONE || app->shredState == RESET)
                    {
                        SHRED_DEBUG("\t weird, tried to home after DONE");
                        SHRED_DEBUG(app->shredState);
                        // return;
                    }
                    SHRED_DEBUG("\t not back homed , try again");
                    app->setShredState(SHREDDED);
                }
            },
            this);
        break;
    }
    case DONE:
    {
        SHRED_DEBUG("DONE SHREDDING ----------------------------------------------------");

        timer.in(
            SWT_SHREDDED_POWER_OFF, [](App *app) -> void {
                if (app->_state == App::APP_STATE::RESET)
                {
                    app->powerSwitch->off(POWER_PRIMARY);
                }
            },
            this);
        plunger->reset();
        _state = App::APP_STATE::RESET;
        shredStateLast = 0;
        shredCancelState = 0;
        shredState = WAITING;
        break;
    }
    default:
        break;
    }
}