Thermostaat met FlappyBird

//**************************************************************************************
// Code voor een wifi thermostaat met een DHT sensor (11 of 22) en touchscreen TFT    **
//**************************************************************************************

//ESPWebtime code voor het ophalen van tijd via
#include <NTPClient.h>

#include <ESP8266WiFi.h>

#include <WiFiUdp.h>

#include <EEPROM.h>

#include "Adafruit_MQTT.h"
#include "Adafruit_MQTT_Client.h"
#define AIO_SERVER "192.168.2.33"
#define AIO_SERVERPORT 1883 // use 8883 for SSL
uint32_t x = 0;

WiFiClient client;
Adafruit_MQTT_Client mqtt(&client, AIO_SERVER, AIO_SERVERPORT);

Adafruit_MQTT_Publish ToggleSwitch1 = Adafruit_MQTT_Publish(&mqtt, "cmnd/Schakelaar01/power");
Adafruit_MQTT_Subscribe ToggleSwitch1Status = Adafruit_MQTT_Subscribe(&mqtt, "stat/Schakelaar01/POWER");
Adafruit_MQTT_Publish ToggleSwitch2 = Adafruit_MQTT_Publish(&mqtt, "cmnd/Schakelaar02/power");
Adafruit_MQTT_Subscribe ToggleSwitch2Status = Adafruit_MQTT_Subscribe(&mqtt, "stat/Schakelaar02/POWER");

#include "flappyBird.h"

const char* ssid = "";
const char* password = "";

const long utcOffsetInSeconds = 3600;

// Define NTP Client to get time
WiFiUDP ntpUDP;
NTPClient timeClient(ntpUDP, "pool.ntp.org", utcOffsetInSeconds);

#include <SPI.h>

#include <Adafruit_GFX.h>

#include <Adafruit_ILI9341.h>

#include <XPT2046_Touchscreen.h>

#define TFT_CS D0 //for D1 mini or TFT I2C Connector Shield (V1.1.0 or later)
#define TFT_DC D8 //for D1 mini or TFT I2C Connector Shield (V1.1.0 or later)
#define TFT_RST -1 //for D1 mini or TFT I2C Connector Shield (V1.1.0 or later)
#define TS_CS D3 //for D1 mini or TFT I2C Connector Shield (V1.1.0 or later)

// #define TFT_CS 14  //for D32 Pro
// #define TFT_DC 27  //for D32 Pro
// #define TFT_RST 33 //for D32 Pro
// #define TS_CS  12 //for D32 Pro

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);
XPT2046_Touchscreen ts(TS_CS);

#include <DHT.h> // DHT sensor (temp en luchtvochtigheid)

// Fonts https://rop.nl/truetype2gfx/
#include "Segment10pt7b.h"

#include "Segment18pt7b.h"

#include "Segment22pt7b.h"

#include "Segment48pt7b.h"

#include "icons14pt7b.h"

#include "icons30pt7b.h"

//DHT sensor
#define DHTPIN D4
#define DHTTYPE DHT11

//Vertalen
#define TS_MINX 230
#define TS_MAXX 3600

#define TS_MINY 330
#define TS_MAXY 3700

//https://github.com/wemos/D1_mini_Examples/blob/master/examples/04.Shields/TFT_2.4_Touch_Shield/TFT_TouchTest/TFT_TouchTest.ino

DHT dht(DHTPIN, DHTTYPE);

// 16-bit kleuren definities: https://ee-programming-notepad.blogspot.com/2016/10/16-bit-color-generator-picker.html
#define BLACK 0x0000
#define WHITE 0xFFFF
#define BUTTONBG 0xFFFF
#define BUTTONBGACT 0xDEFB
#define BUTTONEDGE 0x9492
#define TEXTCOLOR 0x0000
#define SCREENCOLOR 0xFFFF
#define TOGGLEON 0x53C9
#define TOGGLEOFF 0xDEFB
#define HEATON 0xD1E3
#define HEATOFF 0xDEFB
#define WIFION 0x0000
#define WIFIOFF 0xD1E3
#define FLAPPYSKY 0xAE7F
#define FLAPPYGRASS 0x3E29
#define FLAPPYBOX 0x558B

//Drukbereik touchscreen
#define MINPRESSURE 400
#define MAXPRESSURE 3000

//eeprom adressen van de opgeslagen variabelen

// Variabelen
unsigned long currentTimeMillis = millis();
unsigned long previousTimeMillis;
unsigned long currentGetTempMillis = millis();
unsigned long previousGetTempMillis;
unsigned long currentScreenTimOuteMillis = millis();
unsigned long previousScreenTimeOutMillis;
int TimeOutPeriod = 20000; //Timeout voor het dimmen van het scherm
int DimMode = 1; //1= helder, 2 = gedimd, 3 = nachtscherm, 4 = uit
float TempSet = 14; //Ingestelde temperatuur
float PrevTempSet;
int D = 1;
String DayNames[7] = { "Zo", "Ma", "Di", "Wo", "Do", "Vr", "Za" };
String LongDayNames[7] = { "Zondag", "Maandag", "Dinsdag", "Woensdag", "Donderdag", "Vrijdag", "Zaterdag" };
String DD = DayNames[1];
String DDD = LongDayNames[1];
int H = 112; // Uren binnen honderd (om de eerste 1 er weer vanaf te kunnen knippen)
int M = 100; // Minuten
int HM = 1200; // UrenMinuten
String HH;
String MM;
String DisplayTime;
String PrevDisplayTime;
float Temp = dht.readTemperature();
float PrevTemp;
float SchemaTempH = 18.5;
float SchemaTempL = 14;
int SchemaTempHint = 185;
int SchemaTempLint = 140;
float PrevSchemaTempH = 0;
float PrevSchemaTempL = 0;
int Humid = dht.readHumidity();
int PrevHumid;
String RecieveFromSerial;
String TimePartH;
String TimePartM;
int HeatState = 0;
int PrevHeatState = 2;
int power1 = 0;
int power2 = 0;
int DisplayScreen = 1;
int PrevDisplayScreen = 0;
int ScreenTouch = 0;
int Toggle1 = 0;
int Toggle2 = 0;
int PrevToggle1 = 2;
int PrevToggle2 = 2;
int WifiStatus = 0;
int PrevWifiStatus = 2;
int HeatingMode = 3; //1=automatisch 2=uitschakeltimer 3=handmatig
String HeatingModeIcon1 = "Z";
String HeatingModeIcon2 = "k";
String HeatingModeIcon3 = "b";
int PrevHeatingMode = 9;
//String HeatingModeIcon[] = {"Z","k","b"};
int NightModeTime = 2230;
int DayModeTime = 700;
int DimBrightness = 100;
int PrevDimBrightness = 0;
int WifiTimeOutCounter = 0;

//Schema variabelen
int thermostatProg[42];
int EEPROMAddrTempHigh;
int EEPROMAddrTempLow;
int EEPROMAddrTimeHigh;
int EEPROMAddrTimeLow;
int SchemaNumber = 1;
int PrevSchemaNumber = 4;
int TimeHighH = 7;
int TimeHighM = 0;
int TimeHighHM = 7;
int TimeLowH = 18;
int TimeLowM = 0;
int TimeLowHM = 18;
int PrevTimeHighH = 99;
int PrevTimeHighM = 99;
int PrevTimeLowH = 99;
int PrevTimeLowM = 99;
int SetDay = 1;
int PrevSetDay = 9;

