#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 5000
// Interval considering "rather long" to work for pump without pressure increase
#define PUMP_UNPRODUCTIVE_TIMEOUT 60000
// amount of pressure what we consider "productive increase" (in 0.01 of bar)
#define PRESSURE_PRODUCTIVE_INCREASE 50
// In fact, pump needs to be able to increase pressure by PRESSURE_PRODUCTIVE_INCREASE
// in PUMP_UNPRODUCTIVE_TIMEOUT time of working


#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_PUMP_STATE 3
#define REGISTER_PUMP_MODE 4
#define REGISTER_P_ON 5
#define REGISTER_P_OFF 6
#define REGISTER_T_ON 7
#define REGISTER_T_OFF 8
#define REGISTER_HEAT_MODE 9
#define REGISTER_HEAT_STATE 10
#define REGISTER_PUMP_ERROR 11

#define DHTPIN D3
#define DHTTYPE AM2301

#define PUMP_PIN D4
#define HEATER_PIN D0

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_pump_mode = MODE_AUTO;
int cur_pump_state = 0;
int cur_p = 0;
int cur_t = 0;
int cur_hum = 0;
int t_on = 200; // 2 degrees
int t_off = 1500; // 15 degrees
int cur_heat_mode = MODE_AUTO;
int cur_heat_state = 0;

bool error_pump = false;
bool error_heat = false;

void setup() 
{
	pinMode(PUMP_PIN, OUTPUT);
	pinMode(HEATER_PIN, 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_PUMP_STATE, cur_pump_state);
	mb.addHreg(REGISTER_PUMP_MODE, cur_pump_mode);
	mb.addHreg(REGISTER_P_ON, p_on);
	mb.addHreg(REGISTER_P_OFF, p_off);
	mb.addHreg(REGISTER_T_ON, t_on);
	mb.addHreg(REGISTER_T_OFF, t_off);
	mb.addHreg(REGISTER_HEAT_STATE, cur_heat_state);
	mb.addHreg(REGISTER_HEAT_MODE, cur_heat_mode);	
	mb.addHreg(REGISTER_PUMP_ERROR, error_pump);
	// 
	Serial.print("Heater pin ");
	Serial.println(HEATER_PIN);
	Serial.print("Pump pin ");
	Serial.println(PUMP_PIN);
}


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 = 1000;
int measures[MEASURES_AMOUNT];
int measure_pos = 0;
// добавка к измеренному значению (поправка на особенности датчика)
float PRESSURE_CALIBRATION = -2;

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
	// also add one atmosphere (1.01325 bar), because sensor reads excess pressure (not absolute)
	// finally multiply by 100 as we have int registers
	measures[measure_pos] = (int)(((p_volt / 2.5f) * 10 + 1.01325) * 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) + PRESSURE_CALIBRATION * 100;
}

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

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);
		if (error_pump)
		{
			lcd.print(" PUMP ERR");
		}
		else
		{
			lcd.print(" WiFi: ");
			lcd.print(WiFi.status());
		}
		//
		if (error_pump)
		{
			lcd.setRGB(255, 0, 0);
		}
		else
		{
			if (cur_pump_state)
			{
				lcd.setRGB(0, 0, 255);
			}
			else
			{
				lcd.setRGB(0, 255, 0);
			}
		}
		last_screen_update = current;
	}
}

unsigned long last_pump_switch = 0;
unsigned long last_pump_on = 0;
int last_pump_p = 0;

void UpdatePump()
{
	unsigned long current = millis();
	int new_state = cur_pump_state;
	if (cur_pump_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_pump_state 
			&& cur_t > t_on) // turning on pump only if temperature is high enough
		{
			Serial.print("CHANGE STATE ");
			Serial.print(cur_p);
			Serial.print(" ");
			Serial.print(p_on);
			Serial.print(" ");
			Serial.print(p_off);
			cur_pump_state = new_state;
			mb.Hreg(REGISTER_PUMP_STATE, cur_pump_state);
			last_pump_switch = current;
			if (new_state == 1)
			{
				last_pump_on = current;
				last_pump_p = cur_p;
			}
		}
	}
	else
	{
		cur_pump_state = mb.Hreg(REGISTER_PUMP_STATE);
	}
	if (error_pump)
	{
		cur_pump_state = 0;
	}
	digitalWrite(PUMP_PIN, cur_pump_state);
	// check for unproductive
	if (cur_pump_state == 1)
	{
		if (current < last_pump_on || current - last_pump_on >= PUMP_UNPRODUCTIVE_TIMEOUT)
		{
			if (cur_p - last_pump_p < PRESSURE_PRODUCTIVE_INCREASE)
			{
				error_pump = true;
				cur_pump_state = 0;
			}
			else
			{
				// the pump is working, will perform new check in PUMP_UNPRODUCTIVE_TIMEOUT
				last_pump_p = cur_p;
				last_pump_on = current;
			}
		}
	}
}

void UpdateHeater()
{
	int new_state = cur_heat_state;
	if (cur_heat_mode == MODE_AUTO)
	{
		if (cur_t < t_on) new_state = 1;
		else if (cur_t > t_off) new_state = 0;
		if (new_state != cur_heat_state)
		{
			cur_heat_state = new_state;
			mb.Hreg(REGISTER_HEAT_STATE, cur_heat_state);
		}
	}
	else
	{
		cur_heat_state = mb.Hreg(REGISTER_HEAT_STATE);
	}
	digitalWrite(HEATER_PIN, cur_heat_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_pump_mode = mb.Hreg(REGISTER_PUMP_MODE);
	t_on = mb.Hreg(REGISTER_T_ON);
	t_off = mb.Hreg(REGISTER_T_OFF);
	cur_heat_mode = mb.Hreg(REGISTER_HEAT_MODE);
	error_pump= mb.Hreg(REGISTER_PUMP_ERROR);
	UpdatePump();
	UpdateHeater();
	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(" pump ");
		Serial.print(cur_pump_state);
		Serial.print(" heat ");
		Serial.println(cur_heat_state);
	}
	mb.Hreg(REGISTER_P_ON, p_on);
	mb.Hreg(REGISTER_P_OFF, p_off);
	mb.Hreg(REGISTER_PUMP_MODE, cur_pump_mode);
	mb.Hreg(REGISTER_T_ON, t_on);
	mb.Hreg(REGISTER_T_OFF, t_off);
	mb.Hreg(REGISTER_HEAT_MODE, cur_heat_mode);
	mb.Hreg(REGISTER_CURRENT_TEMP, cur_t);
	mb.Hreg(REGISTER_CURRENT_HUM, cur_hum);
	mb.Hreg(REGISTER_CURRENT_PRESS, cur_p);
	mb.Hreg(REGISTER_PUMP_ERROR, error_pump);
	UpdateDisplay();
	if (WiFi.status() != WL_CONNECTED)
	{
	}
}