#include "DHT.h"
#include <ESP8266WiFi.h>
#include <ModbusIP_ESP8266.h>
#include "config.h"

// After how much time without connection we will 
// reboot (in millis)
#define NON_CONNECTED_REBOOT_TIMEOUT 60000 
// Will log current state with this interval
#define LOG_INTERVAL 1000

#define DHTPIN D5
#define DHTTYPE DHT22
#define READ_INTERVAL 100
#define TCP_PORT 15000

IPAddress my_ip(MY_IP);
IPAddress gateway(MY_GW);
IPAddress subnet(MY_NET);

ModbusIP mb;
DHT dht(DHTPIN, DHTTYPE);
char id[64];

#define REGISTER_CURRENT_TEMP 0
#define REGISTER_CURRENT_HUM 1

int cur_t = 0;
int cur_hum = 0;


unsigned long last_poll = 0;
unsigned long current = 0;

Modbus::ResultCode OnModbusRequest(Modbus::FunctionCode fc, const Modbus::RequestData data) 
{
  last_poll = current;
  return Modbus::EX_SUCCESS;
}

void setup() 
{
	Serial.begin(115200);
	Serial.println("Initialised...");
	pinMode(DHTPIN, INPUT);

	dht.begin();

	WiFi.mode(WIFI_STA);
	WiFi.config(my_ip, gateway, subnet);
	WiFi.setAutoReconnect(true);
	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);

	// Modbus init	
  mb.onRequest(OnModbusRequest);
	mb.server(15000);
	mb.addHreg(REGISTER_CURRENT_TEMP, cur_t);
	mb.addHreg(REGISTER_CURRENT_HUM, cur_hum);

  last_poll = millis();
}

void UpdateDHTData()
{
	float h = dht.readHumidity();
	float t = dht.readTemperature();
	cur_hum = (int)(h * 100);
	cur_t = (int)(t * 100);
}

unsigned long last_read = 0;
unsigned long last_connected = 0;
unsigned long last_log = 0;

void loop() 
{
	current = millis();
	if (current < last_read || current - last_read >= READ_INTERVAL)
	{
    UpdateDHTData();
    last_read = current;
	}
	mb.task();
	mb.Hreg(REGISTER_CURRENT_TEMP, cur_t);
	mb.Hreg(REGISTER_CURRENT_HUM, cur_hum);
	if (WiFi.status() != WL_CONNECTED)
	{
		WiFi.reconnect();
	}
	else
	{
		last_connected = current;
	}
	if (current < last_connected || current - last_connected > NON_CONNECTED_REBOOT_TIMEOUT)
	{
    Serial.println("Detected wifi disconnection, will reboot");
		ESP.restart();
	}
  if (current < last_poll || current - last_poll > NON_CONNECTED_REBOOT_TIMEOUT)
	{
  Serial.println(current);
  Serial.println(last_poll);
    Serial.println("Detected absense of polls, consider it as connection failure");
		ESP.restart();
	}
  if (current < last_log || current - last_log > LOG_INTERVAL)
  {
    last_log = current;
    Serial.print("t: ");
    Serial.print(cur_t);
    Serial.print(" h: ");
    Serial.print(cur_hum);
    Serial.print(" WiFi: ");
    Serial.print(WiFi.status());
    Serial.println();
  }
}