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

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

#include <MemoryFree.h>
//#ifdef MODBUS_BRIDGE
#include "ModbusBridge.h"

//#endif

static Addon *addonsArray[10];

#ifdef HAS_DIP
int dipSwitchPins[] = {48, 49};
#endif

short App::ok()
{
    if (now - dt > 100)
    {
        // motor should spin
        /*
        if (vfd->direction &&
            mLoad->lastIdle > MOTOR_MIN_DT &&
            mLoad->dt > MOTOR_MIN_DT  &&
            mLoad->currentState == MotorLoad::MSTATE::NONE)
        {
            return E_VFD_RUN;
        }

        // motor should not spin
        if (!vfd->direction &&
            vfd->dt > MOTOR_MIN_DT &&
            mLoad->lastLoad > MOTOR_MIN_DT * 4  &&
            vfd->direction == VFD::DIRECTION::STOP &&
            mLoad->currentState != MotorLoad::MSTATE::NONE)
        {
            return E_VFD_LOSS;
        }
        */
        dt = now;
    }
    return E_OK;
}

App::App() : Addon("APP", APP, 1 << STATE),
             shredButton(ProximitySensor(CONTROLLINO_A15)),
#ifdef HAS_DIRECTION_SWITCH
             dirSwitch(new DirectionSwitch()),
#endif
#ifdef ENCLOSURE_SENSOR
             enclosureSensor(new EnclosureSensor()),
#endif
#ifdef HAS_VFD
             vfd(new VFD()),
#endif
#ifdef MOTOR_HAS_TEMPERTURE
             mHeat(new MotorTemperature()),
#endif
#ifdef HAS_MOTOR_IR_SENSOR
             mSpeed(new MotorSpeed()),
#endif
#ifdef HAS_DIP
             _dipSwitch(new _DipSwitch(2, dipSwitchPins)),
#endif
#ifdef CARTRIDGE_FULL_1
             cartridgeFull(new CartridgeFull()),
#endif
#ifdef HAS_HOPPER_LOADED
#ifdef HOPPER_LOADED_1
             hopperLoaded(new HopperLoaded(HOPPER_LOADED_0, HOPPER_LOADED_1)),
#else
             hopperLoaded(new HopperLoaded(HOPPER_LOADED_0)),
#endif
#endif
#ifdef MOTOR_LOAD_PIN
             mLoad(new MotorLoad(MOTOR_LOAD_PIN)),
#endif
#ifdef HAS_AUTOREVERSE
             autoReverse(new AutoReverse(this)),
#endif
#ifdef HAS_PLUNGER
             plunger(new Plunger(this, (AddonFnPtr)&App::plungerCB)),
#endif
#ifdef HAS_SERIAL
             serialBridge(new PPSerial(Serial)),
#endif
#ifdef HAS_POWER
#if defined(POWER_0) && defined(POWER_1)
             powerSwitch(new Power(POWER_0, POWER_1)),
#else
             powerSwitch(new Power(POWER_0, POWER_0)),
#endif
#endif
#ifdef HAS_OP_MODE_SWITCH
#ifdef OP_MODE_ANALOG
             opModeSwitch(new OperationModeSwitch(OP_MODE_1_PIN, 120, 60, 30)),
#else
             opModeSwitch(new OperationModeSwitch(OP_MODE_1_PIN, OP_MODE_2_PIN, OP_MODE_3_PIN)),
#endif
#endif
#ifdef HAS_MODBUS_BRIDGE
             modbusBridge(new ModbusBridge()),
#endif
             shredStateLast(0),
             shredCancelState(0)
{
    //#if defined(MODBUS_BRIDGE) && defined(HAS_VFD)
    //    vfd->modbus = modbusBridge;
    //#endif
    /// modbusBridge->debug(&Serial);
}
#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["4"] = hopperLoaded->ok();
    doc["5"] = opModeSwitch->value();
    Serial.println(analogRead(CONTROLLINO_A15));
    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 ...................... ");

    addons.setStorage(addonsArray);
    setup_addons();

#ifdef MEARSURE_PERFORMANCE
    printPerfTS = 0;
    addonLoopTime = 0;
    bridgeLoopTime = 0;
#endif
    debugTS = 0;
    comTS = 0;
    loopTS = 0;
    shredState = 0;
    overloaded = 0;
    _state = 0;
    jamCounter = 0;
    bootTime = millis();
    shredButton.setup();

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

void App::loop_service()
{
#ifdef HAS_POWER
    powerSwitch->on(POWER_PRIMARY);
#endif
    // _loop_motor_manual();
}
void App::_loop_motor_manual()
{
    loop_auto_reverse();
#if defined(HAS_DIRECTION_SWITCH) && defined(HAS_VFD)
    uchar sw = this->dirSwitch->loop();
    if (sw == 2)
    {
        this->vfd->fwd(true);
    }
    else if (sw == 1)
    {
        this->vfd->rev(true);
    }
    else
    {
        this->vfd->stop();
    }
#endif
}

