#include "./config.h"
#include "./enums.h"
#include <Bridge.h>

#ifdef MB_MONITORING_STATUS_VFD_MAX_LOAD
#include <xstatistics.h>
#endif

#ifdef MOTOR_LOAD_PIN
#include "./components/MotorLoad.h"
#endif

#if (defined(MB_ANALOG_3POS_SWITCH_0) && defined(MB_ANALOG_3POS_SWITCH_1) || defined(MB_ANALOG_3POS_SWITCH_3))
#include "./components/3PosAnalog.h"
#endif

#if defined(MB_ANALOG_0) || defined(MB_ANALOG_1) || defined(MB_ANALOG_2)
#include "./components/POT.h"
#endif

#include "OmronVFD.h"
#include "ModbusBridge.h"
#include "./OmronMX2.h"
#include "PHApp.h"

#define valA001 3 // A001 Frequency reference source = 03 (no need to change)
#define valA002 3 // A002 Source of the “Move” command = 03 (no need to change)
#define valC026 5 // C026 Relay output function 5 (AL: error signal) = 05

short OmronVFD::onStart()
{

#ifdef COOLING_RELAY
    digitalWrite(COOLING_RELAY, HIGH);
#endif
#ifdef COOLING_RELAY2
    digitalWrite(COOLING_RELAY2, HIGH);
#endif
#ifdef FEEDSCREW_RELAY
    digitalWrite(FEEDSCREW_RELAY, HIGH);
#endif

    //@todo : handle multi errors
    owner->clearError();
    owner->onRun();
    runTS = millis();
    return E_OK;
}
short OmronVFD::onStop()
{

#ifdef COOLING_RELAY
    digitalWrite(COOLING_RELAY, 0);
#endif
#ifdef COOLING_RELAY2
    digitalWrite(COOLING_RELAY2, 0);
#endif
#ifdef FEEDSCREW_RELAY
    digitalWrite(FEEDSCREW_RELAY, 0);
#endif
    // owner->clearError();
    stopTS = millis();
    return E_OK;
}
uint16_t OmronVFD::configure()
{
    // A001 Frequency reference source = 03 = Modbus
    write_Single(MX2_A001, valA001);
    // A002 Source of the RUN command = 03 = Modbus
    write_Single(MX2_A002, valA002);
    // C026 Relay output function 5 (AL: error signal) = 05
    write_Single(MX2_C026, valC026);
    // A004 setting the maximum frequency
    write_Single(MX2_A004, OMRON_VFD_MAX_FREQ / 10);
}
uint16_t OmronVFD::stop()
{
    onStop();
    direction = E_VFD_DIR::E_VFD_DIR_NONE;
    return write_Bit(MX2_START, 0);
}
uint16_t OmronVFD::run()
{
    onStart();
    return write_Bit(MX2_START, 1);
}
uint16_t OmronVFD::reverse()
{
    direction = E_VFD_DIR::E_VFD_DIR_REVERSE;
    return write_Bit(MX2_SET_DIR, 1);
}
uint16_t OmronVFD::forward()
{
    direction = E_VFD_DIR::E_VFD_DIR_FORWARD;
    return write_Bit(MX2_SET_DIR, 0);
}
uint16_t OmronVFD::setTargetFreq(uint16_t freq)
{
    return write_Single(MX2_TARGET_FR, clamp<int>(freq * 100, 0, OMRON_VFD_MAX_FREQ * 100));
}
short OmronVFD::setup()
{
    onStop();
    bootTS = millis();
    return E_OK;
}
short OmronVFD::directDirection()
{
    if (mode && mode->value == Pos3Analog::POS3_DIRECTION::UP)
    {
        if (dir && dir->value)
        {
            return dir->value;
        }
    }
    return 0;
}

