#include <Arduino.h>
#include <ESP8266WiFi.h>
#include <ArduinoOTA.h>
#include "my_fastled.h"
#include "config.h"
#include "tools.h"
#include "node.h"
#include "edge.h"
#include "corner.h"
#include "prototypes.h"
#include "mqtt.h"

std::vector<Node*> nodes;
std::list<Edge*> edges;
std::vector<Corner*> corners;

CRGB leds[LED_COUNT];
CRGB color = CRGB::Pink;

AnimationMode mode = AM_CORNERS;
AnimationMode temp_mode;
unsigned long temp_mode_until;
int return_to_brightness = -1;
uint8_t speedup = SPEEDUP;

unsigned long last_loop = 0;

#ifdef TEST_MODE
	uint8_t base_hue = 0;
#endif

bool looping;

void setup_layout() {
	LOGln("Setting up layout...");
	uint8_t layout[] = LAYOUT;
	Node* current_node = new Node(0, {0, 0}, 0);
	nodes.push_back(current_node);

	for(uint16_t i=0; i<NODE_COUNT-1; i++) {
		current_node = current_node->create_neighbour(layout[i]);
		nodes.push_back(current_node);
	}

	for(Node* node: nodes) {
		for(Edge* edge: node->edges) {
			auto e = std::find(edges.begin(), edges.end(), edge);
			if (e == edges.end()) {
				edges.push_back(edge);
			}
		}
		LOGln("Node %p:", node);
		for(Node* n : node->neighbours) {
			LOGln("  %p", n);
		}
	}

	for (Node* n1 : nodes) {
		LOGln("Looking for neighbours of node #%d @ %d,%d", n1->_number, n1->coords.x, n1->coords.y);
		for(int edge=0; edge<CORNERS_PER_PART; edge++) {
			Coords c = n1->coords_at_direction(edge);
			LOGln("  Chcking edge %d @ %d,%d...", edge, c.x, c.y);

			Node* found = nullptr;
			for(Node* n2 : nodes) {
				if (n2 == n1) {
					continue;
				}
				if (n2->coords.x == c.x && n2->coords.y == c.y) {
					found = n2;
					break;
				}
			}
			if (found != nullptr) {
				LOGln("    Found node #%d", found->_number);
				uint8_t inverse_dir = (n1->direction + 2*edge + 3) % 6;
				int8_t e = (inverse_dir - found->direction) % 6;
				if (e < 0) e+=6;
				e = e / 2;
				//LOGln("      inverse_dir: %d, edge: %d", inverse_dir, edge);
				int8_t e1 = (edge - 1) % CORNERS_PER_PART;
				if (e1<0) e1+=CORNERS_PER_PART;
				int8_t e2 = (e - 1) % CORNERS_PER_PART;
				if (e2<0) e2+=CORNERS_PER_PART;

				LOGln("      Mapping Corner #%d,%d with #%d,%d", n1->_number, edge, found->_number, e2);
				n1->_corners[edge]->_short_neighbours.push_back(found->_corners[e2]);
				
				LOGln("      Mapping Corner #%d,%d with #%d,%d", n1->_number, e1, found->_number, e);
				n1->_corners[e1]->_short_neighbours.push_back(found->_corners[e]);
			} else {
				LOGln("    No match.");
			}
		}
	}

	for(Corner* corner: corners) {
		LOGln("Corner %p:", corner);
		for(auto c: corner->_long_neighbours) {
			LOGln("  Long:  %p", c);
		}
		for(auto c: corner->_short_neighbours) {
			LOGln("  Short: %p", c);
		}
	}

	LOGln("Counts:");
	LOGln("Nodes:   %3d", nodes.size());
	LOGln("Edges:   %3d", edges.size());
	LOGln("Corners: %3d", corners.size());
}

