/*
 * An unnamed project – esp8266 module
 *
 * Copyright (C) 2017 Gediminas Jakutis
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; version 2.1
 * of the License.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301, USA
 */

#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include <Wire.h>
#include <stddef.h>
#include <string.h>

static const unsigned int iled = 2;
static const unsigned int analog = A0;
static const int port = 2191;
static const int mpu_addr = 0x68;
static char udppacketbuffer[32] = {0};
static char *udppacketcursor = NULL;
IPAddress ip;
WiFiUDP Udp;

static void sleep(void);
static void udp_init_packet(IPAddress ip, const int port);
static void udp_push(const void * const data, const size_t size);
static int udp_flush(void);
static void mpu_wakeup(const int i2caddr);
static void wifi_connect(const char * const ssid, const char * const password, const char doblink, const int ledpin);
static void blink_led(const int pin, const int ontime, const int offtime);
static void discover_client(void);
static void discard(int a);

void setup(void)
{
	static const char * const ssid = "SSID";
	static const char * const password = "password";

	pinMode(iled, OUTPUT);
	wifi_connect(ssid, password, 1, iled);
	Udp.begin(port);
	Wire.begin();
	mpu_wakeup(mpu_addr);
	discover_client();
}

void loop(void)
{
	static unsigned int ticker = 0;
	static unsigned char firstrun = 0;
	short int data;
	char *dataptr;
	size_t i;

	Wire.beginTransmission(mpu_addr);
	Wire.write(0x3b);
	Wire.endTransmission(0);
	Wire.requestFrom(mpu_addr, 14, 1);

	udp_init_packet(ip, port);

	dataptr = (char*) &data;
	data = 0;
	udp_push(&data, sizeof(data));
	udp_push(&ticker, sizeof(ticker));

	for (i = 0; i < 7; ++i) {
		if (i == 3) {
			dataptr[1] = Wire.read();
			dataptr[0] = Wire.read();
			udp_push(&data, sizeof(data));
		} else {
			discard(Wire.read());
		}
	}
	data = analogRead(analog);
	udp_push(&data, sizeof(data));

	if (!(udp_flush())) {
		Udp.stop();
		sleep();
	}

	blink_led(iled, 20, 80);
	++ticker;
}

static void sleep(void)
{
	do {
	blink_led(iled, 3000, 3000);
	} while (1);
}

static void udp_init_packet(IPAddress ip, const int port)
{
	Udp.beginPacket(ip, port);
	memset(udppacketbuffer, 0, sizeof(udppacketbuffer));
	udppacketcursor = udppacketbuffer;
}

static void udp_push(const void * const data, const size_t size)
{
	memcpy(udppacketcursor, data, size);
	udppacketcursor += size;
}

static int udp_flush(void)
{
	Udp.write((const uint8_t *) udppacketbuffer, udppacketcursor - udppacketbuffer);
	return Udp.endPacket();
}

static void mpu_wakeup(const int i2caddr)
{
	Wire.beginTransmission(i2caddr);
	Wire.write(0x6b);
	Wire.write(0);
	Wire.endTransmission(1);
}

static void wifi_connect(const char * const ssid, const char * const password, const char doblink, const int ledpin)
{
	WiFi.begin(ssid, password);

	do {
		if (doblink) {
			blink_led(ledpin, 250, 250);
		} else {
			delay (500);
		}
	} while (WiFi.status() != WL_CONNECTED);
}

static void blink_led(const int pin, const int ontime, const int offtime)
{
			digitalWrite(pin, HIGH);
			delay(ontime);
			digitalWrite(pin, LOW);
			delay(offtime);
}

static void discover_client(void)
{
	IPAddress bcastip(255, 255, 255, 255);
	static const char servermagic[] = "I love coffee!";
	static const char clientmagic[] = "I love tea!";
	char buffer[32] = {0};
	size_t done = 0;

	do {
		udp_init_packet(bcastip, port);
		udp_push(servermagic, sizeof(servermagic));
		udp_flush();
		delay(5);
		while (Udp.parsePacket()) {
			if (Udp.available() >= sizeof(clientmagic)) {
				Udp.read(buffer, sizeof(clientmagic));
				if (!(strcmp(clientmagic, buffer))) {
					ip = Udp.remoteIP();
					++done;
				}
			}
		}
		delay(95);
	} while (!done);
}

static void discard(int a)
{
	return;
}