#include "Plunger.h"
#include "AccelStepper.h"
#include "MultiStepper.h"

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

AccelStepper stepper1(AccelStepper::DRIVER, PLUNGER_MOTOR_1_DIR_PIN, PLUNGER_MOTOR_1_STEP_PIN);
AccelStepper stepper2(AccelStepper::DRIVER, PLUNGER_MOTOR_2_DIR_PIN, PLUNGER_MOTOR_2_STEP_PIN);

MultiStepper steppers;

// #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::change(short newState)
{
    if (newState == _state)
    {
        return false;
    }
    _state = newState;
    return true;
}
short Plunger::setSpeed(short val = 0)
{
    speed = 1000 * val;
}
short Plunger::setup()
{
    stepper1.setMaxSpeed(PLUNGER_BASE_SPEED);
    stepper1.setSpeed(PLUNGER_BASE_SPEED);
    stepper1.setAcceleration(5000);
    steppers.addStepper(stepper1);
    steppers.moveTo(positions);
    stepper1.setCurrentPosition(positions[0]);
    stepper1.setPinsInverted(false);

    u1.setup();
    u1.loop();
    pFlags = 0;
    retracting = false;
}

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

    SBI(pFlags, INITIATED);
    zero();

    if (u1.value)
    {
        SBI(pFlags, FREEING);
        //goToPos(-PLUNGER_RETRACE_DISTANCE, PLUNGER_RETRACT_SPEED);
        return TEST(pFlags, DONE);
    }
    else
    {
        SBI(pFlags, MOVING);
        //goToPos(PLUNGE_PLUNG_DISTANCE, speed);
    }
    return TEST(pFlags, DONE);
}
short Plunger::retract()
{
    u1.loop();
    if (u1.value)
    {
        if (!retracting)
        {
            retracting = true;
            zero();
            stepper1.setMaxSpeed(PLUNGER_RETRACT_SPEED);
            positions[0] += (u1.value ? PLUNGER_RETRACE_DISTANCE : -PLUNGER_RETRACE_DISTANCE);
            positions[1] += (u1.value ? PLUNGER_RETRACE_DISTANCE : -PLUNGER_RETRACE_DISTANCE);
            steppers.moveTo(positions);
            PLUNGER_DEBUG("PLUNGER : collision!");
        }
        SBI(pFlags, RETRACTING);
        return 1;
    }
    else
    {
        if (!steppers.hasToGo())
        {
            retracting = false;
        }
        return 0;
    }
}
short Plunger::goToPos(long pos, long speed)
{
    zero();
    stepper1.setMaxSpeed(speed);
    stepper1.setSpeed(speed);
    Serial.println(speed);
    positions[0] += pos;
    positions[1] += pos;
    steppers.moveTo(positions);
}
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;
    }

    SBI(pFlags, INITIATED);
    zero();

    if (u1.value)
    {
        PLUNGER_DEBUG("PLUNGER : frooze up");
        SBI(pFlags, RETRACTING);
        retract();
        return TEST(pFlags, DONE);
    }
    else
    {
        PLUNGER_DEBUG("PLUNGER : move");
        SBI(pFlags, MOVING);
        goToPos(PLUNGE_HOME_DISTANCE, PLUNGER_HOMING_SPEED);
    }
    return TEST(pFlags, DONE);
}

short Plunger::moveMotors(short val = 0)
{
    _state = MANUAL;
    goToPos(val, PLUNGER_MOVE_SPEED);
}

short Plunger::moveMotor1(short val = 0)
{
    reset();
    zero();
    stepper1.setMaxSpeed(PLUNGER_MOVE_SPEED);
    stepper1.moveTo(val);
    _state = MANUAL;
}
short Plunger::moveMotor2(short val = 0)
{
    reset();
    zero();
    stepper2.setMaxSpeed(PLUNGER_MOVE_SPEED);
    stepper2.moveTo(val);
    _state = MANUAL;
}

short Plunger::stop(short val)
{
    _state = STOPPED;
    pFlags = 0;
}

void Plunger::zero()
{
    positions[0] = 0;
    positions[1] = 0;
    stepper1.setCurrentPosition(positions[0]);
    stepper2.moveTo(positions);
}
short Plunger::reset(short val)
{
    positions[0] = 0;
    positions[1] = 0;
    pFlags = 0;
    _state = NONE;
    retracting = false;
    stepper1.setCurrentPosition(positions[0]);
}
short Plunger::loop()
{
    AddonFnPtr ptr = this->check;
    short ret = 1; (owner->*ptr)(0);
    // u1.loop();
    if (!ret)
    {
        SBI(pFlags, PAUSED);
        return;
    }
    else
    {
        CBI(pFlags, PAUSED);
    }

    while (steppers.run() && !TEST(pFlags, PAUSED) && !TEST(pFlags, DONE))
    {
        // retract();

        ret = (owner->*ptr)(0);

        if (!ret)
        {
            SBI(pFlags, PAUSED);
            break;
        }

        switch (_state)
        {
        case ERROR:
        case MANUAL:
        {
            break;
        }
        case STOPPED:
        {
            break;
        }
        case HOMING:
        case PLUNGING:
        {
            if (retract())
            {
                Serial.print('shit');
            }
            break;
        }
        }
    }

    ret = (owner->*ptr)(0);
    if (!ret)
    {
        SBI(pFlags, PAUSED);
        return;
    }
    else
    {
        SBI(pFlags, PAUSED);
    }

    if (!steppers.hasToGo())
    {
        if (retracting)
        {
            retracting = false;
        }

        if (TEST(pFlags, RETRACTING))
        {
            SBI(pFlags, RETRACTED);
            CBI(pFlags, RETRACTING);
        }

        switch (_state)
        {
        case ERROR:
        case STOPPED:
        {
            _state = NONE;
            pFlags = 0;
            return;
        }
        break;
        case MANUAL:
        {
            _state = NONE;
            pFlags = 0;
            break;
        }
        case PLUNGING:
        case HOMING:
        {
            if (TEST(pFlags, DONE))
            {
                // PLUNGER_DEBUG("PLUNGER ::: SET DONE");
                break;
            }

            if (TEST(pFlags, FREEING) && TEST(pFlags, RETRACTED))
            {
                PLUNGER_DEBUG("PLUNGER : clear retracted after freeing");
                CBI(pFlags, FREEING);
                CBI(pFlags, RETRACTED);
                CBI(pFlags, RETRACTING);
            }

            if (!TEST(pFlags, MOVING) && !TEST(pFlags, RETRACTED) && !TEST(pFlags, RETRACTING))
            {
                PLUNGER_DEBUG("PLUNGER : goto");
                SBI(pFlags, MOVING);
                goToPos(_state == HOMING ? PLUNGE_HOME_DISTANCE : PLUNGE_PLUNG_DISTANCE, _state == HOMING ? PLUNGER_HOMING_SPEED : PLUNGER_PLUNGE_SPEED);
                break;
            }

            if (!TEST(pFlags, FREEING) && TEST(pFlags, RETRACTED))
            {
                SBI(pFlags, DONE);
                PLUNGER_DEBUG("PLUNGER : retracted, set done");
                break;
            }

            if (TEST(pFlags, MOVING) && TEST(pFlags, RETRACTED))
            {
                PLUNGER_DEBUG("PLUNGER : moved & retracted");
                SBI(pFlags, DONE);
                return;
            }

            if (!TEST(pFlags, MOVING) && TEST(pFlags, RETRACTED))
            {
                PLUNGER_DEBUG("PLUNGER : not moved but retracted");
                SBI(pFlags, DONE);
                retracting = false;
            }
            break;
        }
        }
    }
}