#include "controller.h"
#include "../faults/faults.h"

extern u32 currMillis;

Controller::Controller(const uint8_t pAnalaogPin,
                       const uint8_t oneWirePin,
                       const uint8_t t1Pin,
                       const uint8_t t2Pin,
                       const uint8_t pRisePin,
                       const uint8_t pFallPin)
    : _aPin(pAnalaogPin)
    , _oneWire(oneWirePin)
    , _prPin(pRisePin)
    , _pfPin(pFallPin)
    , _t1Pin(t1Pin)
    , _t2Pin(t2Pin)
    , _dallas(&_oneWire)
{
    pinMode(_prPin, OUTPUT);
    pinMode(_pfPin, OUTPUT);
    pinMode(_t1Pin, OUTPUT);
    pinMode(_t2Pin, OUTPUT);
    digitalWrite(_prPin, 0);
    digitalWrite(_pfPin, 0);
    digitalWrite(_t1Pin, 0);
    digitalWrite(_t2Pin, 0);
    vStates.pInc = false;
    vStates.pDec = false;
    vStates.t1 = false;
}

void Controller::init(bool startup=false)
{
    _dallas.begin();
    _dallas.setWaitForConversion(false);
    const uint8_t maxDevices = 1;
    uint8_t deviceCount = _dallas.getDS18Count();
    if (deviceCount > maxDevices) {
        display.tooMuchOneWireDevicesDetected(deviceCount, maxDevices);
    } else if (!deviceCount) {
        display.noOneWireDevicesDetected();
    } else {
        _initOk = true;
        _dallas.getAddress(_tempAddr1, 0);
        _dallas.setResolution(12);
        if (!startup)
            display.oneWireDevicesDetected(deviceCount);
    }
}

void Controller::process()
{
    if (_initOk && currMillis - _lastConversion > _TEMP_CONVERSION_DELAY) {
        _lastConversion = currMillis;
        _requestConversion = true;
        float temp = _dallas.getTempC(_tempAddr1);
        if (temp == DEVICE_DISCONNECTED_C)
            vals.t1 = _SENSOR_FAULT;
        else
            vals.t1 = temp * 100;
        _printsf("Temp: "); _prints(vals.t1);
        if (vals.t1 != _SENSOR_FAULT && params.tEn && params.cEn) {
            switch (_tState) {
            case stateIdle:
                faults.t1Idle(vals.t1);
                _printslnf(", case stateIdle");
                if (vals.t1 < params.ts1 - params.th1) {
                    _setT1Valve(0, stateRising);
                    _printsf(" -> rising");
                } else if (vals.t1 > params.ts1 + params.th1) {
                    _setT1Valve(1, stateFalling);
                     _printsf(" -> falling");
                }
                _printsln();
                break;
            case stateRising:
                faults.check(t1TooLo);
                _printsf(", case stateRising");
                if (vals.t1 > params.ts1 + params.th1) {
                    _setT1Valve(1, stateIdle);
                     _printsf(" -> idle");
                }
                _printsln();
                break;
            case stateFalling:
                faults.check(t1TooHi);
                _printsf(", case stateFalling");
                if (vals.t1 < params.ts1 - params.th1) {
                    _setT1Valve(0, stateIdle);
                     _printsf(" -> idle");
                } else { // um ev. abschalten (Fehler) zu können
                    _setT1Valve(1, stateFalling);
                }
                _printsln();
                break;
            }
        } else if (vals.t1 == _SENSOR_FAULT && params.tEn && params.cEn) {
            faults.check(t1Invalid);
            _setT1Valve(0, stateIdle);
        } else {
            faults.t1Idle();
            _setT1Valve(0, stateIdle);
            _printsln();
        }
#if _MODBUS == 1
#if _MODBUS_AVERAGE_TEMPERATURES == 1
        _averageTemperature1();
#else
        modbusData[_MODBUS_T1_INDEX] = vals.t1;
#endif
#endif
    } else if (_requestConversion) {
        _requestConversion = false;
        _dallas.requestTemperatures();
    }
    if (currMillis - _lastAnalogRead > _ANALOG_READ_DELAY) {
        _lastAnalogRead = currMillis;
        _rawP = analogRead(_aPin);
        _printsf("_rawP: "); _prints(_rawP);
        switch (pSensor) {
        case SMC_1_5V_0_5BAR:
            float p = (float) _rawP / 2.046f; // 1023 / 5 * 100 (0-5 1/100 bar)
            p = (p - 1) * 1.25f; // 1...5 V Sensor
            _printsf(" (Gems), p: "); _prints(p);
            if (p < -0.5f) // Sensor ist definitiv nicht angeschlossen
                vals.p = _SENSOR_FAULT;
            else if (p < 0) // Bei kleiner Toleranz ein bisschen schwindeln ;-)
                vals.p = 0;
            else
                vals.p = p;
            break;
        case GEMS_0_5V_0_6BAR:
            vals.p = (float) _rawP / 1.705f; // 1023 / 6 * 100 (0-5 1/100 bar)
            _printsf(" (Gems), p: "); _prints(vals.p);
        }
        _averagePressure();
        if (vals.p != _SENSOR_FAULT && params.cEn && (params.pInc || params.pDec)) {
            switch (_pState) {
            case stateIdle:
                faults.pIdle(_buildAveragePForFaultDetection());
                _p_i_fault_start = _p_i_curr;
                _printsf(", case stateIdle");
                if (vals.p < params.ps - params.ph) {
                    if (params.pInc) {
                        _printslnf(" -> stateRising");
                        _setPValves(stateRising);
                    } else {
                        _printsln();
                        _setPValves(stateIdle);
                    }
                } else if (vals.p > params.ps + params.ph) {
                    if (params.pDec) {
                        _printslnf(" -> stateFalling");
                        _setPValves(stateFalling);
                    } else {
                        _printsln();
                        _setPValves(stateIdle);
                    }
                } else _printsln();
                break;
            case stateRising:
                if (params.pInc)
                    faults.check(pTooLo, _buildAveragePForFaultDetection());
                _printsf(", case stateRising");
                if ((params.pInc && vals.p > params.ps) || !params.pInc) {
                    _printsf(" -> stateIdle");
                    _setPValves(stateIdle);
                } else { // um ev. abschalten (Fehler) zu können
                    _setPValves(stateRising);
                }
                _printsln();
                break;
            case stateFalling:
                if (params.pDec)
                    faults.check(pTooHi, _buildAveragePForFaultDetection());
                _printsf(", case stateFalling");
                if ((params.pDec && vals.p < params.ps) || !params.pDec) {
                    _printsf(" -> stateIdle");
                    _setPValves(stateIdle);
                } else { // um ev. abschalten (Fehler) zu können
                    _setPValves(stateFalling);
                }
                _printsln();
                break;
            }
        } else if (vals.p == _SENSOR_FAULT && params.cEn && (params.pInc || params.pDec)) {
            _printsln();
            faults.check(pInvalid, _buildAveragePForFaultDetection());
            _setPValves(stateIdle);
        } else {
            _printsln();
            faults.pIdle();
            _setPValves(stateIdle);
            // faults.checkP(_buildAveragePForFaultDetection());
            // _p_i_fault_start = _p_i_curr;
        }
        _printsf("vStates.pInc: "); _prints(vStates.pInc);
        _printsf(", vStates.pDec: "); _printsln(vStates.pDec);
    }
    faults.process();
}