//schermconfiguratie
//Icoontjes: z Power, O sun high, Q Sun low, k clock, = Wifi, ! error, V checkbox checked, W checkbox empty, U Unlocked, L Locked, d Settings, F WaterDrop, 4 up, ~ down, € €, B Menu btn, h Menu open, , arrow < , . arrow >, Y Pin
int MenuBtnX = 140;
int MenuBtnY = 200;
int CurTempX = 5;
int CurTempY = 50;
String TitleCurTemp = "";
int SetTempX = 0;
int SetTempY = 100;
String TitleSetT = "";
int CurTemp2X = 110;
int CurTemp2Y = 50;
String TitleCurTemp2 = "";
int SetTemp2X = 62;
int SetTemp2Y = 100;
String TitleSetT2 = "";
int UpBtnX = 260;
int UpBtnY = 120;
char UpBtnIcon = '+';
int DwnBtnX = 10;
int DwnBtnY = 120;
char DwnBtnIcon = '~';
int TimeDisplX = 100;
int TimeDisplY = 5;
int HumidX = 5;
int HumidY = 210;
String HumidIcon = "F";
int Toggle1X = 210;
int Toggle1Y = 30;
char Toggle1Icon = 'z';
String Toggle1Title = "";
int Toggle2X = 210;
int Toggle2Y = 120;
char Toggle2Icon = 'z';
int HeatStateX = 285;
int HeatStateY = 232;
int HeatModeX = 240;
int HeatModeY = 232;

char HeatOnIcon = 'O';
char HeatOffIcon = 'Q';
int WifiPosX = 10;
int WifiPosY = 25;
char WifiOnIcon = '=';
char WifiOffIcon = '!';

String SaveIcon = "j";
String MenuIcon = "H";
String MenuOption1 = "Automatisch";
String MenuOption2 = "Uitschakeltimer";
String MenuOption3 = "Handmatig";
String MenuOption4 = "Schema";
String MenuOption5 = "Schakelaarklok";
String MenuOption6 = "Andere ruimte";

String MenuOption7 = "Instellingen";
String MenuOption8 = "Scherm";
String MenuOption9 = "Nachtscherm";
String MenuOption10 = "FlappyBird";

void setup()
{
    Serial.begin(9600);
    EEPROM.begin(512);
    EEPROM.get(0, DimBrightness);
    timeClient.begin();
    timeClient.setUpdateInterval(3600000); //Elk uur updaten i.p.v. elke minuut
    dht.begin();
    initDisplay();
    pinMode(D1, OUTPUT);
    digitalWrite(D1, 1);
    tft.fillScreen(SCREENCOLOR);
    tft.setTextColor(TEXTCOLOR);
    tft.setFont(&Segment10pt7b);
    tft.setCursor(50, 150);
    tft.print("Verbinden");

    WiFi.begin(ssid, password);

    while ((WiFi.status() != WL_CONNECTED) && (WifiTimeOutCounter < 30)) {
        delay(500);
        tft.print(".");
        WifiTimeOutCounter += 1;
    }
    GetWifiStatus();
    tft.fillScreen(SCREENCOLOR);
    GetTime();
    delay(500);
    mqtt.subscribe(&ToggleSwitch1Status);
}

void MQTT_connect();

void initDisplay()
{
    ts.begin();
    ts.setRotation(3);
    tft.begin();
    tft.setRotation(3);
}

void ScreenTimeOut()
{
    currentScreenTimOuteMillis = millis();
    if ((previousScreenTimeOutMillis + TimeOutPeriod < currentScreenTimOuteMillis) && (DisplayScreen != 8)) {
        if (((H - 100) * 100 + (M - 100) > NightModeTime) || ((H - 100) * 100 + (M - 100) < DayModeTime)) {
            digitalWrite(D1, 0);
            DisplayScreen = -1;
        }
        else {
            analogWrite(D1, DimBrightness);
        }
    }
    if (previousScreenTimeOutMillis + TimeOutPeriod + 2000 < currentScreenTimOuteMillis) {
        if (((H - 100) * 100 + (M - 100) > NightModeTime) || ((H - 100) * 100 + (M - 100) < DayModeTime)) {
        }
        else {
            DisplayScreen = 1;
        }
    }
}

void GetWifiStatus()
{
    if (WiFi.status() == WL_CONNECTED) {
        WifiStatus = 1;
    }
    else {
        WifiStatus = 0;
        WiFi.reconnect();
    }
}

void DrawWifiStatus(int posX, int posY)
{
    if ((WifiStatus != PrevWifiStatus) || (DisplayScreen != PrevDisplayScreen)) {
        tft.fillRect(posX - 4, posY - 20, 35, 35, SCREENCOLOR);
        tft.setCursor(posX, posY);
        tft.setFont(&icons14pt7b);
        if (WifiStatus == 1) {
            tft.setTextColor(WIFION);
            tft.print(WifiOnIcon);
        }
        else {
            tft.setTextColor(WIFIOFF);
            tft.print(WifiOffIcon);
        }
        PrevWifiStatus = WifiStatus;
    }
}
void GetTime()
{
    timeClient.update();

    H = timeClient.getHours() + 100;
    M = timeClient.getMinutes() + 100;
    D = timeClient.getDay();
    DD = DayNames[D];
    DDD = LongDayNames[D];
}

void DrawHeatState(int posX, int posY)
{
    if (DisplayScreen != PrevDisplayScreen) {
        PrevHeatState = 2;
    }
    tft.setCursor(posX, posY);
    tft.setFont(&icons14pt7b);
    if (Temp < TempSet) {
        HeatState = 1;
        tft.setTextColor(HEATON);
        if (HeatState != PrevHeatState) {
            tft.fillRect(posX - 4, posY - 20, 35, 35, SCREENCOLOR);
            tft.print(HeatOnIcon);
            PrevHeatState = 1;
        }
    }
    else {
        HeatState = 0;
        tft.setTextColor(HEATOFF);
        if (HeatState != PrevHeatState) {
            tft.fillRect(posX - 4, posY - 20, 35, 35, SCREENCOLOR);
            tft.print(HeatOffIcon);
            PrevHeatState = 0;
        }
    }
}

void DrawHeatingMode(int posX, int posY)
{
    if ((DisplayScreen != PrevDisplayScreen) || (HeatingMode != PrevHeatingMode)) {
        tft.setCursor(posX, posY);
        tft.setFont(&icons14pt7b);
        tft.setTextColor(TEXTCOLOR);
        tft.fillRect(posX - 4, posY - 20, 35, 35, SCREENCOLOR);
        if (HeatingMode == 1) {
            tft.print(HeatingModeIcon1);
        }
        else if (HeatingMode == 2) {
            tft.print(HeatingModeIcon2);
        }
        else if (HeatingMode == 3) {
            tft.print(HeatingModeIcon3);
        }
        PrevHeatingMode = HeatingMode;
    }
}
void ToggleHeating()
{
    if (DisplayScreen != PrevDisplayScreen) {
        //        getTemp();
        DrawHeatState(HeatStateX, HeatStateY);
    }
}

