#include <Arduino.h>
#include <OneWire.h>
#include <DallasTemperature.h>
#include <SPI.h>
#include <U8g2lib.h>
#include <PID_v1.h>
#include <EEPROM.h>
#include "TwoWayMotorisedBallValve.h"
#include "EncoderTimer.h"
#include "config.h"

OneWire oneWire1(T_GLOWICA_BUS);
OneWire oneWire2(T_ZBIORNIK_BUS);
OneWire oneWire3(T_10_POLKA_BUS);
OneWire oneWire4(T_WODA_BUS);
OneWire oneWire5(T_OLM_BUS);
DallasTemperature tGlowicaSensor(&oneWire1);
DallasTemperature tZbiornikSensor(&oneWire2);
DallasTemperature t10PolkaSensor(&oneWire3);
DallasTemperature tWodaSensor(&oneWire4);
DallasTemperature tOlmSensor(&oneWire5);
Temperatures temperatures;

U8G2_SSD1306_128X64_NONAME_1_HW_I2C display(U8G2_R0);

Encoder encoder(ENC_DT, ENC_CLK, ENC_SW);

volatile bool TwoWayMotorisedBallValve::openLimitReached;
volatile bool TwoWayMotorisedBallValve::closeLimitReached;
uint8_t TwoWayMotorisedBallValve::openLimitPin;
uint8_t TwoWayMotorisedBallValve::closeLimitPin;
double coolingWaterPIDSetpoint = 0.0;
double coolingWaterPIDInput = 0.0;
double ValveSetPoint = 0.0;
PID coolingWaterValvePID(&coolingWaterPIDInput, &ValveSetPoint, &coolingWaterPIDSetpoint, 2, 5, 1, DIRECT);
TwoWayMotorisedBallValve coolingWaterValve;

uint8_t currentStage = 0;
Settings settings;

unsigned long timestamp = 0;
unsigned long waterTempOkTime = 0;
bool tDniaOk = false;
bool inMenu = false;
bool inSetValue = false;
String displayText = "";
int8_t currentSelection = 0;

void setNewStage(uint8_t newStage)
{
    currentStage = newStage;
}

void readTemperatureSensors()
{
    static bool requestSent = false;
    static unsigned long lastRequestTime = 0;

    if (!requestSent && (millis() - lastRequestTime >= SECONDS_1))
    {
        tGlowicaSensor.requestTemperatures();
        tZbiornikSensor.requestTemperatures();
        t10PolkaSensor.requestTemperatures();
        tWodaSensor.requestTemperatures();
        tOlmSensor.requestTemperatures();

        requestSent = true;
        lastRequestTime = millis();
    }

    if(!tGlowicaSensor.isConversionComplete() || !tZbiornikSensor.isConversionComplete() || !t10PolkaSensor.isConversionComplete() ||
       !tWodaSensor.isConversionComplete() || !tOlmSensor.isConversionComplete())
    {
        return;
    }

    float t = tGlowicaSensor.getTempCByIndex(0);
    temperatures.glowica = (t == DEVICE_DISCONNECTED_C ? temperatures.glowica : t);
    t = tZbiornikSensor.getTempCByIndex(0);
    temperatures.zbiornik = (t == DEVICE_DISCONNECTED_C ? temperatures.zbiornik : t);
    t = t10PolkaSensor.getTempCByIndex(0);
    temperatures.t10polka = (t == DEVICE_DISCONNECTED_C ? temperatures.t10polka : t);
    t = tWodaSensor.getTempCByIndex(0);
    temperatures.woda = (t == DEVICE_DISCONNECTED_C ? temperatures.woda : t);
    t = tOlmSensor.getTempCByIndex(0);
    temperatures.olm = (t == DEVICE_DISCONNECTED_C ? temperatures.olm : t);
    requestSent = false;
}

void printStrCenter(String text, int y, int offset = 0)
{
    int titlePosX = (display.getWidth() / 2) - (display.getStrWidth(text.c_str()) / 2);
    display.drawStr(titlePosX + offset, y, text.c_str());
}

