#include "Adafruit_MQTT.h"
#include "Adafruit_MQTT_Client.h"
#include <WiFi.h>

const uint8_t red_led_pin = 12;
const uint8_t green_led_pin = 14;
const uint8_t switch_pin = 13;

#define WLAN_SSID		"Wokwi-GUEST"
#define WLAN_PASS		""
#define AIO_SERVER		"io.adafruit.com"
#define AIO_SERVERPORT	1883
#define AIO_USERNAME	"vanzhesa"
#define AIO_KEY			"aio_SIcr55GdWcCrUDVKVncvU01mKHoO"
#define AIO_FEED		AIO_USERNAME "/feeds/switchsensor"

WiFiClient client;
Adafruit_MQTT_Client mqtt(&client, AIO_SERVER, AIO_SERVERPORT, AIO_USERNAME, AIO_KEY);
Adafruit_MQTT_Publish switchsensor = Adafruit_MQTT_Publish(&mqtt, AIO_FEED);
Adafruit_MQTT_Subscribe out = Adafruit_MQTT_Subscribe(&mqtt, AIO_FEED);
Adafruit_MQTT_Subscribe *subscription;

void setup() {
	Serial. begin(115200);
	delay(10);

	pinMode(switch_pin, INPUT);
	pinMode(red_led_pin, OUTPUT);
	pinMode(green_led_pin, OUTPUT);

	Serial.println("MQTT VIRTUAL MULTITASKING");

WiFi.begin(WLAN_SSID, WLAN_PASS);
	while (WiFi.status() != WL_CONNECTED) { 
		delay(500);
		Serial.print(".");
	}
	Serial.println();
	mqtt.subscribe(&out);

	xTaskCreate(
		task1,
		"MQTT Publish",
		4096,
		NULL,
		1,
		NULL
	);

	xTaskCreate(
		task2,
		"MQTT Subscribe",
		4096,
		NULL,
		1,
		NULL
	);
}

void loop() {
  	for (;;) {
			vTaskDelay(20);
			delay(10);
		}
}

void task1(void *pvParameters) {
	for (;;){
		uint8_t i = digitalRead(switch_pin);
		if (!(MQTT_connect() && switchsensor.publish(i)))
			Serial.println("ГРЕШКА.");
		delay(1000);	
	}

	vTaskDelete(NULL);
}

void task2(void *pvParameters) {
	for (;;) {
		while (subscription = mqtt.readSubscription(500)) {
			if (subscription == &out) {
				uint8_t out_state = atoi((char*) out.lastread);
				Serial.print("СУИТЧ ВРЪЩА: ");
				Serial.println(out_state);

				if (out_state == 1) {
					digitalWrite(green_led_pin, HIGH);
					digitalWrite(red_led_pin, LOW);
				} else if (out_state == 0) {
					digitalWrite(green_led_pin, LOW);
					digitalWrite(red_led_pin, HIGH);
				}
			}
		}
	}

	vTaskDelete(NULL);
}
boolean MQTT_connect() {
	if (mqtt.connected())
		return true;

	uint8_t retries = 3;
	while (mqtt.connect()) {
		mqtt.disconnect();
		delay(2000);

		if (--retries == 0)
			return false;
	}
	return true;
}