void RecTime()
{
    if ((Serial.read()) > (0)) {
        RecieveFromSerial = String(Serial.readStringUntil('\n'));
        if (((RecieveFromSerial.substring(1, 1)) == ("T"))) {
            TimePartH = RecieveFromSerial.substring(0, 2);
            TimePartM = RecieveFromSerial.substring(2, 4);
            H = TimePartH.toInt() + 100;
            M = TimePartM.toInt() + 100;
            DrawTime(TimeDisplX, TimeDisplY);
        }
    }
}

void DrawCurTemp(int posX, int posY)
{
    getTemp();
    if ((Temp != PrevTemp) || (DisplayScreen != PrevDisplayScreen)) {
        tft.setFont(&Segment10pt7b);
        tft.setCursor(posX, posY - 4);
        tft.setTextColor(TEXTCOLOR);
        tft.print(TitleCurTemp);
        getTemp();
        char T[8];
        dtostrf(Temp, 6, 1, T);
        tft.fillRoundRect(posX, posY, 110, 45, 5, BUTTONBG);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&Segment22pt7b);
        tft.setCursor(posX - 25, posY + 42);
        tft.print(T);
        int curposx = tft.getCursorX();
        int curposy = tft.getCursorY();
        tft.setCursor(curposx + 2, curposy - 16);
        tft.setFont(&Segment10pt7b);
        tft.print("o");
        PrevTemp = Temp;
    }
}

void DrawSetTemp(int posX, int posY)
{
    if ((TempSet != PrevTempSet) || (DisplayScreen != PrevDisplayScreen)) {
        char TsetT[8];
        dtostrf(TempSet, 6, 1, TsetT);
        String TempString = String(TsetT);
        String TempString2 = String(TsetT);
        TempString.replace("1", "");
        tft.setFont(&Segment10pt7b);
        tft.setCursor(SetTempX, SetTempY);
        tft.setTextColor(TEXTCOLOR);
        tft.print(TitleSetT);
        tft.fillRoundRect(posX, posY + 4, 190, 82, 5, BUTTONBG);
        //   tft.drawRoundRect(setTposX, setTposY + 4, 190, 82, 5, BUTTONEDGE);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&Segment48pt7b);
        //als er een 1 in e temperatuur zit dan is er een andere uitlijning nodig
        if (TempString == TempString2) {
            tft.setCursor(posX - 86, posY + 75);
        }
        else {
            tft.setCursor(posX - 70, posY + 75);
        }
        tft.print(TsetT);
        int curposx = tft.getCursorX();
        int curposy = tft.getCursorY();
        tft.setCursor(curposx + 2, curposy - 35);
        tft.setFont(&Segment22pt7b);
        tft.print("o");
        PrevTempSet = TempSet;
    }
}

void DrawSaveBtn(int posX, int posY)
{
    if (DisplayScreen != PrevDisplayScreen) {
        tft.fillRoundRect(posX, posY, 35, 35, 5, BUTTONBG);
        tft.drawRoundRect(posX, posY, 35, 35, 5, BUTTONEDGE);
        tft.setCursor(posX + 7, posY + 26);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&icons14pt7b);
        tft.print(SaveIcon);
    }
}
void DrawMenuBtn(int posX, int posY)
{
    if (DisplayScreen != PrevDisplayScreen) {
        tft.fillRoundRect(posX, posY, 35, 35, 5, BUTTONBG);
        tft.drawRoundRect(posX, posY, 35, 35, 5, BUTTONEDGE);
        tft.setCursor(posX + 7, posY + 26);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&icons14pt7b);
        tft.print(MenuIcon);
    }
}

void DrawMenu()
{
    DrawMenuBtn(MenuBtnX, MenuBtnY);
    tft.setTextColor(TEXTCOLOR);
    tft.setFont(&Segment10pt7b);
    int xpos = 5;
    int ypos = 10;

    if (HeatingMode == 1) {
        tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBGACT);
    }
    else {
        tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBG);
    }
    tft.drawRoundRect(xpos, ypos, 150, 50, 5, BUTTONEDGE);
    tft.setCursor(xpos + 10, ypos + 32);
    tft.print(MenuOption1);
    ypos += 65;

    if (HeatingMode == 2) {
        tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBGACT);
    }
    else {
        tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBG);
    }
    tft.drawRoundRect(xpos, ypos, 150, 50, 5, BUTTONEDGE);
    tft.setCursor(xpos + 10, ypos + 32);
    tft.print(MenuOption2);
    ypos += 65;

    if (HeatingMode == 3) {
        tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBGACT);
    }
    else {
        tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBG);
    }
    tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBG);
    tft.drawRoundRect(xpos, ypos, 150, 50, 5, BUTTONEDGE);
    tft.setCursor(xpos + 10, ypos + 32);
    tft.print(MenuOption3);
    ypos += 65;

    xpos = 165;
    ypos = 10;

    tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBG);
    tft.drawRoundRect(xpos, ypos, 150, 50, 5, BUTTONEDGE);
    tft.setCursor(xpos + 10, ypos + 32);
    tft.print(MenuOption4);
    ypos += 65;

    tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBG);
    tft.drawRoundRect(xpos, ypos, 150, 50, 5, BUTTONEDGE);
    tft.setCursor(xpos + 10, ypos + 32);
    tft.print(MenuOption5);
    ypos += 65;

    tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBG);
    tft.drawRoundRect(xpos, ypos, 150, 50, 5, BUTTONEDGE);
    tft.setCursor(xpos + 10, ypos + 32);
    tft.print(MenuOption6);
    ypos += 65;
}

void DrawMenu2()
{
    DrawMenuBtn(MenuBtnX, MenuBtnY);
    tft.setTextColor(TEXTCOLOR);
    tft.setFont(&Segment10pt7b);
    int xpos = 5;
    int ypos = 10;

    tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBG);
    tft.drawRoundRect(xpos, ypos, 150, 50, 5, BUTTONEDGE);
    tft.setCursor(xpos + 10, ypos + 32);
    tft.print(MenuOption7);
    ypos += 65;

    tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBG);
    tft.drawRoundRect(xpos, ypos, 150, 50, 5, BUTTONEDGE);
    tft.setCursor(xpos + 10, ypos + 32);
    tft.print(MenuOption8);

    xpos = 165;
    ypos = 10;

    tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBG);
    tft.drawRoundRect(xpos, ypos, 150, 50, 5, BUTTONEDGE);
    tft.setCursor(xpos + 10, ypos + 32);
    tft.print(MenuOption9);
    ypos += 65;

    tft.fillRoundRect(xpos, ypos, 150, 50, 5, BUTTONBG);
    tft.drawRoundRect(xpos, ypos, 150, 50, 5, BUTTONEDGE);
    tft.setCursor(xpos + 10, ypos + 32);
    tft.print(MenuOption10);
    ypos += 65;
}

