#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include "config.h"

#define READ_INTERVAL 1000
#define UDP_PORT 7000
#define PING_INTERVAL 60000 // one minute seems to be ok for us
#define RELAY_PIN 2

WiFiUDP udp;
char id[64];

unsigned long last_ping;
int relay_status = LOW;
unsigned long last_send = 0;

void setup() 
{
	Serial.begin(115200);
	Serial.println("Initialised...");

	WiFi.begin(WIFI_NAME, WIFI_PASS);
	Serial.print("Connecting");
	while (WiFi.status() != WL_CONNECTED)
	{
		delay(500);
		Serial.print(".");
	}
	Serial.println();

	Serial.print("Connected, IP address: ");
	Serial.println(WiFi.localIP());
	uint8_t macAddr[6];
	WiFi.macAddress(macAddr);
	sprintf(id, "%02x:%02x:%02x:%02x:%02x:%02x", macAddr[0], macAddr[1], macAddr[2], macAddr[3], macAddr[4], macAddr[5]);
	Serial.printf("Connected, mac address: %s\n", id);
	udp.begin(UDP_PORT);

	last_ping = millis();
	pinMode(RELAY_PIN, OUTPUT);
	digitalWrite(RELAY_PIN, relay_status);
}

char incoming_packet[4096];

void ParseIncomingPacket()
{
	int len = udp.read(incoming_packet, sizeof(incoming_packet));
	if (len == 0) return;
	// find command
	int pos = 0;
	while (pos < len && incoming_packet[pos] != 0x01) pos++;
	if (strncmp(incoming_packet, "ping", pos) == 0)
	{
		Serial.println("Ping command received");
		last_ping = millis();
	}
	else if (strncmp(incoming_packet, "r_write", pos) == 0)
	{
		pos++;
		int s = pos;
		while (pos < len && incoming_packet[pos] != 0x01) pos++;
		if (incoming_packet[s] == '0') relay_status = LOW;
		else if (incoming_packet[s] == '1') relay_status = HIGH;
		else Serial.println("Wrong state in incoming packet");
		// to force re-send of current status
		last_send = 0;
	}
	else
	{
		// unknown command
		Serial.println("Unknown command received");
	}
}

char payload[1024];

void SendState()
{
	int len = snprintf(payload, sizeof(payload), "{\"r\":%d,\"id\":\"%s\",\"ip\":\"%s\"}", relay_status, id, WiFi.localIP().toString().c_str());
	Serial.print("Sending payload:\n");
	Serial.println(payload);

	if (!udp.beginPacket(REMOTE_IP, REMOTE_PORT))
	{
		Serial.println("Error with destination ip or port");
		return;
	}
	udp.write(payload, len);
	udp.endPacket();
}


void loop() 
{
	unsigned long current = millis();
	if (current < last_send || current - last_send >= READ_INTERVAL)
	{
		SendState();
		last_send = current;
	}
	int packetSize = udp.parsePacket();
	if (packetSize)
	{
		ParseIncomingPacket();
	}
	digitalWrite(RELAY_PIN, relay_status);
	// renew current, because during processing of packet some time could pass
	// and we will thing that last_ping is in future
	current = millis();
	if (current < last_ping || current - last_ping >= PING_INTERVAL)
	{
		// no pings for long time, reboot
		Serial.println("No pings for long time, reboot");
		Serial.println("Current");
		Serial.println(current);
		Serial.println("Last ping");
		Serial.println(last_ping);
		ESP.restart();
	}
}