#include <Vector.h>
#include <Streaming.h>
#include <Arduino.h>

#include "app.h"
#include "features.h"

#include <MemoryFree.h>
#include "Version.h"

static Addon *addonsArray[6];

short App::ok()
{
    return E_OK;
}

App::App() : Addon("APP", APP, 1 << STATE),
             shredButton(ProximitySensor(SHRED_START_PIN)),
             statusLightOk(StatusLight(CONTROLLINO_D2)),
             statusLightError(StatusLight(CONTROLLINO_D3)),
#ifdef HAS_DIRECTION_SWITCH
             dirSwitch(new DirectionSwitch()),
#endif
#ifdef HAS_VFD
             vfd(new VFD()),
#endif
#ifdef MOTOR_LOAD_PIN
             mLoad(new MotorLoad(MOTOR_LOAD_PIN)),
#endif
#ifdef HAS_PLUNGER
             plunger(new Plunger(
                 PLUNGER_LIMIT_UP_1,
                 PLUNGER_LIMIT_DOWN_1,
                 PLUNGER_MOTOR_1_DIR_PIN,
                 PLUNGER_MOTOR_1_STEP_PIN,
                 PLUNGER_MOTOR_2_STEP_PIN,
                 -1,
                 -1,
                 PLUNGER_PLUNGE_TIMEOUT,
                 PLUNGER_HOME_TIMEOUT,
                 this, (AddonFnPtr)&App::plungerCB)),
#endif
#ifdef HAS_OP_MODE_SWITCH
             opModeSwitch(new OperationModeSwitch(OP_MODE_1_PIN)),
#endif
             shredStateLast(0),
             shredCancelState(0)
{
}
#ifdef HAS_STATES
String App::state()
{
    const int capacity = JSON_OBJECT_SIZE(6);
    StaticJsonDocument<capacity> doc;
    doc["0"] = id;
    doc["1"] = _state;
    doc["2"] = shredState;
    doc["3"] = overloaded;
    doc["5"] = opModeSwitch->value();
    return doc.as<String>();
}
#endif

short App::getAppState(short val)
{
    return _state;
}
void (*resetFunction)(void) = 0; // Self reset (to be used with watchdog)

short App::setAppState(short newState)
{
    switch (newState)
    {
    case App::APP_STATE::RESET:
    {

        _state = App::APP_STATE::STANDBY;
        shredStateLast = 0;
        shredCancelState = 0;
        shredState = DONE;
        plunger->reset();
        vfd->stop();
        resetFunction();
        return;
    }
    case App::APP_STATE::STANDBY:
    {
        switch (_state)
        {
        case App::APP_STATE::SHREDDING:
        {
            shredCancelState = App::SHRED_STATE::INIT;
            shredState = App::SHRED_STATE::CANCELLING;
            plunger->reset();
            break;
        }
        }
    }
    }
    _state = newState;
}

void printMem()
{
    Serial.print("mem: ");
    Serial.print(freeMemory());
    Serial.println('--');
}
short App::setup()
{
    Serial.begin(DEBUG_BAUD_RATE);

    Serial.println("Booting Firmware ...................... ");
#ifdef PRINT_VERSION
    Serial.print(FIRMWARE_VERSION);
    Serial.print(" | VERSION: ");
    Serial.print(VERSION);
    Serial.print("\n SUPPORT :");
    Serial.print(SUPPORT);
    Serial.print("\n | BUILD: ");
    Serial.print(BUILD);
    Serial.print("\n | FIRMWARE SOURCE: ");
    Serial.print(FW_SRC);
    Serial.print("\n CID:");
    Serial.print(CID);
    Serial.println(" - \n");
#endif
    addons.setStorage(addonsArray);
    setup_addons();

#ifdef MEARSURE_PERFORMANCE
    printPerfTS = 0;
    addonLoopTime = 0;
    bridgeLoopTime = 0;
#endif
    debugTS = 0;
    loopTS = 0;
    shredState = 0;
    overloaded = 0;
    _state = 0;
    jamCounter = 0;
    bootTime = millis();
    shredButton.setup();
    lastOpMode = opModeSwitch->loop();
    statusLightOk.on();
    statusLightError.on();

    /*
    timer.every(5000, [](App *app) -> void {
        printMem();
    },this);
    */
}

void App::_loop_motor_manual()
{

    uchar sw = this->dirSwitch->loop();

    if (sw == 0)
    {
        this->vfd->stop();
        this->shredState = INIT;
        this->_state = RESET;
        this->statusLightError.status_blink(false);
        return;
    }

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

        statusLightError.status_blink(true);
        if (this->mLoad->jammed())
        {
            if (!this->isAutoReversing())
            {
                this->setShredState(JAMMED);
                this->loop_auto_reverse();
            }
            return E_OK;
        }
        else
        {
            this->vfd->fwd(true);
        }
    }
    else if (sw == 1)
    {
        this->vfd->rev(true);
        this->statusLightError.status_blink(true);
    }
}

short App::loop()
{
    loop_addons();
    timer.tick();
    now = millis();
    statusLightError.loop();
    statusLightOk.loop();
    opModeSwitch->loop();

    //Serial.println(analogRead(CONTROLLINO_A2));

    if (now - bootTime < BOOT_DELAY)
    {
        return;
    }
#ifdef HAS_OP_MODE_SWITCH
    short op = opModeSwitch->value();

    if (lastOpMode != op)
    {
        if (lastOpMode == OP_NORMAL)
        {
            plunger->stop();
            plunger->reset();
            _state = App::APP_STATE::RESET;
            shredStateLast = 0;
            shredCancelState = 0;
            shredState = 0;
            vfd->stop();
            plunger->home();
        }
        else if (lastOpMode == OP_MANUAL)
        {
            plunger->stop();
            plunger->reset();
            vfd->stop();
            _state = App::APP_STATE::RESET;
            shredStateLast = 0;
            shredCancelState = 0;
            shredState = 0;
        }

        lastOpMode = op;
        return;
    }

    switch (op)
    {
    case OP_NORMAL:
    {
        loopShred();
        debug();
        break;
    }

    case OP_MANUAL:
    {
        _loop_motor_manual();
        plunger->stop();
        plunger->reset();
        debug();
        break;
    }
    }
#endif
}