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

std::vector<Node*> nodes;
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);
	current_node->distance_from_start = 1;
	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);
	}

	bool verbose = false;
	for (Node* n1 : nodes) {
		if (verbose) 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);
			if (verbose) LOGln("  Checking 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) {
				if (verbose) 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;

				if (verbose) LOGln("      Mapping Corner #%d,%d with #%d,%d", n1->_number, edge, found->_number, e2);
				n1->_corners[edge]->_short_neighbours.push_back(found->_corners[e2]);
				
				if (verbose) 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 {
				if (verbose) LOGln("    No match.");
			}
		}
	}

	bool checked_all = false;
	while (!checked_all) {
		checked_all = true;
		for(Node* n : nodes) {
			if (n->distance_from_start == 0) {
				checked_all = false;
			} else {
				for(int i=0; i<CORNERS_PER_PART; i++) {
					if (n->neighbours[i] != nullptr && (n->neighbours[i]->distance_from_start==0 || n->neighbours[i]->distance_from_start > n->distance_from_start)) {
						n->neighbours[i]->distance_from_start = n->distance_from_start+1;
					}
				}
			}
		}
	}

	for (Node* node : nodes) {
		LOGln("Node #%d:", node->_number);
		LOGln("  Distance from start: %d", node->distance_from_start);
		for(Corner* corner : node->_corners) {
			LOGln("  Corner #%d,%d:", node->_number, corner->number);
			for (auto c: corner->_long_neighbours) {
				LOGln("    Long neighbour:  #%d,%d", c->node->_number, c->number);
			}
			for (auto c: corner->_short_neighbours) {
				LOGln("    Short neighbour: #%d,%d", c->node->_number, c->number);
			}
		}
		for (int i=0; i<CORNERS_PER_PART; i++) {
			if (node->neighbours[i]==nullptr) {
				LOGln("  Neighbour %d: -", i);
			} else {
				LOGln("  Neighbour %d: #%d", i, node->neighbours[i]->_number);
			}
		}
	}

	LOGln("Counts:");
	LOGln("Nodes:   %3d", nodes.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 show_all() {
	for(Node* node : nodes) {
		node->draw();
	}
	FastLED.show();
}

void show_status(uint8_t status, CRGB color=CRGB::Green) {
	for (int i=0; i<status*LEDS_PER_CORNER && i<LED_COUNT; i++) {
		leds[i] = color;
	}
	
	for (int i=status*LEDS_PER_CORNER; i<LED_COUNT; i++) {
		leds[i] = CRGB::Black;
	}
	FastLED.show();
}

void display_layout() {
	for(Corner* corner : corners) {
		corner->set_color(CRGB::Blue);
		for(Corner* c : corner->_short_neighbours) {
			c->set_color(CRGB::Red);
		}
		for(Corner* c : corner->_long_neighbours) {
			c->set_color(CRGB::Green);
		}
		show_all();
		delay(1500);
		for(Corner* c : corners) {
			c->set_color(CRGB::Black);
		}
	}
}

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

	
	setup_fastled();
	show_status(1);

	#ifdef TEST_MODE
		LOGln("TEST_MODE is active!");
	#else
		show_status(2);
		wifi_setup();

		show_status(3);
		setup_layout();

		show_status(4);
		mqtt_setup();
		ArduinoOTA.setHostname(OTA_HOSTNAME);
		ArduinoOTA.onProgress([&](unsigned int progress, unsigned int total){
			uint8_t count = progress * corners.size() / total;
			show_status(count, CRGB::Blue);
		});
		ArduinoOTA.onEnd([](){ show_status(0); });
		ArduinoOTA.begin();

		#ifdef WAIT_FOR_OTA
			show_status(5);
			LOGln("Waiting %d seconds for OTA requests...", WAIT_FOR_OTA);
			unsigned long ota_target_time = millis() + WAIT_FOR_OTA*1000;
			while (millis() < ota_target_time) {
				ArduinoOTA.handle();
				yield();
			}
			LOGln("Done waiting for OTA requests.");
		#endif

		show_status(255);
		delay(250);
		show_status(0);
	#endif

	LOGln("Setup done.");

	#ifdef SHOW_LAYOUT_AT_BOOT
		LOGln("SHOW_LAYOUT_AT_BOOT is active - showing layout...");
		display_layout();
		LOGln("Showing layout is done.");
	#endif

	for(Corner* corner : corners) {
		corner->set_color(CRGB::Black);
	}

	State::publish_current_state();
}

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 = State::get_active_mode();

			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();
				}

				if (random8(128)==0) {
					uint16_t corner = (active_mode == AM_FIRST_CORNER) ? 0 : random16(corners.size());
					CHSV color = rgb2hsv_approximate(corners[corner]->color);
					corners[corner]->blend_to(CHSV(color.h - COLOR_DIFFERENCE + random8(2*COLOR_DIFFERENCE), 255, 255));
				}
			} else if (active_mode == AM_NODES || active_mode == AM_FIRST_NODE) {
				for(Node* node : nodes) {
					node->step();
					node->infect(512);
				}

				if (random8(128)==0) {
					uint16_t corner = (active_mode == AM_FIRST_NODE) ? 0 : random8(nodes.size());
					CHSV color = rgb2hsv_approximate(corners[corner]->color);
					nodes[corner]->blend_to(CHSV(color.h - COLOR_DIFFERENCE + random8(2*COLOR_DIFFERENCE), 255, 255));
				}
			} else if (active_mode == AM_FLASH) {
				for (Node* node : nodes) {
					node->step();
					node->infect(512);
				}

				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);
				}
			} else if (active_mode == AM_RAINBOW) {
				const uint8_t speed = 5; // seconds (approx.) for full color sweep
				const uint8_t diff_per_node = 5;
				uint8_t base_hue = (millis() * 256 / speed) / 1000;
				for(Node* node : nodes) {
					node->set_color(CHSV(base_hue + diff_per_node * node->distance_from_start, 255, 255));
				}
			} else { // This includes AM_ERROR
				for(Node* node : nodes) {
					node->set_color(CRGB::Black);
				}
				nodes[0]->set_color(CRGB::Red);
			}

			last_loop = millis();
		}
	#endif

	show_all();
}