void setup_fastled() {
	LOGln("Setting up FastLED...");
	// GPIO5 = D1
	// GPIO2 = D4
	FastLED.addLeds<WS2812B, 5, GRB>(leds, LEDS_PER_CORNER * CORNERS_PER_PART * NODE_COUNT).setCorrection(TypicalLEDStrip);
	LOGln("LEDs:    %3d", LED_COUNT);
	FastLED.setBrightness(255);
	//FastLED.setDither(DISABLE_DITHER);
	FastLED.setMaxPowerInVoltsAndMilliamps(5, MAX_MILLIAMPS);
	set_all_leds(CRGB::Black);
}

void setup() {
	Serial.begin(74880);
	LOGln("ESPleaf starting.");

	setup_layout();
	setup_fastled();
	#ifdef TEST_MODE
		LOGln("TEST_MODE is active!");
	#else
		wifi_setup();
		mqtt_setup();
		ArduinoOTA.setHostname(OTA_HOSTNAME);
		ArduinoOTA.begin();
	#endif
	
}

void loop() {
	#ifdef TEST_MODE
		EVERY_N_MILLISECONDS(20) {
			int i=0;
			for(Node* node : nodes) {
				CHSV color(base_hue + 150*i, 255, 255);
				for(int j=0; j<CORNERS_PER_PART*LEDS_PER_CORNER; j++) {
					leds[i*CORNERS_PER_PART*LEDS_PER_CORNER+j] = color;
				}
				i++;
			}
			base_hue += 1;
		}
	#else
		// Normal mode
		mqtt_loop();
		ArduinoOTA.handle();

		if (speedup > 0 && (millis() - last_loop > (20 / speedup) || last_loop > millis())) {
			looping = false;

			AnimationMode active_mode = mode;
			if (temp_mode_until > 0) {
				if (temp_mode_until>millis()) {
					active_mode = temp_mode;
				} else {
					temp_mode_until = 0;
					if (return_to_brightness != -1) {
						FastLED.setBrightness(return_to_brightness);
						return_to_brightness = -1;
					}
				}
			}

			if (active_mode == AM_CORNERS || active_mode == AM_FIRST_CORNER) {
				for(Corner* corner: corners) {
					corner->step();
					if (active_mode == AM_FIRST_CORNER) {
						corner->infect(512, 512);
					} else {
						corner->infect(300, 600);
					}
					looping |= !corner->is_finished();
					corner->draw();
				}

				if (random8(128)==0) {
					if (active_mode == AM_FIRST_CORNER) {
						corners[0]->blend_to(CHSV(random8(), 255, 255));
					} else {
						corners[random16(corners.size())]->blend_to(CHSV(random8(), 255, 255));
					}
				}
			} else if (active_mode == AM_NODES || active_mode == AM_FIRST_NODE) {
				for(Node* node : nodes) {
					node->step();
					node->infect(512);
					node->draw();
				}

				if (random8(128)==0) {
					if (active_mode == AM_FIRST_NODE) {
						nodes[0]->blend_to(CHSV(random8(), 255, 255));
					} else {
						nodes[random8(nodes.size())]->blend_to(CHSV(random8(), 255, 255));
					}
				}
			} else if (active_mode == AM_FLASH) {
				for (Node* node : nodes) {
					node->step();
					node->infect(512);
					node->draw();
				}

				if (millis() / 1000 > last_loop / 1000) {
					nodes[0]->blend_to(((millis() / 1000) % 2 == 0) ? CRGB::Black : color, 0, 64);
				}

			} else if (active_mode == AM_OFF || active_mode == AM_STATIC) {
				for(Node* node : nodes) {
					node->set_color(active_mode == AM_OFF ? CRGB::Black : color);
					node->draw();
				}
			} else { // This includes AM_ERROR
				for(Node* node : nodes) {
					node->set_color(CRGB::Black);
				}
				nodes[0]->set_color(CRGB::Red);
				for(Node* node : nodes) {
					node->draw();
				}
			}

			last_loop = millis();
		}
	#endif

	FastLED.show();
}