void DrawToggle1(int PosX, int PosY)
{
    if ((Toggle1 != PrevToggle1) || (DisplayScreen != PrevDisplayScreen)) {
        tft.fillRoundRect(PosX, PosY, 80, 80, 15, BUTTONBG);
        tft.drawRoundRect(PosX, PosY, 80, 80, 15, BUTTONEDGE);
        tft.setFont(&icons30pt7b);
        tft.setCursor(PosX + 16, PosY + 55);
        if (Toggle1 == 0) {
            tft.setTextColor(TOGGLEOFF);
            PrevToggle1 = 0;
            tft.print(Toggle1Icon);
        }
        else {
            tft.setTextColor(TOGGLEON);
            PrevToggle1 = 1;
            tft.print(Toggle1Icon);
        }
    }
}
void DrawToggle1Dark(int PosX, int PosY)
{
    if ((Toggle1 != PrevToggle1) || (DisplayScreen != PrevDisplayScreen)) {
        tft.fillRoundRect(PosX, PosY, 80, 80, 15, BLACK);
        tft.drawRoundRect(PosX, PosY, 80, 80, 15, WHITE);
        tft.setFont(&icons30pt7b);
        tft.setCursor(PosX + 16, PosY + 55);
        if (Toggle1 == 0) {
            tft.setTextColor(TOGGLEOFF);
            PrevToggle1 = 0;
            tft.print(Toggle1Icon);
        }
        else {
            tft.setTextColor(TOGGLEON);
            PrevToggle1 = 1;
            tft.print(Toggle1Icon);
        }
    }
}

void SetToggle1()
{
    if (Toggle1 == 0) {
        Serial.print(F("cmnd/Schakelaar02/power"));
        Serial.print(x);
        Serial.print("...");
        if (!ToggleSwitch1.publish(1)) {
            Serial.println(F("Failed"));
        }
        else {
            Serial.println(F("OK!"));
        }

        Toggle1 = 1;
    }
    else {
        Toggle1 = 0;
        if (!ToggleSwitch1.publish(0)) {
            Serial.println(F("Failed"));
        }
        else {
            Serial.println(F("OK!"));
        }
    }
    DrawToggle1(Toggle1X, Toggle1Y);
}
void DrawToggle2(int PosX, int PosY)
{
    if ((Toggle2 != PrevToggle2) || (DisplayScreen != PrevDisplayScreen)) {
        tft.fillRoundRect(PosX, PosY, 80, 80, 15, BUTTONBG);
        tft.drawRoundRect(PosX, PosY, 80, 80, 15, BUTTONEDGE);
        tft.setFont(&icons30pt7b);
        tft.setCursor(PosX + 16, PosY + 55);
        if (Toggle2 == 0) {
            tft.setTextColor(TOGGLEOFF);
            PrevToggle2 = 0;
            tft.print(Toggle2Icon);
        }
        else {
            tft.setTextColor(TOGGLEON);
            PrevToggle2 = 1;
            tft.print(Toggle2Icon);
        }
    }
}
void DrawToggle2Dark(int PosX, int PosY)
{
    if ((Toggle2 != PrevToggle2) || (DisplayScreen != PrevDisplayScreen)) {
        tft.fillRoundRect(PosX, PosY, 80, 80, 15, BLACK);
        tft.drawRoundRect(PosX, PosY, 80, 80, 15, WHITE);
        tft.setFont(&icons30pt7b);
        tft.setCursor(PosX + 16, PosY + 55);
        if (Toggle2 == 0) {
            tft.setTextColor(TOGGLEOFF);
            PrevToggle2 = 0;
            tft.print(Toggle2Icon);
        }
        else {
            tft.setTextColor(TOGGLEON);
            PrevToggle2 = 1;
            tft.print(Toggle2Icon);
        }
    }
}

void SetToggle2()
{
    if (Toggle2 == 0) {
        ToggleSwitch2.publish(1);
        Toggle2 = 1;
    }
    else {
        ToggleSwitch2.publish(0);
        Toggle2 = 0;
    }
    DrawToggle2(Toggle2X, Toggle2Y);
}

void DrawUpBtn(int posX, int posY)
{
    if (DisplayScreen != PrevDisplayScreen) {
        tft.fillRoundRect(posX, posY, 50, 50, 5, BUTTONBG);
        tft.drawRoundRect(posX, posY, 50, 50, 5, BUTTONEDGE);
        tft.setCursor(posX + 12, posY + 37);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&Segment22pt7b);
        tft.print("+");
    }
}
void DrawDwnBtn(int posX, int posY)
{
    if (DisplayScreen != PrevDisplayScreen) {
        tft.fillRoundRect(posX, posY, 50, 50, 5, BUTTONBG);
        tft.drawRoundRect(posX, posY, 50, 50, 5, BUTTONEDGE);
        tft.setCursor(posX + 15, posY + 40);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&Segment22pt7b);
        tft.print("-");
    }
}
void DrawUpBtnSm(int posX, int posY)
{
    if (DisplayScreen != PrevDisplayScreen) {
        tft.fillRoundRect(posX, posY, 35, 35, 5, BUTTONBG);
        tft.drawRoundRect(posX, posY, 35, 35, 5, BUTTONEDGE);
        tft.setCursor(posX + 6, posY + 30);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&Segment22pt7b);
        tft.print("+");
    }
}
void DrawDwnBtnSm(int posX, int posY)
{
    if (DisplayScreen != PrevDisplayScreen) {
        tft.fillRoundRect(posX, posY, 35, 35, 5, BUTTONBG);
        tft.drawRoundRect(posX, posY, 35, 35, 5, BUTTONEDGE);
        tft.setCursor(posX + 9, posY + 34);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&Segment22pt7b);
        tft.print("-");
    }
}

void Clock()
{
    currentTimeMillis = millis();
    if ((M) > (159)) {
        M = 100;
        H += 1;
    }
    if ((H) > (123)) {
        H = 100;
    }
    HH = String(H);
    MM = String(M);
    if ((H) > (109)) {
        DisplayTime = String(HH.substring(2 - 1, 3) + String(":") + MM.substring(2 - 1, 3));
    }
    else {
        DisplayTime = String(HH.substring(3 - 1, 3) + String(":") + MM.substring(2 - 1, 3));
    }
    //    DisplayTime = DD + " "+DisplayTime;
    if (currentTimeMillis - previousTimeMillis > 60000) {
        previousTimeMillis = previousTimeMillis + 60000;
        M += 1;
    }
}

void DrawTime(int posX, int posY)
{
    Clock();
    if ((DisplayTime != PrevDisplayTime) || (DisplayScreen != PrevDisplayScreen)) {
        tft.fillRect(posX, posY, 100, 35, SCREENCOLOR);
        tft.setCursor(posX + 7, posY + 30);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&Segment22pt7b);
        tft.print(DisplayTime);
        PrevDisplayTime = DisplayTime;
    }
}