void App::loop_normal()
{
#ifdef HAS_HOPPER_LOADED
    loop_auto_reverse();
    if (hopperLoaded->ok())
    {
        powerSwitch->on(POWER_PRIMARY);
        vfd->fwd(true);
        plunger->plunge();
    }
    else
    {
        this->vfd->stop();
        //  plunger->home();
        powerSwitch->off(POWER_PRIMARY);
    }
#endif
}
void App::debug_mode_loop()
{
    uchar s = addons.size();
    for (uchar i = 0; i < s; i++)
    {
        Addon *addon = addons[i];
        if (addon->hasFlag(LOOP))
        {
            addon->loop();
        }
    }

#if defined(HAS_POWER) && defined(HAS_HOPPER_LOADED)
    uchar sw = this->dirSwitch->loop();
    if (sw)
    {
        powerSwitch->on(POWER_PRIMARY);
    }
    if (sw == 2)
    {
        this->vfd->fwd(true);
    }
    else if (sw == 1)
    {
        this->vfd->rev(true);
    }
    else
    {
        this->vfd->stop();
        powerSwitch->off(POWER_PRIMARY);
    }

    debug();
    if (sw)
    {
        delay(LOOP_DELAY);
        return;
    }
    ///////////////////////////////////////////////////
    if (hopperLoaded->ok())
    {
        powerSwitch->on(POWER_PRIMARY);
        this->vfd->fwd(true);
        delay(3000);
    }
    else
    {
        this->vfd->stop();
        powerSwitch->off(POWER_PRIMARY);
    }

    delay(LOOP_DELAY);
#endif
}

short App::loop()
{
    loop_addons();
    loop_com();
    timer.tick();
    now = millis();
    shredButton.loop();

    if (now - bootTime < BOOT_DELAY)
    {
        return;
    }

    if (shredState == STUCK)
    {
        _error = E_STUCK;
        setAppState(App::SHRED_STATE::CANCELLING);
        return;
    }

    short error = ok();
    if (error)
    {
        _error = error;
        return;
    }

#ifdef HAS_OP_MODE_SWITCH
    short op = opModeSwitch->value();
    op = 0;
    switch (op)
    {
        //pos 1
    case OP_DEBUG:
    {
#ifdef HAS_POWER
        // powerSwitch->on(POWER_PRIMARY);
#endif
        break;
    }
    // pos2
    case OP_NORMAL:
    {
#ifdef HAS_POWER
        // powerSwitch->on(POWER_PRIMARY);
        // powerSwitch->on(POWER_SECONDARY);
#endif
        loop_normal();
        debug();
        break;
    }
    //pos 0
    case OP_NONE:
    {
#ifdef HAS_POWER
        // powerSwitch->off(POWER_PRIMARY);
#endif
        // vfd->stop();
        //  plunger->stop();

        if (hopperLoaded->ok() && _state != SHREDDING)
        {
            Serial.println("hopper loaded");
            shred();
        }
        loopShred();
        break;
    }
    case OP_SERVICE:
    {
        // loop_normal();
        powerSwitch->on(POWER_PRIMARY);
        vfd->rev(true);

        break;
    }
    }
#endif
}

void App::loop_com()
{
    if (millis() - comTS > 300)
    {
#if defined(HAS_BRIDGE) && defined(HAS_SERIAL)
        PPSerial::Message *msg = serialBridge->read();
        if (msg)
        {
            switch (msg->verb)
            {

            case Bridge::EC_METHOD:
            {
                char *strings[3];
                char *ptr = NULL;
                byte index = 0;

                ptr = strtok(msg->payload, ":");

                while (ptr != NULL && index < 4)
                {
                    strings[index] = ptr;
                    index++;
                    ptr = strtok(NULL, ":");
                }

                int id = atoi(strings[0]);
                char *_method = strings[1];

                SKeyVal *method = VSL::instance()->hasMethod(id, _method);
                if (method)
                {
                    int arg = atoi(strings[2]);
                    Addon *addon = (Addon *)method->instance;
                    AddonFnPtr ptr = method->mPtr;
                    short ret = (addon->*ptr)(arg);

                    if (TEST(msg->flags, Bridge::STATE))
                    {
#ifdef HAS_STATES
                        this->appState(0);
#endif
                    }
                    else if (TEST(msg->flags, Bridge::RECEIPT))
                    {
#ifdef BRIDGE_HAS_RESPONSE
                        const char *response = Bridge::CreateResponse(msg->id, 0, ret);
                        Serial.write(response);
#endif
                    }
                    if (TEST(msg->flags, Bridge::DEBUG))
                    {
                        Serial.println("Called command");
                    }
                    Serial.println("Called command");
                    Serial.println(addon->name);
                    Serial.println(_method);
                }
                else
                {
                    VSL::instance()->debug();
                    if (TEST(msg->flags, Bridge::DEBUG))
                    {

                        Serial.print("Incoming message, cant find class & method ");
                        /*
                        Serial.print(_class);
                        Serial.print(":");
                        Serial.print(_method);
                        */
                        Serial.print(_method);
                        Serial.print("\n");
                    }
                }
                break;
            }
            }
            msg->payload = NULL;
        }
#endif
        comTS = millis();
    }
}