void updateDisplay(bool forceUpdate = false)
{
    static unsigned long lastUpdate = 0;

    if (!forceUpdate && (millis() - lastUpdate < SECONDS_1))
    {
        return;
    }

    display.firstPage();
    do
    {
        if (inSetValue)
        {
            display.setFont(u8g2_font_6x12_tf);
            printStrCenter(currentSelection == 1 ? "MOC ROBOCZA" : "KOREKTA ODBIORU", 10);
            
            uint8_t value = currentSelection == 1 ? settings.g1HeaterPower : settings.ezGonValue;
            uint8_t barWidth = map(value, 0, 100, 0, 128);
            display.drawFrame(0, 20, 128, 10);
            display.drawBox(0, 20, barWidth, 10);

            display.setCursor(0, 50);
            printStrCenter(String(value) + "%", 40);
        }
        else if (inMenu)
        {
            display.setFont(u8g2_font_6x12_tf);
            printStrCenter("MENU", 10);
            for (int i = 0; i < 4; i++)
            {
                if (i == currentSelection)
                {
                    display.drawBox(0, 12 + i * 12, 128, 12);
                    display.setDrawColor(0);
                }

                display.setCursor(2, 22 + i * 12);
                switch (i)
                {
                    case 0: display.print("START/STOP"); break;
                    case 1: display.print("MOC ROBOCZA"); break;
                    case 2: display.print("KOREKTA ODBIORU"); break;
                    case 3: display.print("WYJSCIE"); break;
                }

                if (i == currentSelection)
                {
                    display.setDrawColor(1);
                }
            }
        }
        else
        {
            display.drawFrame(0, 0, display.getWidth(), display.getHeight());

            display.setFont(u8g2_font_6x12_tf);
            printStrCenter("GLOWICA", 10, -38);
            printStrCenter("10 POLKA", 27, -38);
            printStrCenter("OLM", 44, -38);
            printStrCenter("ZBIORNIK", 10, 37);
            printStrCenter("WODA", 27, 37);
            printStrCenter("T. DNIA", 44, 37);

            display.drawBox(0, 53, display.getWidth(), 11);
            display.setDrawColor(0);
            printStrCenter(displayText, 62);
            display.setDrawColor(1);

            display.setFont(u8g2_font_4x6_tf);
            printStrCenter(String(temperatures.glowica, 2), 17, -38);
            printStrCenter(String(temperatures.t10polka, 2), 34, -38);
            printStrCenter(String(temperatures.olm, 2), 51, -38);
            printStrCenter(String(temperatures.zbiornik, 2), 17, 37);
            printStrCenter(String(temperatures.woda, 2), 34, 37);
            printStrCenter(tDniaOk ? String(temperatures.tDnia, 2) : "----", 51, 37);
        }
    } while (display.nextPage());

    lastUpdate = millis();
}

void displayPrint(String text, uint8_t y)
{
    display.firstPage();
    do
    {
        display.setFont(u8g2_font_6x12_tf);
        display.setDrawColor(1);
        printStrCenter(text, y);

    } while (display.nextPage());
}

void handleCoolingValvePID(double setpoint, bool fullyClose = false)
{
    if (fullyClose)
    {
        coolingWaterValve.fullyClose();
        return;
    }

    coolingWaterPIDSetpoint = setpoint;
    coolingWaterPIDInput = temperatures.woda;
    Serial.print(coolingWaterPIDSetpoint);
    Serial.print(" ");
    Serial.print(coolingWaterPIDInput);
    Serial.print(" ");
    Serial.println(ValveSetPoint);
    coolingWaterValvePID.Compute();
    coolingWaterValve.update();
}

void stopAll()
{
    analogWrite(HEATER_1, 0);
    digitalWrite(HEATER_2, LOW);
    digitalWrite(EZ_POG, LOW);
    analogWrite(EZ_GON, 0);
    handleCoolingValvePID(0, true);
    tDniaOk = false;
}