void DrawSchemaTimeH(int posX, int posY)
{
    String DisplM;
    if ((TimeHighH != PrevTimeHighH) || (TimeHighM != PrevTimeHighM) || (DisplayScreen != PrevDisplayScreen)) {
        DisplM = String(TimeHighM + 100);
        DisplM = String(DisplM.substring(1, 3));
        tft.fillRect(posX, posY, 95, 50, BUTTONBGACT);
        tft.setCursor(posX + 4, posY + 40);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&Segment18pt7b);
        tft.print(TimeHighH);
        tft.print(":");
        tft.print(DisplM);
        //        TimeHighHM = TimeHighH * 100 + TimeHighM;
        PrevTimeHighH = TimeHighH;
        PrevTimeHighM = TimeHighM;
    }
}
void DrawSchemaTimeL(int posX, int posY)
{
    String DisplM;
    if ((TimeLowH != PrevTimeLowH) || (TimeLowM != PrevTimeLowM) || (DisplayScreen != PrevDisplayScreen)) {
        DisplM = String(TimeLowM + 100);
        DisplM = String(DisplM.substring(1, 3));
        tft.fillRect(posX, posY, 95, 50, BUTTONBGACT);
        tft.setCursor(posX + 4, posY + 40);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&Segment18pt7b);
        tft.print(TimeLowH);
        tft.print(":");
        tft.print(DisplM);
        PrevTimeLowH = TimeLowH;
        PrevTimeLowM = TimeLowM;
    }
}

void DrawSchemaTempH(int posX, int posY)
{
    if ((SchemaTempH != PrevSchemaTempH) || (DisplayScreen != PrevDisplayScreen)) {
        tft.fillRect(posX, posY, 65, 50, BUTTONBGACT);
        tft.setCursor(posX + 2, posY + 40);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&Segment18pt7b);
        tft.printf("%.1f", SchemaTempH);
        PrevSchemaTempH = SchemaTempH;
    }
}

void DrawSchemaTempL(int posX, int posY)
{
    if ((SchemaTempL != PrevSchemaTempL) || (DisplayScreen != PrevDisplayScreen)) {
        tft.fillRect(posX, posY, 65, 50, BUTTONBGACT);
        tft.setCursor(posX + 2, posY + 40);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&Segment18pt7b);
        tft.printf("%.1f", SchemaTempL);
        PrevSchemaTempL = SchemaTempL;
    }
}

void DrawHumid(int posX, int posY)
{
    if ((Humid != PrevHumid) || (DisplayScreen != PrevDisplayScreen)) {
        Humid = dht.readHumidity() - 1.4;
        tft.fillRect(posX, posY, 80, 35, SCREENCOLOR);
        tft.setCursor(posX + 6, posY + 22);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&icons14pt7b);
        tft.print(HumidIcon);
        tft.setFont(&Segment10pt7b);
        tft.print(" ");
        tft.print(Humid);
        tft.print("%");
        PrevHumid = Humid;
    }
}

void TempUp()
{
    TempSet = TempSet + 0.5;
}

void TempDown()
{
    TempSet = TempSet - 0.5;
}

void DrawBackLightValue(int posX, int posY)
{
    if ((DimBrightness != PrevDimBrightness) || (DisplayScreen != PrevDisplayScreen)) {
        int DimBrightnessPerc = float(DimBrightness) / 260 * 100;
        tft.setFont(&Segment10pt7b);
        tft.setCursor(posX, posY);
        tft.setTextColor(TEXTCOLOR);
        tft.print("Helderheid gedimd scherm");
        tft.fillRoundRect(posX, posY + 4, 190, 82, 5, BUTTONBG);
        //   tft.drawRoundRect(setTposX, setTposY + 4, 190, 82, 5, BUTTONEDGE);
        tft.setTextColor(TEXTCOLOR);
        tft.setFont(&Segment48pt7b);
        tft.setCursor(posX + 10, posY + 75);
        tft.print(DimBrightnessPerc);
        int curposx = tft.getCursorX();
        int curposy = tft.getCursorY();
        tft.setCursor(curposx + 2, curposy);
        tft.setFont(&Segment22pt7b);
        tft.print("%");
        analogWrite(D1, DimBrightness);
        PrevDimBrightness = DimBrightness;
    }
}

void StoreBackLight()
{
    EEPROM.put(0, DimBrightness);
    EEPROM.commit();
}

void BackLightUp()
{
    if (DimBrightness < 255) {
        DimBrightness += 10;
        DrawBackLightValue(SetTemp2X, SetTemp2Y);
    }
}

void BackLightDwn()
{
    if (DimBrightness > 0) {
        DimBrightness += -10;
        DrawBackLightValue(SetTemp2X, SetTemp2Y);
    }
}

void getTemp()
{
    Temp = dht.readTemperature() - 1.4;
}

void StoreSchema()
{
    EEPROM.put(EEPROMAddrTimeHigh, TimeHighHM);
    EEPROM.put(EEPROMAddrTimeLow, TimeLowHM);
    EEPROM.put(EEPROMAddrTempHigh, SchemaTempHint);
    EEPROM.put(EEPROMAddrTempLow, SchemaTempLint);

    //    TimeHighH = (floor(float(TimeHighHM) / 100));
    //    TimeHighM = TimeHighHM - (TimeHighH * 100);
    EEPROM.commit();
    delay(150);
}

void GetSchemaTemp()
{
    if ((HeatingMode == 1) && (DisplayScreen == 1)) {
        if (DisplayTime != PrevDisplayTime) {
            EEPROMAddrTimeHigh = ((D * 42) + (3 * 16) - 12);
            EEPROMAddrTimeLow = ((D * 42) + (3 * 16) - 8);
            EEPROMAddrTempHigh = ((D * 42) + (3 * 16) - 4);
            EEPROMAddrTempLow = ((D * 42) + (3 * 16));
            EEPROM.get(EEPROMAddrTimeHigh, TimeHighHM);
            EEPROM.get(EEPROMAddrTimeLow, TimeLowHM);
            EEPROM.get(EEPROMAddrTempHigh, SchemaTempHint);
            EEPROM.get(EEPROMAddrTempLow, SchemaTempLint);
        }
    }
}
void GetSchema()
{
    EEPROMAddrTimeHigh = ((SetDay * 42) + (SchemaNumber * 16) - 12);
    EEPROMAddrTimeLow = ((SetDay * 42) + (SchemaNumber * 16) - 8);
    EEPROMAddrTempHigh = ((SetDay * 42) + (SchemaNumber * 16) - 4);
    EEPROMAddrTempLow = ((SetDay * 42) + (SchemaNumber * 16));
    EEPROM.get(EEPROMAddrTimeHigh, TimeHighHM);
    EEPROM.get(EEPROMAddrTimeLow, TimeLowHM);
    EEPROM.get(EEPROMAddrTempHigh, SchemaTempHint);
    EEPROM.get(EEPROMAddrTempLow, SchemaTempLint);
    delay(10);
    if ((TimeHighHM > 2359) || (TimeHighHM < 0)) {
        TimeHighHM = 0;
    }
    if ((TimeLowHM > 2359) || (TimeLowHM < 0)) {
        TimeLowHM = 0;
    }
    TimeHighH = (floor(float(TimeHighHM) / 100));
    TimeHighM = TimeHighHM - (TimeHighH * 100);
    TimeLowH = (floor(float(TimeLowHM) / 100));
    TimeLowM = TimeLowHM - (TimeLowH * 100);
    if ((SchemaTempHint > 240) || (SchemaTempHint < 0)) {
        SchemaTempHint = 0;
    }
    if ((SchemaTempLint > 240) || (SchemaTempLint < 0)) {
        SchemaTempLint = 0;
    }
    SchemaTempH = float(SchemaTempHint) / 10;
    SchemaTempL = float(SchemaTempLint) / 10;
}