bool ready = false;
short OmronVFD::loop()
{

    if (millis() - debugTS > OMRON_MX2_DEBUG_INTERVAL)
    {
        debugTS = now;
    }

#ifdef MOTOR_LOAD_PIN
    if (motorLoad)
    {
        currentStats.add(abs(motorLoad->value));
        states[0].max_current = currentStats.maximum();
        if (motorLoad->value > MOTOR_OVERLOAD_RANGE_MIN)
        {
            stop();
            return onError(E_VFD_OVERLOAD);
        }
    }
#endif
    bool hasDirectMode = true;
    if (hasDirectMode && runMode == E_VFD_RUN_MODE::E_VFD_RUN_MODE_STOP_RETRACT)
    {
        short state = states[0].state;
        switch (retractState)
        {

        case E_VFD_RETRACT_STATE::E_VFD_RETRACT_STATE_BRAKING:
        {
            if (state == OMRON_STATE_DECELERATING)
            {
            }
            if (state == OMRON_STATE_STOPPED)
            {
                retractState = E_VFD_RETRACT_STATE::E_VFD_RETRACT_STATE_STOPPED;
            }
            break;
        }
        case E_VFD_RETRACT_STATE::E_VFD_RETRACT_STATE_STOPPED:
        {
            reverse();
            run();
            retractState = E_VFD_RETRACT_STATE::E_VFD_RETRACT_STATE_REVERSING;
            reverseStartTS = millis();
            break;
        }
        case E_VFD_RETRACT_STATE::E_VFD_RETRACT_STATE_REVERSING:
        {
            if (now - reverseStartTS > VFD_RETRACT_REVERSE_DURATION)
            {
                stop();
                retractState = E_VFD_RETRACT_STATE::E_VFD_RETRACT_STATE_BRAKE_REVERSING;
            }
            break;
        }
        case E_VFD_RETRACT_STATE::E_VFD_RETRACT_STATE_BRAKE_REVERSING:
        {
            if (state == OMRON_STATE_DECELERATING || OMRON_STATE_STOPPED || OMRON_STATE_RUNNING)
            {
                forward();
                retractState = E_VFD_RETRACT_STATE::E_VFD_RETRACT_STATE_RETRACTED;
                runMode == E_VFD_RUN_MODE::E_VFD_RUN_MODE_NONE;
            }
            break;
        }
        }
    }

    /*
    // left direct mode
    if (mode && mode->last_switch && !mode->value)
    {
        mode->last_switch = mode->value;
        if (lastError == E_VFD_OVERLOAD)
        {
            owner->clearError();
        }
        stop();
    }*/
    /*
        // left direct mode
        if (mode && mode->last_switch && !mode->value && dir && !dir->value && dir->last_switch)
        {
            mode->last_switch = mode->value;
            dir->last_switch = dir->value;
            if (lastError == E_VFD_OVERLOAD)
            {
                owner->clearError();
            }
            stop();
        }*/

    // cancel retract
    if (hasDirectMode && mode && mode->value && dir && dir->value)
    {
        runMode = E_VFD_RUN_MODE::E_VFD_RUN_MODE_NONE;
        retractState = E_VFD_RETRACT_STATE::E_VFD_RETRACT_STATE_NONE;
    }

    // direct mode : control panel
    if (mode && mode->value == Pos3Analog::POS3_DIRECTION::UP)
    {
        // speed
        if (speed && abs(speed->last_value - speed->value) > ANALOG_INPUT_MIN_DT_0)
        {
            speed->last_value = speed->value;
            int dstSpeed = (speed->value * 100 / (ANALOG_INPUT_MAX_LEVEL_0 / 100)) / (OMRON_VFD_MAX_FREQ);
            setTargetFreq(abs(dstSpeed));
        }

        // direction
        if (dir && dir->last_switch != dir->value)
        {

            dir->last_switch = dir->value;
            switch (dir->value)
            {
            case Pos3Analog::POS3_DIRECTION::UP:
                // Log.notice("VFD: direct %s\n", "forward");
                forward();
                run();
                break;
            case Pos3Analog::POS3_DIRECTION::DOWN:
                //  Log.notice("VFD: direct %s\n", "reverse");
                reverse();
                run();
                break;
            case Pos3Analog::POS3_DIRECTION::MIDDLE:
                //  Log.notice("VFD: direct %s\n", "stop");
                stop();
                break;
            }
        }
    }

    modbusLoop();
    return E_OK;
}

short OmronVFD::debug()
{
    //*stream << this->name << ":" << this->ok();
    modbus->print();
    return false;
}
short OmronVFD::info()
{
    //*stream << this->name << "\n\t : " SPACE("Pin:" << MOTOR_IDLE_PIN);
    return false;
}
short OmronVFD::onRegisterMethods(Bridge *bridge)
{
    // bridge->registerMemberFunction(id, this, C_STR("debug"), (ComponentFnPtr)&OmronVFD::debug);
    return E_OK;
}