void handleEncoder()
{
    static bool lastButtonState = false;

    int8_t encVal = encoder.deltaTick2();
    bool buttonState = encoder.button();
    bool buttonStateChanged = lastButtonState != buttonState;

    if (inSetValue)
    {
        if (currentSelection == 1)
        {
            settings.g1HeaterPower += encVal;
        }
        else
        {
            settings.ezGonValue += encVal;
        }

        settings.g1HeaterPower = constrain(settings.g1HeaterPower, 0, 100);
        settings.ezGonValue = constrain(settings.ezGonValue, 0, 100);

        if (buttonStateChanged && buttonState)
        {
            inSetValue = false;
            EEPROM.put(0, settings);

            if (currentStage != 0)
            {
                analogWrite(HEATER_1, (255 * settings.g1HeaterPower) / 100);
            }

            if (currentStage == 6)
            {
                analogWrite(EZ_GON, (255 * settings.ezGonValue) / 100);
            }
        }

        if (encVal != 0 || (buttonState && buttonStateChanged))
        {
            updateDisplay(true);
        }
    }
    else if (inMenu)
    {
        currentSelection += encVal;
        currentSelection = constrain(currentSelection, 0, 3);

        if (buttonState && buttonStateChanged)
        {
            if (currentSelection == 0)
            {
                if (currentStage == 0)
                {
                    analogWrite(HEATER_1, (255 * settings.g1HeaterPower) / 100);
                    digitalWrite(HEATER_2, HIGH);
                    setNewStage(1);
                    waterTempOkTime = millis();
                    temperatures.tDnia = 82.0;
                    tDniaOk = false;
                    displayText = "START PROCESU";
                }
                else
                {
                    stopAll();
                    displayText = "OCZEKIWANIE NA START";
                    setNewStage(0); 
                }
                
                inMenu = false;
                updateDisplay(true);
            }
            else if (currentSelection == 1 || currentSelection == 2)
            {
                inSetValue = true;
            }
            else if (currentSelection == 3)
            {
                inMenu = false;
            }
        }

        if (encVal != 0 || (buttonState && buttonStateChanged))
        {
            updateDisplay(true);
        }
    }
    else if (buttonState && buttonStateChanged)
    {
        inMenu = true;
        currentSelection = 0;
        updateDisplay(true);
    }

    lastButtonState = buttonState;
}

void setup()
{
    Serial.begin(115200);
    EEPROM.get(0, settings);
    settings.g1HeaterPower = constrain(settings.g1HeaterPower, 0, 100);
    settings.ezGonValue = constrain(settings.ezGonValue, 0, 100);

    pinMode(HEATER_1, OUTPUT);
    pinMode(HEATER_2, OUTPUT);
    pinMode(EZ_GON, OUTPUT);
    pinMode(EZ_POG, OUTPUT);

    display.begin();
    displayPrint("Kalibracja zaworu", 30);
    EncoderInterrupt.begin(&encoder);

    tGlowicaSensor.begin();
    tZbiornikSensor.begin();
    t10PolkaSensor.begin();
    tWodaSensor.begin();
    tOlmSensor.begin();
    tGlowicaSensor.setWaitForConversion(false);
    tZbiornikSensor.setWaitForConversion(false);
    t10PolkaSensor.setWaitForConversion(false);
    tWodaSensor.setWaitForConversion(false);
    tOlmSensor.setWaitForConversion(false);
    memset(&temperatures, 0, sizeof(temperatures));
    temperatures.tDnia = 82.0;

    coolingWaterValvePID.SetMode(AUTOMATIC);
    coolingWaterValve.begin(&ValveSetPoint, OPEN_VALVE_PIN, CLOSE_VALVE_PIN, MOTOR_SPEED_PIN, OPEN_LIMIT_PIN, CLOSE_LIMIT_PIN);

    stopAll();
    setNewStage(2);
    displayText = "OCZEKIWANIE NA START";
}