void PlayFalppyBird()
{
    int UpDown = 0;
    int GameOver = 0;
    int BirdY = 30;
    int TopY = -50;
    int BottomY = 170;
    int GameSpeed = 50;
    int BoxX = 380;
    int BoxY = 100;
    int BoxSpeed = 7;
    int GapSize = 90;
    int FBScore = 0;
    unsigned long gameMillis = millis();
    unsigned long PrevGameMillis = gameMillis;
    tft.fillScreen(FLAPPYSKY);
    tft.fillRect(0, 210, 320, 30, FLAPPYGRASS);
    tft.fillRect(200, 210, 120, 30, FLAPPYGRASS);
    tft.setCursor(205, 235);
    tft.setFont(&Segment10pt7b);
    tft.setTextColor(WHITE);
    tft.print("Score: ");
    tft.setCursor(265, 235);
    tft.print(FBScore);

    while (GameOver == 0) {
        gameMillis = millis();

        if ((ts.touched()) && (UpDown <= 15)) {
            UpDown += -5;
        }
        if ((UpDown >= -15) && (PrevGameMillis + GameSpeed < gameMillis)) {
            UpDown += 1;
        }
        BirdY += UpDown;

        if ((UpDown >= 5) && (PrevGameMillis + GameSpeed < gameMillis)) {
            tft.fillRect(30, BirdY - 100, 50, 100, FLAPPYSKY);
            tft.drawRGBBitmap(30, BirdY, flappyBird2, 50, 50);
            if (BirdY > 100) {
                tft.fillRect(30, 210, 50, 30, FLAPPYGRASS);
            }
        }
        else if ((UpDown < 5) && (UpDown > -5) && (PrevGameMillis + GameSpeed < gameMillis)) {
            tft.fillRect(30, BirdY - 6, 50, 6, FLAPPYSKY);
            tft.fillRect(30, BirdY + 56, 50, 6, FLAPPYSKY);
            tft.drawRGBBitmap(30, BirdY, flappyBird1, 50, 50);
            if (BirdY > 100) {
                tft.fillRect(30, 210, 50, 30, FLAPPYGRASS);
            }
        }
        else {
            tft.fillRect(30, BirdY + 50, 50, 50, FLAPPYSKY);
            tft.drawRGBBitmap(30, BirdY, flappyBird3, 50, 50);
            if (BirdY > 100) {
                tft.fillRect(30, 210, 50, 30, FLAPPYGRASS);
            }
        }
        if (PrevGameMillis + GameSpeed < gameMillis) {
            //onderbox
            tft.fillRect(BoxX, BoxY, 50, 210 - BoxY, FLAPPYBOX);
            tft.fillRect(BoxX + 50, BoxY, BoxSpeed * 2, 210 - BoxY, FLAPPYSKY);
            //bovenbox
            tft.fillRect(BoxX, 0, 50, BoxY - GapSize, FLAPPYBOX);
            tft.fillRect(BoxX + 50, 0, BoxSpeed * 2, BoxY - GapSize, FLAPPYSKY);
            BoxX = BoxX - BoxSpeed;
            BoxX = BoxX - BoxSpeed;
            if (BoxX < -100) {
                BoxX = 380;
                BoxY = random(80, 160 + 1);
                BoxSpeed += 1;
                FBScore += 5;
                tft.fillRect(260, 210, 60, 30, FLAPPYGRASS);
                tft.setFont(&Segment10pt7b);
                tft.setTextColor(WHITE);
                tft.setCursor(265, 235);
                tft.print(FBScore);
            }
        }
        if ((BirdY > BottomY) || (BirdY < TopY) || (((BirdY + 40 > BoxY) || (BirdY + 10 + GapSize < BoxY)) && (BoxX > 0) && (BoxX < 50))) {
            tft.setCursor(50, 130);
            tft.setFont(&Segment22pt7b);
            tft.setTextColor(WHITE);
            tft.print("Game Over");
            delay(5000);
            previousScreenTimeOutMillis = millis();
            GameOver = 1;
        }

        if (PrevGameMillis + GameSpeed < gameMillis) {
            PrevGameMillis = gameMillis;
        }
    }
}
void MQTT_connect()
{
    int8_t ret;

    // Stop if already connected.
    if (mqtt.connected()) {
        return;
    }

    Serial.print("Connecting to MQTT... ");

    uint8_t retries = 3;
    while ((ret = mqtt.connect()) != 0) { // connect will return 0 for connected
        Serial.println(mqtt.connectErrorString(ret));
        Serial.println("Retrying MQTT connection in 5 seconds...");
        mqtt.disconnect();
        delay(5000); // wait 5 seconds
        retries--;
        if (retries == 0) {
            // basically die and wait for WDT to reset me
            while (1)
                ;
        }
    }
    Serial.println("MQTT Connected!");
}