void Controller::_setT1Valve(uint8_t val, States state)
{
    _tState = state;
    if (faults.tActive() && !faultSettings.tOnIfError)
        val = 0; // Ventile ausschalten
    digitalWrite(_t1Pin, val);
    vStates.t1 = val;
    _setModbusValve(_MODBUS_VALVE_T1_INDEX, val);
}

void Controller::_setPValves(States state)
{
    _pState = state;
    if (faults.pActive() && !faultSettings.pOnIfError)
        state = stateIdle; // Ventile ausschalten
    switch (state) {
    case stateRising:
        digitalWrite(_prPin, 1);
        vStates.pInc = true;
        _setModbusValve(_MODBUS_VALVE_PR_INDEX, 1);
        digitalWrite(_pfPin, 0);
        vStates.pDec = false;
        _setModbusValve(_MODBUS_VALVE_PF_INDEX, 0);
        break;
    case stateFalling:
        digitalWrite(_pfPin, 1);
        vStates.pDec = true;
        _setModbusValve(_MODBUS_VALVE_PF_INDEX, 1);
        digitalWrite(_prPin, 0);
        vStates.pInc = false;
        _setModbusValve(_MODBUS_VALVE_PR_INDEX, 0);
        break;
    default: // idle valves
        digitalWrite(_pfPin, 0);
        vStates.pDec = false;
        _setModbusValve(_MODBUS_VALVE_PF_INDEX, 0);
        digitalWrite(_prPin, 0);
        vStates.pInc = false;
        _setModbusValve(_MODBUS_VALVE_PR_INDEX, 0);
    }
}

#if _MODBUS == 1
void Controller::resetAverageCounters()
{
    _printbf("Setze Zähler zurück (");
#if _MODBUS_AVERAGE_TEMPERATURES == 1
    _printbf("_t1_c: "); _printb(_t1_c);
    _printbf(", _t2_c: "); _printb(_t2_c);
    _printbf(", ");
    _t1_c = 0;
    _t2_c = 0;
    _t1_of = false;
    _t2_of = false;
#endif
    _printbf("_p_i_curr: "); _printb(_p_i_curr); _printblnf(")");
    _p_i_mb_start = _p_i_curr;
    _p_mb_full = false;
}