void loop()
{
    readTemperatureSensors();

    if (currentStage != 0)
    {
        if (temperatures.t10polka >= 80.0)
        {
            digitalWrite(HEATER_2, LOW);
        }

        if (temperatures.woda >= 70.0)
        {
            if (millis() - waterTempOkTime >= MINUTES_3)
            {
                stopAll();
                displayText = "OCZEKIWANIE NA START";
                setNewStage(0);
            }
        }
        else if (temperatures.woda >= 60.0)
        {
            String tempText = displayText;
            displayText = "SPRAWDZ CHLODZENIE";
            updateDisplay(true);
            delay(1000);
            displayText = tempText;
            waterTempOkTime = millis();
        }
        else
        {
            waterTempOkTime = millis();
        }
    }

    if (currentStage == 0)
    {
        coolingWaterValve.update();
    }

    if (currentStage >= 2)
    {
        handleCoolingValvePID(37.5);
    }

    if (currentStage == 1)
    {
        if (temperatures.olm >= 65.0)
        {
            timestamp = millis();
            setNewStage(2);
        }
    }
    else if (currentStage == 2)
    {
        if (temperatures.olm >= 75.0)
        {
            // Ustalanie temperatury dnia
            if ((temperatures.t10polka < (temperatures.tDnia - 0.125)) || (temperatures.t10polka > (temperatures.tDnia + 0.125)))
            {
                temperatures.tDnia = temperatures.t10polka;
                timestamp = millis();
            }

            if (millis() - timestamp >= MINUTES_11)
            {
                temperatures.tDnia += 0.125;
                tDniaOk = true;
                setNewStage(3);
            }
        }
        else
        {
            timestamp = millis();
        }
    }
    else if (currentStage == 3)
    {
        if (temperatures.t10polka >= 75.0)
        {
            timestamp = millis();
            setNewStage(4);
        }
    }
    else if (currentStage == 4)
    {
        displayText = "STABILIZACJA: " + String((MINUTES_10 - (millis() - timestamp)) / 1000);

        if ((millis() - timestamp >= MINUTES_10) && (temperatures.olm >= 75.0))
        {
            displayText = "ODBIOR PRZEDGONU";
            digitalWrite(EZ_POG, HIGH);
            timestamp = millis();
            setNewStage(5);
        }
    }
    else if (currentStage == 5)
    {
        if (millis() - timestamp >= MINUTES_15)
        {
            displayText = "ODBIOR GONU";
            digitalWrite(EZ_POG, LOW);
            analogWrite(EZ_GON, (255 * settings.ezGonValue) / 100);
            setNewStage(6);
        }
    }
    else if (currentStage == 6)
    {
        if ((temperatures.t10polka > temperatures.tDnia + 0.5) || (temperatures.t10polka < temperatures.tDnia - 0.5))
        {
            analogWrite(EZ_GON, 0);
            timestamp = millis();
        }

        if (millis() - timestamp >= SECONDS_5)
        {
            analogWrite(EZ_GON, (255 * settings.ezGonValue) / 100);
        }

        if (temperatures.zbiornik >= 98.1)
        {
            displayText = "ODBIOR POGONU";
            analogWrite(EZ_GON, 0);
            digitalWrite(EZ_POG, HIGH);
            setNewStage(7);
        }
    }
    else if (currentStage == 7)
    {
        if ((temperatures.t10polka >= (temperatures.tDnia + 2.0)) && temperatures.zbiornik >= 99.0)
        {
            displayText = "KONIEC PROCESU";
            digitalWrite(EZ_POG, LOW);
            analogWrite(HEATER_1, 0);
            timestamp = millis();
            setNewStage(8);
        }
    }
    else if (currentStage == 8)
    {
        if (millis() - timestamp >= MINUTES_2)
        {
            stopAll();
            displayText = "OCZEKIWANIE NA START";
            setNewStage(0);
        }
    }

    updateDisplay();
    handleEncoder();
}