#include "Plunger.h"
#include "config.h"
#ifdef HAS_STATES
#include <ArduinoJson.h>
#endif

// #define HAS_PLUNGER_DEBUG

#ifdef HAS_PLUNGER_DEBUG
#define PLUNGER_DEBUG(A) Serial.println(A);
#else
#define PLUNGER_DEBUG(A)
#endif

#define PLUNGER_NOTIFY(STATE)                  \
    if (owner && onChange)                     \
    {                                          \
        short ret = (owner->*onChange)(STATE); \
        return ret;                            \
    }

#define PLUNGER_ALARM_ABORT                               \
    if (motorAlarmPin && analogRead(motorAlarmPin) > 700) \
    {                                                     \
        PLUNGER_NOTIFY(ERROR);                            \
        PLUNGER_DEBUG("SERVO ALARM")             \
        return ERROR;                                     \
    }

#ifdef HAS_STATES
String Plunger::state()
{
    const int capacity = JSON_OBJECT_SIZE(3);
    StaticJsonDocument<capacity> doc;
    doc[0] = id;
    doc[1] = _state;
    doc[2] = pFlags;

#ifdef HAS_PLUNGER_DEBUG
    Serial.println("state : ");
    Serial.println(_state);
    Serial.print("pflags low : ");
    Serial.println(pFlags);
    Serial.print("limit high : ");
    Serial.println(u1.value);

    Serial.print("moving: ");
    Serial.println(TEST(pFlags, MOVING));
    Serial.print("retracting: ");
    Serial.println(TEST(pFlags, RETRACTING));
    Serial.print("freeing: ");
    Serial.println(TEST(pFlags, FREEING));
    Serial.print("done: ");
    Serial.println(TEST(pFlags, DONE));
    Serial.print("retracted: ");
    Serial.println(TEST(pFlags, RETRACTED));
    Serial.print("retracting on : ");
    Serial.println(retracting);
#endif

    /*
    Serial.println("pflags");
    Serial.print("moving: ");
    Serial.println(TEST(pFlags, MOVING));
    Serial.print("retracting: ");
    Serial.println(TEST(pFlags, RETRACTING));
    Serial.print("freeing: ");
    Serial.println(TEST(pFlags, FREEING));
    Serial.print("done: ");
    Serial.println(TEST(pFlags, DONE));
    Serial.print("retracted: ");
    Serial.println(TEST(pFlags, RETRACTED));
    Serial.print("retracting on : ");
    Serial.println(retracting);*/

    return doc.as<String>();
}
#endif