#if _MODBUS_AVERAGE_TEMPERATURES == 1
inline void Controller::_averageTemperature1()
{
    int32_t total = 0;
    int16_t result = _SENSOR_FAULT;
    u8 valid = 0;
    u8 invalid = 0;
    if (_t1_c >= _T_ARR_LEN) {
        _t1_c = 0;
        _t1_of = true;
    }
    // Falls das Array schon voll ist, wird es wieder vom Anfang befüllt
    _t1_arr[_t1_c++] = vals.t1;
    // Dabei läuft die for-Schleife dann über alle Einträge, ansonsten nur bis zum aktuellen
    u16 loopTill = (_t1_of) ? _T_ARR_LEN : _t1_c;
    for (u16 i=0; i<loopTill; i++) {
        if (_t1_arr[i] != _SENSOR_FAULT) {
            total += _t1_arr[i];
            valid++;
        } else {
            invalid++;
        }
    }
    if (valid >= invalid) {
        result = total / valid;
    }
    modbusData[_MODBUS_T1_INDEX] = result;
    // _setModbusValue(_MODBUS_T1_INDEX, result);
}

inline void Controller::_averageTemperature2()
{
    // noch nicht implementiert
}
#endif // _MODBUS_AVERAGE_TEMPERATURES == 1
#endif // _MODBUS == 1

inline void Controller::_averagePressure()
{
    // Falls das Ende des Arrays erreicht ist, wird es wieder vom Anfang befüllt
    if (_p_i_curr > _P_ARR_LEN) {
        _p_i_curr = 0;
        _p_mb_full = true;
    }
    _p_arr[_p_i_curr] = vals.p;
#if _MODBUS == 1
    int32_t total = 0;
    int16_t result = _SENSOR_FAULT;
    u16 valid = 0;
    u16 invalid = 0;
    u16 i;
    if (_p_i_curr > _p_i_mb_start && !_p_mb_full) {
        for (i=_p_i_mb_start; i<_p_i_curr; i++) {
            if (_p_arr[i] != _SENSOR_FAULT) {
                total += _p_arr[i];
                valid++;
            } else {
                invalid++;
            }
        }
    } else if (_p_i_curr < _p_i_mb_start && !_p_mb_full) {
        for (i=0; i<_p_i_curr; i++) {
            if (_p_arr[i] != _SENSOR_FAULT) {
                total += _p_arr[i];
                valid++;
            } else {
                invalid++;
            }
        }
        for (i=_p_i_mb_start; i<_P_ARR_LEN; i++) {
            if (_p_arr[i] != _SENSOR_FAULT) {
                total += _p_arr[i];
                valid++;
            } else {
                invalid++;
            }
        }
    } else {
        for (i=0; i<_P_ARR_LEN; i++) {
            if (_p_arr[i] != _SENSOR_FAULT) {
                total += _p_arr[i];
                valid++;
            } else {
                invalid++;
            }
        }
    }
    if (valid >= invalid) {
        result = total / valid;
    }
    modbusData[_MODBUS_P_INDEX] = result;
#endif // _MODBUS == 1
    _p_i_curr++;
}

inline int16_t Controller::_buildAveragePForFaultDetection()
{
    int32_t total = 0;
    int16_t result = _SENSOR_FAULT;
    u16 valid = 0;
    u16 invalid = 0;
    u16 i;
    if (_p_i_curr > _p_i_fault_start) {
        for (i=_p_i_fault_start; i<_p_i_curr; i++) {
            if (_p_arr[i] != _SENSOR_FAULT) {
                total += _p_arr[i];
                valid++;
            } else {
                invalid++;
            }
        }
    } else if (_p_i_curr < _p_i_fault_start) {
        for (i=0; i<_p_i_curr; i++) {
            if (_p_arr[i] != _SENSOR_FAULT) {
                total += _p_arr[i];
                valid++;
            } else {
                invalid++;
            }
        }
        for (i=_p_i_fault_start; i<_P_ARR_LEN; i++) {
            if (_p_arr[i] != _SENSOR_FAULT) {
                total += _p_arr[i];
                valid++;
            } else {
                invalid++;
            }
        }
    } else {
        for (i=0; i<_P_ARR_LEN; i++) {
            if (_p_arr[i] != _SENSOR_FAULT) {
                total += _p_arr[i];
                valid++;
            } else {
                invalid++;
            }
        }
    }
    if (valid >= invalid) {
        result = total / valid;
    }
    // _p_i_fault_start = _p_i_curr;
    // _printf("Durchschnittsdruck: "); _println(result);
    return result;
}