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

#define TCP_PORT 15000

#define SENSOR_UPDATE_INTERVAL 500
#define SCREEN_UPDATE_INTERVAL 200
// Interval between allowed switch-states of the pump
#define PUMP_SWITCH_INTERVAL 1000

#define MODE_AUTO 0
#define MODE_MANUAL 1

#define REGISTER_CURRENT_TEMP 0
#define REGISTER_CURRENT_HUM 1
#define REGISTER_CURRENT_PRESS 2
#define REGISTER_CUR_STATE 3
#define REGISTER_MODE 4
#define REGISTER_P_ON 5
#define REGISTER_P_OFF 6

#define DHTPIN D3
#define DHTTYPE AM2301

ModbusIP mb;
DHT dht(DHTPIN, DHTTYPE);
rgb_lcd lcd;

char id[64];

unsigned long last_update;

int p_on = 150;
int p_off = 250;
int cur_mode = MODE_AUTO;
int cur_state = 0;
int cur_p = 0;
int cur_t = 0;
int cur_hum = 0;

void setup() 
{
	pinMode(D4, OUTPUT);
	pinMode(DHTPIN, INPUT);
	pinMode(A0, INPUT);

	lcd.begin(16, 2);
	lcd.setRGB(0, 255, 0);
	Serial.begin(115200);
	Serial.println("Initialized...");

	WiFi.mode(WIFI_STA);
	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);
	// DHT sensor init
	dht.begin();
	// Modbus init	
	mb.server(15000);
	mb.addHreg(REGISTER_CURRENT_TEMP, cur_t);
	mb.addHreg(REGISTER_CURRENT_HUM, cur_hum);
	mb.addHreg(REGISTER_CURRENT_PRESS, cur_p);
	mb.addHreg(REGISTER_CUR_STATE, cur_state);
	mb.addHreg(REGISTER_MODE, cur_mode);
	mb.addHreg(REGISTER_P_ON, p_on);
	mb.addHreg(REGISTER_P_OFF, p_off);
}


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

///////////////////////////////////////////////////////////////////////////////////////////////////
// Workings with pressure sensor
///////////////////////////////////////////////////////////////////////////////////////////////////

const int MEASURES_AMOUNT = 100;
int measures[MEASURES_AMOUNT];
int measure_pos = 0;

void UpdatePressure()
{
	int p_int = analogRead(A0);
	// we have 10-bit ADC, so get value between 0 and 1 and then multiply by max voltage
	// (it is 3.3)
	float p_volt = (p_int / 1024.0f) * 3.3f;
	// divide by maximal input voltage from sensor (it is 2.5)
	// and then multiply by sensor max value (10 bar) - we will get pressure in bar
	// finally multiply by 100 as we have int registers
	measures[measure_pos] = (int)(((p_volt / 2.5f) * 10) * 100);
	measure_pos++;
	// We will use average of MEASURES_AMOUNT last sensor readings
	// excluding minimal and maximal value (to get rid of peaks)
	if (measure_pos >= MEASURES_AMOUNT) measure_pos = 0;
	int min_pos = 0;
	int max_pos = 0;
	for (int i = 0; i < MEASURES_AMOUNT; i++)
	{
		if (measures[i] > measures[max_pos]) max_pos = i;
		if (measures[i] < measures[min_pos]) min_pos = i;
	}
	int sum = 0;
	for (int i = 0; i < MEASURES_AMOUNT; i++)
	{
		if (i == min_pos || i == max_pos) continue;
		sum += measures[i];
	}
	cur_p = sum / (MEASURES_AMOUNT - 2);
}

///////////////////////////////////////////////////////////////////////////////////////////////////

unsigned long last_screen_update = 0;

void UpdateDisplay()
{
	unsigned long current = millis();
	if (current < last_screen_update || current - last_screen_update >= SCREEN_UPDATE_INTERVAL)
	{
		lcd.setCursor(0, 0);
		lcd.print("P: ");
		lcd.print((cur_p / 100.0), 2);
		lcd.print(" T: ");
		lcd.print((cur_t / 100.0), 1);
		lcd.setCursor(0, 1);
		lcd.print("H: ");
		lcd.print(cur_hum / 100);
		lcd.print(" WiFi: ");
		lcd.print(WiFi.status());
		// print current error
		//
		if (cur_state)
		{
			lcd.setRGB(0, 0, 255);
		}
		else
		{
			lcd.setRGB(0, 255, 0);
		}
		last_screen_update = current;
	}
}

unsigned long last_pump_switch = 0;

void UpdatePump()
{
	unsigned long current = millis();
	int new_state = 0;
	if (cur_mode == MODE_AUTO)
	{
		if (cur_p < p_on) new_state = 1;
		else if (cur_p > p_off) new_state = 0;
		if ((current < last_pump_switch || current - last_pump_switch > PUMP_SWITCH_INTERVAL)
			&& new_state != cur_state)
		{
			cur_state = new_state;
			mb.Hreg(REGISTER_CUR_STATE, cur_state);
			last_pump_switch = current;
		}
	}
	else
	{
		cur_state = mb.Hreg(REGISTER_CUR_STATE);
	}
	digitalWrite(D4, cur_state);
}

void loop() 
{
	unsigned long current = millis();
	int u = 0;
	if (current < last_update || current - last_update >= SENSOR_UPDATE_INTERVAL)
	{
		UpdateDHTData();
		last_update = current;
		u = 1;
	}
	UpdatePressure();
	mb.task();
	p_on = mb.Hreg(REGISTER_P_ON);
	p_off = mb.Hreg(REGISTER_P_OFF);
	cur_mode = mb.Hreg(REGISTER_MODE);
	UpdatePump();
	if (u) 
	{
		Serial.print("Current pressure: ");
		Serial.print(cur_p);
		Serial.print(" t ");
		Serial.print(cur_t);
		Serial.print(" h ");
		Serial.print(cur_hum);
		Serial.print(" state ");
		Serial.println(cur_state);
	}
	mb.Hreg(REGISTER_P_ON, p_on);
	mb.Hreg(REGISTER_P_OFF, p_off);
	mb.Hreg(REGISTER_MODE, cur_mode);
	mb.Hreg(REGISTER_CURRENT_TEMP, cur_t);
	mb.Hreg(REGISTER_CURRENT_HUM, cur_hum);
	mb.Hreg(REGISTER_CURRENT_PRESS, cur_p);
	UpdateDisplay();
	if (WiFi.status() != WL_CONNECTED)
	{
	}
}