#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

#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()
{
    CBI(pFlags, PAUSED);
    if (_state = PLUNGING && !u1.value && !l1.value)
    {
        move(0);
    }
    if (_state = HOMING && !u1.value && !l1.value)
    {
        move(1);
    }
}
short Plunger::move(short dir)
{

    digitalWrite(PLUNGER_MOTOR_1_DIR_PIN, !dir);
    digitalWrite(PLUNGER_MOTOR_2_STEP_PIN, LOW);
    digitalWrite(PLUNGER_MOTOR_1_STEP_PIN, HIGH);
    SBI(pFlags, MOVING);
}

short Plunger::moveFast(short dir)
{

    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::setSpeed(short val = 0)
{
    speed = 1000 * val;
}
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;
}
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;
            PLUNGER_DEBUG("PLUNGER : collision!");
        }
        SBI(pFlags, RETRACTING);
        return 1;
    }
    else
    {

        retracting = false;
        return 0;
    }
}
short Plunger::home(short force = false)
{
    if (!change(HOMING))
    {
        PLUNGER_DEBUG("PLUNGER already homing");
        if (!TEST(pFlags, DONE))
        {
            PLUNGER_DEBUG("PLUNGER already homing : not done yet");
            return TEST(pFlags, DONE);
        }
        else
        {
            if (force == false)
            {
                return TEST(pFlags, DONE);
            }
            else
            {
                PLUNGER_DEBUG("PLUNGER already homing : doing it again");
                reset();
                _state = HOMING;
            }
        }
    }
    else
    {
        reset();
        _state = HOMING;
    }

    if (u1.value)
    {
        PLUNGER_DEBUG("PLUNGER : frooze up");
        SBI(pFlags, FREEING);
        return TEST(pFlags, DONE);
    }
    else
    {
        PLUNGER_DEBUG("PLUNGER : move");
        moveFast(1);
    }
    return TEST(pFlags, DONE);
}

short Plunger::stop(short val)
{
    // _state = STOPPED;
    digitalWrite(PLUNGER_MOTOR_1_STEP_PIN, LOW);
    digitalWrite(PLUNGER_MOTOR_2_STEP_PIN, LOW);
    CBI(pFlags, MOVING);
    CBI(pFlags, FREEING);
}

void Plunger::zero()
{
}
short Plunger::reset(short val)
{
    pFlags = 0;
    _state = NONE;
    retracting = false;
}
short Plunger::loop()
{
    AddonFnPtr ptr = this->check;
    l1.loop();
    u1.loop();

    // retract
    if (TEST(pFlags, MOVING))
    {
        if (u1.value || l1.value)
        {
            if (u1.value && !TEST(pFlags, RETRACTING))
            {
                PLUNGER_DEBUG("retract down");
                SBI(pFlags, RETRACTING);
                move(0);
            }

            if (l1.value && !TEST(pFlags, RETRACTING))
            {
                PLUNGER_DEBUG("retract up");
                SBI(pFlags, RETRACTING);
                move(1);
            }
        }
        else
        {
            if (TEST(pFlags, RETRACTING))
            {
                PLUNGER_DEBUG("retracting done");
                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("homing : move on");
            move(1);
        }
        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);
        }
        break;
    }
    }
}