bool Plunger::pause()
{
    SBI(pFlags, PAUSED);
    digitalWrite(PLUNGER_MOTOR_1_STEP_PIN, LOW);
}
bool Plunger::resume()
{
    PLUNGER_ALARM_ABORT;
    CBI(pFlags, PAUSED);
    if (_state = PLUNGING && !u1.value && !l1.value)
    {
        move(0);
        plungeStartTS = now;
    }
    if (_state = HOMING && !u1.value && !l1.value)
    {
        homeStartTS = now;
        move(1);
    }
}
short Plunger::move(short dir)
{
    // PLUNGER_ALARM_ABORT;
    digitalWrite(PLUNGER_MOTOR_1_DIR_PIN, dir == 0 ? HIGH : LOW);
    digitalWrite(PLUNGER_MOTOR_2_STEP_PIN, LOW);
    digitalWrite(PLUNGER_MOTOR_1_STEP_PIN, HIGH);
    SBI(pFlags, MOVING);
}
short Plunger::moveFast(short dir)
{

    PLUNGER_ALARM_ABORT;
    digitalWrite(PLUNGER_MOTOR_1_DIR_PIN, !dir);
    digitalWrite(PLUNGER_MOTOR_1_STEP_PIN, LOW);
    digitalWrite(PLUNGER_MOTOR_2_STEP_PIN, HIGH);
    SBI(pFlags, MOVING);
}
bool Plunger::change(short newState)
{
    if (newState == _state)
    {
        return false;
    }
    _state = newState;
    return true;
}
short Plunger::setup()
{
    u1.setup();
    u1.loop();
    l1.setup();
    l1.loop();
    pFlags = 0;
    digitalWrite(PLUNGER_MOTOR_1_STEP_PIN, LOW);
    digitalWrite(PLUNGER_MOTOR_2_STEP_PIN, LOW);
    retracting = false;
}
void Plunger::debug(Stream *stream)
{
    *stream << name << " : " << u1.value << " : " << l1.value << " : " << pFlags;
}
short Plunger::plunge(short force)
{
    if (!change(PLUNGING))
    {
        if (!TEST(pFlags, DONE))
        {
            return TEST(pFlags, DONE);
        }
        else
        {
            if (force)
            {
                reset();
                _state = PLUNGING;
            }
            else
            {
                return TEST(pFlags, DONE);
            }
        }
    }
    else
    {
        reset();
        _state = PLUNGING;
    }
    if (u1.value)
    {
        SBI(pFlags, FREEING);
        move(0);
        return TEST(pFlags, DONE);
    }
    else
    {
        SBI(pFlags, MOVING);
        move(0);
    }
    return TEST(pFlags, DONE);
}
short Plunger::retract()
{
    u1.loop();
    l1.loop();
    if (u1.value)
    {
        if (!retracting)
        {
            retracting = true;
        }
        SBI(pFlags, RETRACTING);
        return 1;
    }
    else
    {
        retracting = false;
        return 0;
    }
}
short Plunger::home(short force = false)
{

    if (!change(HOMING))
    {
        
        if (!TEST(pFlags, DONE))
        {
            return TEST(pFlags, DONE);
        }
        else
        {
            if (force == false)
            {
                return TEST(pFlags, DONE);
            }
            else
            {
                reset();
                _state = HOMING;
            }
        }
    }
    else
    {
        reset();
        _state = HOMING;
        homeStartTS = now;
    }

    if (u1.value)
    {
        SBI(pFlags, FREEING);
        return TEST(pFlags, DONE);
    }
    else
    {
        moveFast(1);
    }
    return TEST(pFlags, DONE);
}
short Plunger::stop(short val)
{
    digitalWrite(PLUNGER_MOTOR_1_STEP_PIN, LOW);
    digitalWrite(PLUNGER_MOTOR_2_STEP_PIN, LOW);
    CBI(pFlags, MOVING);
    CBI(pFlags, FREEING);
}
short Plunger::reset(short val)
{
    pFlags = 0;
    _state = NONE;
    retracting = false;
}
short Plunger::loop()
{
    l1.loop();
    u1.loop();

    // retract
    if (TEST(pFlags, MOVING))
    {
        if (u1.value || l1.value)
        {
            if (u1.value)
            {
                SBI(pFlags, RETRACTING);
                move(0);
                return;
            }

            if (l1.value && !TEST(pFlags, RETRACTING))
            {
                SBI(pFlags, RETRACTING);
                move(1);
                return;
            }
        }
        else
        {
            if (TEST(pFlags, RETRACTING))
            {
                stop();
                SBI(pFlags, RETRACTED);
                CBI(pFlags, RETRACTING);
            }
        }
    }

    switch (_state)
    {
    case ERROR:
    case MANUAL:
    {
        break;
    }
    case STOPPED:
    {
        break;
    }
    case HOMING:
    {
        if (TEST(pFlags, DONE))
        {
            return;
        }
        if (TEST(pFlags, RETRACTED))
        {
            stop();
            SBI(pFlags, DONE);
            CBI(pFlags, RETRACTED);
            PLUNGER_DEBUG("homed");
        }
        else if (!TEST(pFlags, RETRACTING) && !TEST(pFlags, MOVING))
        {
            PLUNGER_DEBUG("h : move on");
            move(1);
        }

        /*
        if (homeTimeout && now - homeStartTS > homeTimeout)
        {
            stop();
            _state = ERROR;
            PLUNGER_NOTIFY(ERROR_FATAL);
            return;
        }
        */

        break;
    }
    case PLUNGING:
    {

        if (TEST(pFlags, DONE))
        {
            return;
        }
        if (TEST(pFlags, RETRACTED))
        {
            stop();
            SBI(pFlags, DONE);
            CBI(pFlags, RETRACTED);
            PLUNGER_DEBUG("plunged");
        }
        else if (!TEST(pFlags, RETRACTING) && !TEST(pFlags, MOVING))
        {
            PLUNGER_DEBUG("plunging : move on");
            move(0);
        }

        /*
        if (plungeTimeout && now - plungeStartTS > plungeTimeout)
        {
            stop();
            _state = ERROR;
            PLUNGER_NOTIFY(ERROR_FATAL);
            return;
        }*/

        break;
    }
    }
}