void loop()
{
    MQTT_connect();
    /*      Adafruit_MQTT_Subscribe *subscription;
  while ((subscription = mqtt.readSubscription(5000))) {
      if (subscription == &ToggleSwitch1Status) {
      Serial.print(F("Got: "));
      Serial.println((char *)ToggleSwitch1Status.lastread);
    }
  }
*/
    RecTime();
    GetTime();
    GetSetTemp();
    ToggleHeating();
    GetWifiStatus();
    ScreenTimeOut();
    if (DisplayScreen != PrevDisplayScreen) {
        tft.fillScreen(SCREENCOLOR);
    }

    unsigned long currentMillis = millis();
    unsigned long previousMillis;
    if (DisplayScreen == 10) {
        if (DisplayScreen != PrevDisplayScreen) {
            DrawMenu();
            PrevDisplayScreen = 10;
        }
    }

    else if (DisplayScreen == 11) {
        if (DisplayScreen != PrevDisplayScreen) {
            DrawMenu2();
            PrevDisplayScreen = 11;
        }
    }

    else if (DisplayScreen == 1) {
        GetWifiStatus();
        DrawWifiStatus(WifiPosX, WifiPosY);
        DrawMenuBtn(MenuBtnX, MenuBtnY);
        DrawCurTemp(CurTempX, CurTempY);
        DrawSetTemp(SetTempX, SetTempY);
        //         DrawUpBtn(UpBtnX,UpBtnY);
        //         DrawDwnBtn(DwnBtnX,DwnBtnY);
        DrawToggle1(Toggle1X, Toggle1Y);
        DrawToggle2(Toggle2X, Toggle2Y);
        DrawTime(TimeDisplX, TimeDisplY);
        DrawHumid(HumidX, HumidY);
        DrawHeatState(HeatStateX, HeatStateY);
        DrawHeatingMode(HeatModeX, HeatModeY);
        PrevDisplayScreen = 1;
    }

    else if (DisplayScreen == 2) {
        GetWifiStatus();
        DrawWifiStatus(WifiPosX, WifiPosY);
        //        DrawMenuBtn(MenuBtnX, MenuBtnY);
        DrawCurTemp(CurTemp2X, CurTemp2Y);
        DrawSetTemp(SetTemp2X, SetTemp2Y);
        DrawUpBtn(UpBtnX, UpBtnY);
        DrawDwnBtn(DwnBtnX, DwnBtnY);
        //         DrawToggle1(Toggle1X,Toggle1Y);
        //         DrawToggle2(Toggle2X,Toggle2Y);
        DrawTime(TimeDisplX, TimeDisplY);
        DrawHumid(HumidX, HumidY);
        DrawHeatState(HeatStateX, HeatStateY);
        PrevDisplayScreen = 2;
    }

    else if (DisplayScreen == 3) {
        if (SchemaNumber != PrevSchemaNumber) {

            tft.setCursor(60, 48);
            tft.setFont(&Segment18pt7b);
            tft.setTextColor(TEXTCOLOR);
            tft.fillRect(50, 10, 220, 50, BUTTONBGACT);
            tft.print(LongDayNames[SetDay]);
            tft.print(" ");
            tft.print(SchemaNumber);
            PrevSchemaNumber = SchemaNumber;
        }
        DrawDwnBtnSm(10, 18);
        DrawUpBtnSm(278, 18);

        DrawSchemaTimeH(45, 70);
        DrawDwnBtnSm(10, 80);
        DrawUpBtnSm(140, 80);

        DrawSchemaTempH(212, 70);
        DrawDwnBtnSm(177, 80);
        DrawUpBtnSm(278, 80);

        DrawSchemaTimeL(45, 140);
        DrawDwnBtnSm(10, 150);
        DrawUpBtnSm(140, 150);

        DrawSchemaTempL(212, 140);
        DrawDwnBtnSm(177, 150);
        DrawUpBtnSm(278, 150);
        DrawSaveBtn(MenuBtnX, MenuBtnY);
        PrevDisplayScreen = 3;
    }

    else if (DisplayScreen == 8) {
        GetWifiStatus();
        DrawWifiStatus(WifiPosX, WifiPosY);
        //        DrawMenuBtn(MenuBtnX, MenuBtnY);
        DrawBackLightValue(SetTemp2X, SetTemp2Y);
        DrawUpBtn(UpBtnX, UpBtnY);
        DrawDwnBtn(DwnBtnX, DwnBtnY);
        //         DrawToggle1(Toggle1X,Toggle1Y);
        //         DrawToggle2(Toggle2X,Toggle2Y);
        DrawTime(TimeDisplX, TimeDisplY);
        DrawHumid(HumidX, HumidY);
        DrawHeatState(HeatStateX, HeatStateY);
        PrevDisplayScreen = 8;
    }
    else if (DisplayScreen == 20) {
        PlayFalppyBird();
        DisplayScreen = 1;
    }

    //Als het scherm wordt aangeraakt
    if (ts.touched()) {
        TS_Point p = ts.getPoint();
        //Vertaal de input van het touchscreen naar coordinaten op het scherm
        p.x = map(p.x, TS_MAXX, TS_MINX, 0, tft.width());
        p.y = map(p.y, TS_MINY, TS_MAXY, 0, tft.height());

        previousScreenTimeOutMillis = millis();
        if (DisplayScreen != 8) {
            digitalWrite(D1, 1);
        }

        delay(150);

        //Knoppen van de Menuopties

        if (DisplayScreen == 10) {
            int xpos = 5;
            int ypos = 10;

            if (((p.x) > (xpos)) && ((p.x) < (xpos + 150)) && ((p.y) > (ypos)) && ((p.y) < (ypos + 50))) {
                DisplayScreen = 1;
                HeatingMode = 1;
            }

            ypos += 65;
            if (((p.x) > (xpos)) && ((p.x) < (xpos + 150)) && ((p.y) > (ypos)) && ((p.y) < (ypos + 50))) {
                DisplayScreen = 1;
                HeatingMode = 2;
            }

            ypos += 65;
            if (((p.x) > (xpos)) && ((p.x) < (xpos + 150)) && ((p.y) > (ypos)) && ((p.y) < (ypos + 50))) {
                DisplayScreen = 1;
                HeatingMode = 3;
            }
            xpos = 165;
            ypos = 10;

            if (((p.x) > (xpos)) && ((p.x) < (xpos + 150)) && ((p.y) > (ypos)) && ((p.y) < (ypos + 50))) {
                DisplayScreen = 3;
                PrevSetDay = 9;
                SetDay = 1;
                PrevSchemaNumber = 4;
                SchemaNumber = 1;
                GetSchema();
            }

            ypos += 65;
            if (((p.x) > (xpos)) && ((p.x) < (xpos + 150)) && ((p.y) > (ypos)) && ((p.y) < (ypos + 50))) {
                DisplayScreen = 1;
            }

            ypos += 65;
            if (((p.x) > (xpos)) && ((p.x) < (xpos + 150)) && ((p.y) > (ypos)) && ((p.y) < (ypos + 50))) {
                DisplayScreen = 1;
            }

            if (((p.x) > (MenuBtnX)) && ((p.x) < (MenuBtnX + 50)) && ((p.y) > (MenuBtnY)) && ((p.y) < (MenuBtnY + 50))) {
                DisplayScreen = 11;
            }
        }
        else if (DisplayScreen == 11) {
            int xpos = 5;
            int ypos = 10;

            if (((p.x) > (xpos)) && ((p.x) < (xpos + 150)) && ((p.y) > (ypos)) && ((p.y) < (ypos + 50))) {
                DisplayScreen = 1;
            }

            ypos += 65;
            if (((p.x) > (xpos)) && ((p.x) < (xpos + 150)) && ((p.y) > (ypos)) && ((p.y) < (ypos + 50))) {
                DisplayScreen = 8;
            }
            xpos = 165;
            ypos = 10;

            ypos += 65;
            if (((p.x) > (xpos)) && ((p.x) < (xpos + 150)) && ((p.y) > (ypos)) && ((p.y) < (ypos + 50))) {
                DisplayScreen = 3;
            }

            if (((p.x) > (xpos)) && ((p.x) < (xpos + 150)) && ((p.y) > (ypos)) && ((p.y) < (ypos + 50))) {
                DisplayScreen = 20;
            }

            if (((p.x) > (MenuBtnX)) && ((p.x) < (MenuBtnX + 50)) && ((p.y) > (MenuBtnY)) && ((p.y) < (MenuBtnY + 50))) {
                DisplayScreen = 10;
            }
        }

        //Startscherm met temperatuur en Toggle knoppen
        else if (DisplayScreen == 1) {

            if (((p.x) > (SetTempX)) && ((p.x) < (SetTempX + 200)) && ((p.y) > (SetTempY)) && ((p.y) < (SetTempY + 100))) {
                DisplayScreen = 2;
            }

            if (((p.x) > (Toggle1X)) && ((p.x) < (Toggle1X + 80)) && ((p.y) > (Toggle1Y)) && ((p.y) < (Toggle1Y + 80))) {
                SetToggle1();
            }
            if (((p.x) > (Toggle2X)) && ((p.x) < (Toggle2X + 80)) && ((p.y) > (Toggle2Y)) && ((p.y) < (Toggle2Y + 80))) {
                SetToggle2();
            }

            if (((p.x) > (MenuBtnX)) && ((p.x) < (MenuBtnX + 50)) && ((p.y) > (MenuBtnY)) && ((p.y) < (MenuBtnY + 50))) {
                DisplayScreen = 10;
            }
        }

        //Knoppen scherm 2
        else if (DisplayScreen == 2) {
            //Temp omhoog knop wordt aangeraakt
            if (((p.x) > (SetTemp2X)) && ((p.x) < (SetTemp2X + 200)) && ((p.y) > (SetTemp2Y + 20)) && ((p.y) < (SetTemp2Y + 100))) {
                DisplayScreen = 1;
            }

            if (((p.x) > (UpBtnX)) && ((p.x) < (UpBtnX + 50)) && ((p.y) > (UpBtnY)) && ((p.y) < (UpBtnY + 50))) {
                TempUp();
                DrawSetTemp(SetTemp2X, SetTemp2Y);
                DrawHeatState(HeatStateX, HeatStateY);
            }

            //Temp omlaag knop wordt aangeraakt
            if (((p.x) > (DwnBtnX)) && ((p.x) < (DwnBtnX + 50)) && ((p.y) > (DwnBtnY)) && ((p.y) < (DwnBtnY + 50))) {
                TempDown();
                DrawSetTemp(SetTemp2X, SetTemp2Y);
                DrawHeatState(HeatStateX, HeatStateY);
            }
        }
        //Schema instelscherm
        else if (DisplayScreen == 3) {
            //DagSchema's + en -
            // Schema vooruit
            if (((p.x) > (260)) && ((p.x) < (310)) && ((p.y) > (10)) && ((p.y) < (60))) {
                // delay(10);
                StoreSchema();

                if (SchemaNumber >= 3) {
                    SetDay += 1;
                    SchemaNumber = 1;
                }
                else {
                    SchemaNumber += 1;
                }
                if (SetDay > 6) {
                    SetDay = 0;
                }
                GetSchema();
            }

            // Schema achteruit
            if (((p.x) > (10)) && ((p.x) < (60)) && ((p.y) > (10)) && ((p.y) < (60))) {
                StoreSchema();
                if (SchemaNumber <= 1) {
                    SetDay += -1;
                    SchemaNumber = 3;
                }
                else {
                    SchemaNumber += -1;
                }
                if (SetDay < 0) {
                    SetDay = 6;
                }
                GetSchema();
            }

            // Tijd1 omhoog
            if (((p.x) > (130)) && ((p.x) < (170)) && ((p.y) > (75)) && ((p.y) < (120))) {
                if (TimeHighM >= 45) {
                    TimeHighH += 1;
                    TimeHighM = 0;
                }
                else {
                    TimeHighM += 15;
                }
                if (TimeHighH > 23) {
                    TimeHighH = 0;
                }
                TimeHighHM = TimeHighH * 100 + TimeHighM;
            }
            // Tijd1 omlaag
            if (((p.x) > (0)) && ((p.x) < (95)) && ((p.y) > (75)) && ((p.y) < (120))) {
                if (TimeHighM <= 0) {
                    TimeHighH += -1;
                    TimeHighM = 45;
                }
                else {
                    TimeHighM += -15;
                }
                if (TimeHighH > 23) {
                    TimeHighH = 0;
                }
                TimeHighHM = TimeHighH * 100 + TimeHighM;
            }

            // Temp1 omhoog
            if (((p.x) > (270)) && ((p.x) < (320)) && ((p.y) > (75)) && ((p.y) < (120))) {
                SchemaTempH += 0.5;
                SchemaTempHint = int(SchemaTempH * 10);
            }
            // Temp1 omlaag
            if (((p.x) > (170)) && ((p.x) < (210)) && ((p.y) > (75)) && ((p.y) < (120))) {
                SchemaTempH += -0.5;
                SchemaTempHint = int(SchemaTempH * 10);
            }
            // Temp2 omhoog
            if (((p.x) > (270)) && ((p.x) < (320)) && ((p.y) > (140)) && ((p.y) < (180))) {
                SchemaTempL += 0.5;
                SchemaTempLint = int(SchemaTempL * 10);
            }
            // Temp2 omlaag
            if (((p.x) > (170)) && ((p.x) < (210)) && ((p.y) > (140)) && ((p.y) < (180))) {
                SchemaTempL += -0.5;
                SchemaTempLint = int(SchemaTempL * 10);
            }

            // Tijd2 omhoog
            if (((p.x) > (130)) && ((p.x) < (170)) && ((p.y) > (140)) && ((p.y) < (180))) {
                if (TimeLowM >= 45) {
                    TimeLowH += 1;
                    TimeLowM = 0;
                }
                else {
                    TimeLowM += 15;
                }
                if (TimeLowH > 23) {
                    TimeLowH = 0;
                }
                TimeLowHM = TimeLowH * 100 + TimeLowM;
            }
            // Tijd2 omlaag
            if (((p.x) > (0)) && ((p.x) < (95)) && ((p.y) > (140)) && ((p.y) < (180))) {
                if (TimeLowM <= 0) {
                    TimeLowH += -1;
                    TimeLowM = 45;
                }
                else {
                    TimeLowM += -15;
                }
                if (TimeLowH > 23) {
                    TimeLowH = 0;
                }
                TimeLowHM = TimeLowH * 100 + TimeLowM;
            }

            if (((p.x) > (MenuBtnX)) && ((p.x) < (MenuBtnX + 50)) && ((p.y) > (MenuBtnY)) && ((p.y) < (MenuBtnY + 50))) {
                DisplayScreen = 10;
            }
        }
        else if (DisplayScreen == 8) {
            //BackLightWaarde wordt aangeraakt
            if (((p.x) > (SetTemp2X + 20)) && ((p.x) < (SetTemp2X + 200)) && ((p.y) > (SetTemp2Y)) && ((p.y) < (SetTemp2Y + 100))) {
                StoreBackLight();
                digitalWrite(D1, 1);
                DisplayScreen = 1;
            }

            if (((p.x) > (UpBtnX)) && ((p.x) < (UpBtnX + 50)) && ((p.y) > (UpBtnY)) && ((p.y) < (UpBtnY + 50))) {
                BackLightUp();
            }

            //Temp omlaag knop wordt aangeraakt
            if (((p.x) > (DwnBtnX)) && ((p.x) < (DwnBtnX + 50)) && ((p.y) > (DwnBtnY)) && ((p.y) < (DwnBtnY + 50))) {
                BackLightDwn();
            }
        }

        else if (DisplayScreen == -1) {
            DisplayScreen = 1;
            PrevDisplayScreen = -1;
        }

        Serial.print("(");
        Serial.print(p.x);
        Serial.print(", ");
        Serial.print(p.y);
        Serial.println(")");
    }
}