#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;
}

short App::getLastError(short val = 0)
{
    return _error;
}

App::App() : Addon("APP", APP, 1 << STATE),
             statusLightOk(StatusLight(STATUS_OK_PIN)),
             statusLightError(StatusLight(STATUS_ERROR_PIN)),
#ifdef HAS_DIRECTION_SWITCH
             dirSwitch(new DirectionSwitch()),
#endif
#ifdef HAS_VFD
            vfd(new VFD(VFD_SPEED_INPUT, -1)),
#endif
#ifdef MOTOR_LOAD_PIN
             mLoad(new MotorLoad(MOTOR_LOAD_PIN)),
#endif
#ifdef HAS_OP_MODE_SWITCH
             opModeSwitch(new OperationModeSwitch(OP_MODE_1_PIN)),
#endif
             plungeStateLast(0),
             plungeState(PLUNGE_STATE::WAITING)
{
}

void (*resetFunction)(void) = 0; // Self reset (to be used with watchdog)

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

#ifdef PRINT_VERSION
    Serial.print(FIRMWARE_VERSION);
    Serial.print(" | VERSION: ");
    Serial.print(VERSION);
    Serial.print("\n SUPPORT :");    
#endif
    addons.setStorage(addonsArray);
    setup_addons();

#ifdef MEARSURE_PERFORMANCE
    printPerfTS = 0;
    addonLoopTime = 0;
    bridgeLoopTime = 0;
#endif

    debugTS = 0;
    loopTS = 0;
    plungeState = PLUNGE_STATE::WAITING;
    overloaded = 0;
    _state = 0;
    bootTime = millis();
    //  lastOpMode = opModeSwitch->loop();
    //  statusLightOk.on();
    //  statusLightError.on();
    lastDirection = VFD::DIRECTION::STOP;
    return;
    /*
    timer.every(5000, [](App *app) -> void {
        printMem();
    },this);
    */
}

ushort App::loopManual()
{
    uchar sw = this->dirSwitch->loop();

    if (sw != lastDirection)
    {
        lastDirection = sw;
    }

    // we are in homing mode, hand over control to loopHome
    if (plungeState == PLUNGE_STATE::HOMING)
    {
        PLUNGE_DEBUG("Goto loop homing");
        return loopHome();
    }

    if (mLoad->jammed())
    {
        this->vfd->stop();
        setPlungeState(PLUNGE_STATE::JAMMED);
        PLUNGE_DEBUG("jammed");
        return E_MOTOR_DT_OVERLOAD;
    }

    // direction switch in same position and still jammed, abort
    if (plungeState == PLUNGE_STATE::JAMMED &&
        sw == lastDirection &&
        sw != VFD::DIRECTION::STOP)
    {
        PLUNGE_DEBUG("still jammed");
        return E_MOTOR_DT_OVERLOAD;
    }

    // clear state on stop
    if (mLoad->idle() &&
        plungeState == PLUNGE_STATE::JAMMED &&
        sw == VFD::DIRECTION::STOP)
    {
        PLUNGE_DEBUG("clear");
        setPlungeState(PLUNGE_STATE::WAITING);
        return E_OK;
    }

    
    // if idle, and direction switch kept up, set into 'homing' state
    if (plungeState == PLUNGE_STATE::WAITING &&
        sw == VFD::DIRECTION::REVERSE &&
        now- dirSwitch->lastChanged() > HOMING_WAIT_THRESHOLD)
    {
        PLUNGE_DEBUG("start homing");
        setPlungeState(PLUNGE_STATE::HOMING);   
        this->vfd->rev(true);
        return E_OK;
    }

    
    // if idle and all ok and start button on, set into plunge !
    if (analogRead(PLUNGE_START_PIN) > PLUNGE_START_LEVEL &&
        sw == VFD::DIRECTION::STOP &&
        plungeState == PLUNGE_STATE::WAITING)
    {
        PLUNGE_DEBUG("start plunging");
        setPlungeState(PLUNGE_STATE::PLUNGING);
        this->vfd->fwd(true);
        return E_OK;
    }

    // we are in plunge mode, hand over control to loopPlunge
    if (plungeState == PLUNGE_STATE::PLUNGING)
    {
        PLUNGE_DEBUG("Goto loop plunge");
        return loopPlunge();
    }

    // manual control cases
    if (sw == VFD::DIRECTION::STOP)
    {
        this->vfd->stop();
        setPlungeState(PLUNGE_STATE::WAITING);
        return E_OK;
    }

    if (sw == VFD::DIRECTION::FORWARD)
    {
        this->vfd->fwd(true);
        return E_OK;
    }
    else if (sw == VFD::DIRECTION::REVERSE)
    {
        if (mLoad->highLoad())
        {
            this->vfd->stop();
            setPlungeState(PLUNGE_STATE::JAMMED);
            return E_OK;
        }
        this->vfd->rev(true);
        return E_OK;
    }

    return E_OK;
}

short App::loop()
{

    loop_addons();
    timer.tick();
    now = millis();

    // statusLightError.loop();
    // statusLightOk.loop();
    //  opModeSwitch->loop();

    loopManual();
    delay(LOOP_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:
    {
        loopPlunge();
        debug();
        break;
    }

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