#include <Arduino.h>
#include <FreeRTOS_SAMD21.h>
#include <WiFiNINA.h>
#include <WiFiUdp.h>
#include <AccelStepper.h>

#define RED_PIN (9)
#define GREEN_PIN (6)
#define BLUE_PIN (5)
#define WHITE_PIN (3)
#define LASER_LEFT_PIN (A4)
#define LASER_RIGHT_PIN (A5)
#define FOG_LEFT_PIN (A6)
#define FOG_RIGHT_PIN (A7)

void myDelayMs(int ms);
void reconnectThread(void *pvParameters);
void tcpThread(void *pvParametes);
void ledThread(void *pvParametes);

char const * const ssid = "citronet";
//char const * const ssid = "Free Hill-Fi 2.4";
//char const * const ssid = "telenet-D6A9117";
char const * const password = "Ni3lsG33nsRul3z";
//char const * const password = "gewoonzonderhoofdletters";

unsigned int const udpPort = 2390;
IPAddress udpAddress(224, 0, 0, 123);
unsigned int const tcpPort = 2390;
IPAddress tcpAddress;

uint8_t dmxBuffer[512];
uint8_t dmxAddress = 0;

wl_status_t status = WL_IDLE_STATUS;
SemaphoreHandle_t wifiMutex;

TaskHandle_t reconnectThreadHandle;
TaskHandle_t tcpThreadHandle;
TaskHandle_t ledThreadHandle;

void setup() {
	Serial.begin(115200);
	//while(!Serial);
	Serial.println("setup");
	pinMode(LED_BUILTIN, OUTPUT);
	pinMode(RED_PIN, OUTPUT);
	pinMode(GREEN_PIN, OUTPUT);
	pinMode(BLUE_PIN, OUTPUT);
	pinMode(WHITE_PIN, OUTPUT);
	pinMode(LASER_LEFT_PIN, OUTPUT);
	pinMode(LASER_RIGHT_PIN, OUTPUT);
	pinMode(FOG_LEFT_PIN, OUTPUT);
	pinMode(FOG_RIGHT_PIN, OUTPUT);
	wifiMutex = xSemaphoreCreateMutex();
	while( wifiMutex == NULL );
	xSemaphoreGive(wifiMutex);
	xTaskCreate(reconnectThread, "Reconnect Thread", 256, NULL, tskIDLE_PRIORITY + 3, &reconnectThreadHandle);
	xTaskCreate(tcpThread, "Tcp Thread", 256, NULL, tskIDLE_PRIORITY + 1, &tcpThreadHandle);
	xTaskCreate(ledThread, "Led Thread", 256, NULL, tskIDLE_PRIORITY + 4, &ledThreadHandle);
	Serial.println("starting scheduler");
	vTaskStartScheduler();
	Serial.println("scheduler failed");
}

void loop() {
	delay(10);
	//Serial.println("loop");
}

void myDelayMs(int ms) {
	vTaskDelay((ms*1000)/portTICK_PERIOD_US);
}

void reconnectThread(void *pvParameters) {
	for(;;) {
		if(xSemaphoreTake(wifiMutex, portMAX_DELAY)) {
			status = (wl_status_t) WiFi.status();
			Serial.print("wifi status: ");
			Serial.println(status);
			if(WL_CONNECTED != status) {
				Serial.print("connecting to: ");
				Serial.println(ssid);
				status = (wl_status_t) WiFi.begin(ssid, password);
				if(WL_CONNECTED == status) {
					Serial.println("wifi connected");
					WiFiUDP udp;
					udp.beginMulticast(udpAddress, udpPort);
					char ipBuffer[16];
					for(int i = 0; i < 5; i++) {
						Serial.println("waiting for udp packet");
						int packetSize = udp.parsePacket();
						if(packetSize) {
							udp.read(ipBuffer, sizeof(ipBuffer));
							ipBuffer[packetSize] = 0;
							break;
						}
						myDelayMs(1000);
					}
					String ipString = ipBuffer;
					tcpAddress.fromString(ipString);
					if(tcpAddress) {
						Serial.print("ip address: ");
						Serial.println(ipString);
					} else {
						WiFi.end();
					}
				}
			}
            		xSemaphoreGive(wifiMutex);
		}
		myDelayMs(10000);
	}
}

void tcpThread(void *pvParametes) {
	for(;;) {
		WiFiClient client;
		for(;;) {
			while(!tcpAddress) myDelayMs(1000);
			Serial.println("starting tcp connection");
			bool connected = false;
			if(xSemaphoreTake(wifiMutex, portMAX_DELAY)) {
				connected = client.connect(tcpAddress, tcpPort);
				xSemaphoreGive(wifiMutex);
			}
			if(connected) {
				Serial.println("tcp connected");
				break;
			} else {
				Serial.println("could not connect tcp socket");
				myDelayMs(1000);
			}
		}
		client.setTimeout(5000);
		for(;;) {
			if(xSemaphoreTake(wifiMutex, portMAX_DELAY)) {
				unsigned int bufferLen = 0;
				unsigned long readStart = millis();
				while(bufferLen != sizeof(dmxBuffer)) {
					signed int readLen = client.read(dmxBuffer + bufferLen, sizeof(dmxBuffer) - bufferLen);
					if(readLen < 0) break;
					if(millis() - readStart > 1000) break;
					bufferLen += readLen;
				}
				xSemaphoreGive(wifiMutex);
				if(bufferLen < sizeof(dmxBuffer)) break;
			}
		}
		Serial.println("disconnected");
	}
}


void ledThread(void *pvParametes) {
	for(;;) {
		digitalWrite(LED_BUILTIN, dmxBuffer[dmxAddress + 0]);
		analogWrite(RED_PIN, dmxBuffer[dmxAddress + 0]);
		analogWrite(GREEN_PIN, dmxBuffer[dmxAddress + 1]);
		analogWrite(BLUE_PIN, dmxBuffer[dmxAddress + 2]);
		analogWrite(WHITE_PIN, dmxBuffer[dmxAddress + 3]);
		digitalWrite(LASER_LEFT_PIN, dmxBuffer[dmxAddress + 4] > 0);
		digitalWrite(LASER_RIGHT_PIN, dmxBuffer[dmxAddress + 5] > 0);
		digitalWrite(FOG_LEFT_PIN, dmxBuffer[dmxAddress + 6] > 0);
		digitalWrite(FOG_RIGHT_PIN, dmxBuffer[dmxAddress + 7] > 0);
		myDelayMs(10);
	}
}
