#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(-1, -1)),
#endif
#ifdef MOTOR_LOAD_PIN
             mLoad(new MotorLoad(MOTOR_LOAD_PIN)),
#endif
#ifdef HAS_PLUNGER_HOME
             plungerHome(new ProximitySensor(PLUNGE_HOME_PIN)),
#endif
#ifdef HAS_OP_MODE_SWITCH
             opModeSwitch(new OperationModeSwitch(OP_MODE_1_PIN)),
#endif
             plungeState(PLUNGE_STATE::IDLE)
{
}

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, loopTS, overloaded, _state, plungeStart, lastChangeTS = 0;
    plungeState = PLUNGE_STATE::IDLE;
    startBtnTS, startBtn2TS, lastChangeTS, bootTime = millis();
    dirSwitch->clear();
    Serial.println("Say hello Elena ZMAX-RC2 :)");
    lastDirection = VFD::DIRECTION::STOP;
    printMem();
    debugIntern(&Serial);

    return E_OK;
}

short App::debugIntern(Stream *stream)
{
    return E_OK;
    *stream << this->name << " : "
            << "State : " << plungeState << SPACE(" Start: ") << analogRead(PLUNGE_START_PIN) << SPACE(" Last: ") << (now - lastChangeTS) << SPACE(" LastTS: ") << (lastChangeTS) << "\n";
    return E_OK;
}

short App::plunge(short value)
{
    setPlungeState(PLUNGING);
    plungeStart = millis();
    this->vfd->fwd(true);
#ifdef HOME_LOW_SPEED_PIN
    analogWrite(HOME_LOW_SPEED_PIN, RELAY_ON);
#endif
#ifdef HOME_MEDIUM_SPEED_PIN
    analogWrite(HOME_MEDIUM_SPEED_PIN, RELAY_ON);
#endif
    return E_OK;
}

short App::stop(short value)
{
    setPlungeState(IDLE);
    this->vfd->stop();
#ifdef HOME_LOW_SPEED_PIN
    analogWrite(HOME_LOW_SPEED_PIN, RELAY_ON);
#endif
#ifdef HOME_MEDIUM_SPEED_PIN
    analogWrite(HOME_MEDIUM_SPEED_PIN, RELAY_ON);
#endif
    return E_OK;
}

short App::home(short value)
{
    setPlungeState(PLUNGE_STATE::HOMING);
    this->vfd->rev(true);
#ifdef HOME_LOW_SPEED_PIN
    analogWrite(HOME_LOW_SPEED_PIN, RELAY_OFF);
#endif
#ifdef HOME_MEDIUM_SPEED_PIN
    analogWrite(HOME_MEDIUM_SPEED_PIN, RELAY_OFF);
#endif
    return E_OK;
}

short App::setPlungeState(short newState)
{
    plungeState = newState;
    if (newState != PLUNGE_STATE::IDLE)
    {
        // Serial.print("Set Plunge State: ");
        // Serial.println(plungeState);
    }
    return newState;
}

short App::reset()
{
#ifdef VFD_FAULT_RESET_PIN
    analogWrite(VFD_FAULT_RESET_PIN, RELAY_OFF);
    delay(100);
    analogWrite(VFD_FAULT_RESET_PIN, RELAY_ON);
#endif
    return E_OK;
}

ushort App::loopManual()
{
    uchar sw = dirSwitch->loop();
    uchar startBtnOn = analogRead(PLUNGE_START_PIN) > ANALOG_READ_THRESHOLD;
    uchar startBtnOn2 = analogRead(PLUNGE_START_MAX_PIN) > ANALOG_READ_THRESHOLD;
    uchar homeEndSwitchOn = plungerHome->read();
    uchar isPlunging = mLoad->plunging();
    uchar isIdle = mLoad->idle();
    uchar directionChanged = sw != lastDirection && sw;
    uchar isJammed = mLoad->jammed();

    if (directionChanged)
    {
        PLUNGE_DEBUG("Direction changed");
        lastDirection = sw;
        if (sw)
        {
            lastChangeTS = millis();
            reset();
        }
    }

    if (!sw)
    {
        lastChangeTS = millis();
    }

    // Handle jamming & end switches
    if (isJammed)
    {
        stop();
        setPlungeState(PLUNGE_STATE::JAMMED);
        PLUNGE_DEBUG("jammed");
        mLoad->debug(&Serial);
        analogWrite(VFD_FAULT_RESET_PIN, RELAY_ON);
        analogWrite(VFD_FAULT_RESET_PIN, RELAY_OFF);
        return E_MOTOR_DT_OVERLOAD;
    }

    // Ensure state handling for jammed condition
    if (plungeState == PLUNGE_STATE::JAMMED &&
        sw == lastDirection &&
        sw != VFD::DIRECTION::STOP)
    {
        PLUNGE_DEBUG("still jammed");
        mLoad->debug(&Serial);
        analogWrite(VFD_FAULT_RESET_PIN, RELAY_ON);
        analogWrite(VFD_FAULT_RESET_PIN, RELAY_OFF);
        return E_MOTOR_DT_OVERLOAD;
    }

    // Handle auto drive (homing/plunging)
    if (plungeState == PLUNGE_STATE::IDLE ||
        plungeState == PLUNGE_STATE::HOMING ||
        plungeState == PLUNGE_STATE::PLUNGING)
    {
        if (sw == VFD::DIRECTION::REVERSE && (now - lastChangeTS > MIN_DT_AUTO))
        {
            debugIntern(&Serial);
            home();
            setPlungeState(PLUNGE_STATE::AUTO);
            lastAutoTS = now;
            PLUNGE_DEBUG("start homing");
            return E_OK;
        }

        if (sw == VFD::DIRECTION::FORWARD && (now - lastChangeTS > MIN_DT_AUTO))
        {
            debugIntern(&Serial);
            plunge();
            setPlungeState(PLUNGE_STATE::AUTO);
            lastAutoTS = now;
            PLUNGE_DEBUG("start plunging");
            return E_OK;
        }
    }

    if (plungeState == PLUNGE_STATE::AUTO)
    {
        if (startBtnOn || startBtnOn2 || directionChanged)
        {
            stop();
            setPlungeState(PLUNGE_STATE::IDLE);
            PLUNGE_DEBUG("abort auto");
            return E_OK;
        }
        return E_OK;
    }

    // Self-initiate Plunging
    if (startBtnOn &&
        now - lastAutoTS > MIN_DT_AUTO &&
        plungeState == PLUNGE_STATE::IDLE && sw == VFD::DIRECTION::STOP)
    {
        PLUNGE_DEBUG("start plunging");
        plunge();
        setPlungeState(PLUNGE_STATE::AUTO);
        lastAutoTS = now;
        return E_OK;
    }
    
    // Manual control cases
    if (sw == VFD::DIRECTION::STOP)
    {
        stop();
        return E_OK;
    }

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

    return E_OK;
}

short App::loop()
{
    loop_addons();
    timer.tick();
    now = millis();
    short result = loopManual();
    delay(LOOP_DELAY);
    return result;
}