#include "Adafruit_SSD1306/Adafruit_SSD1306.h"
#include "MQTT/MQTT.h"
#include "OneWire/OneWire.h"
SYSTEM_THREAD(ENABLED);
SYSTEM_MODE(MANUAL);
STARTUP(WiFi.selectAntenna(ANT_EXTERNAL));
STARTUP(System.enableFeature(FEATURE_RETAINED_MEMORY));
struct counter_struct {
  float value;
  byte state;
  int pin;
};
struct valve_struct {
  byte state;
  int pin;
};
struct sensor_struct {
  int timeout;
  byte state;
  int pin;
};
unsigned long currentMillis = 0;
unsigned long previous_conected = 100000; 
unsigned long previous_wifi_uptime = 100000; 
unsigned long previous_counter_read = 0; 
unsigned long wifi_uptime;
unsigned long start_temp_timer = 0;
unsigned long read_temp_timer = 0;
byte display_timeout = 0;
OneWire ds0 = OneWire(D2);
OneWire ds1 = OneWire(D3);
byte addr0[8];
byte addr1[8];
bool presense0 = false;
bool presense1 = false;
byte data[12];
#define OLED_RESET A7
Adafruit_SSD1306 display(OLED_RESET);
retained valve_struct valve[2] = { {0, D4}, {0, D5} };
retained counter_struct counter[2] = { {0, 1, A0}, {0, 1, A1} };
volatile int pressure[2] = {A2, A3};
#define SENSOR_TIMEOUT 10
volatile sensor_struct sensor[2] = { {0, 1, D6}, {0, 1, D7} };
void callback(char* topic, byte* payload, unsigned int length);
byte server[] = { 192,168,2,101};
MQTT client(server, 1883, callback);
bool publish_message(const char* t, const char* p, bool retain) 
{
    return client.publish(t, (uint8_t*)p, sizeof(p), retain);
}
bool publish_message(const char* t, int p, bool retain) 
{   
    char buf_d[12];
    int n = sprintf(buf_d,"%d",p);
    return client.publish(t, (uint8_t*)buf_d, n, retain);
}
bool publish_message(const char* t, float p, bool retain) 
{   
    
    String s(p, 4);
    
    return client.publish(t, (uint8_t*)s.c_str(), s.length(), retain);
}
void callback(char* topic, byte* payload, unsigned int length) {
    char p[length + 1];
    memcpy(p, payload, length);
    p[length] = NULL;
    String message(p);
    String t(topic);
    if (t.equals("home/water_count/spark/set"))
    {
        if (message.equalsIgnoreCase("1"))
        {
            Particle.connect();
            if (waitFor(Particle.connected, 10000)) 
                {publish_message("home/water_count/spark", 1, false);}
            else
                {Particle.disconnect(); publish_message("home/water_count/spark", 0, false);}
        }
        else
        {
            Particle.disconnect();
            publish_message("home/water_count/spark", 0, false);
        }
    }
    else if (t.startsWith("home/water_count/valve/"))
    {
        int m = message.toInt();
        int x = t.substring(23,24).toInt();
        if (m > -1 && m < 2 && x > -1 && x <2) 
        {
            set_valve(x, m);
        }
        else
        {
            publish_message("home/water_count/valve/" + t.substring(23,24),  valve[x].state , true);
        }
    }
    else if (t.startsWith("home/water_count/counter/"))
    {
        float m = message.toFloat();
        int x = t.substring(25,26).toInt();
        if (m > -1 && m <= 999999 && x > -1 && x <2) 
        {
            counter[x].value = m;
        }
        publish_message("home/water_count/counter/" + t.substring(25,26),  counter[x].value , true);
    }
}
void setup() {
    WiFi.on();
    WiFi.connect();
    if (waitFor(WiFi.ready, 5000)) {mqtt_connect();}
    for (int i=0; i < 2; i++) 
    {
        pinMode(valve[i].pin, OUTPUT);
        digitalWrite(valve[i].pin, valve[i].state);
        pinMode(counter[i].pin, INPUT);
        pinMode(sensor[i].pin, INPUT);
        counter[i].state = digitalRead(counter[i].pin);
        pinMode(pressure[i], AN_INPUT);
    }
    pinMode(A4, INPUT_PULLUP);
    display.begin(SSD1306_SWITCHCAPVCC, 0x3C);  
    display.clearDisplay();   
    
}
void loop() 
{
    currentMillis = millis();
    
    if (currentMillis - previous_conected >= 30000 || previous_conected > currentMillis)
    {
        previous_conected = currentMillis;
        if (!client.isConnected() & wifi_uptime > 60)
        {
            mqtt_connect();
        }
        publish_message("home/water_count/rssi", WiFi.RSSI(), true);
    }
    if (currentMillis - previous_wifi_uptime >= 1000 || previous_wifi_uptime > currentMillis)
    {
        previous_wifi_uptime = currentMillis;
        WiFi.ready() ? wifi_uptime++ : wifi_uptime = 0;
        
        int fg = digitalRead(A4);
        if (display_timeout > 0)
        {
            display_timeout -= 1;
            if (display_timeout == 0) 
            { 
                display.clearDisplay();
                display.display();
            } 
        }
        if (fg == 0)
        {
            if (display_timeout == 0)
            {
                display.clearDisplay();   
                display.setTextSize(2);
                display.setTextColor(WHITE);
                display.setCursor(0,0);
                display.print("C=");
                display.println(counter[0].value, 4);
                display.setCursor(0,16);
                display.print("H=");
                display.println(counter[1].value, 4);
                display.setCursor(0,32);
                display.print("Valve=");
                display.print(valve[0].state);
                display.print("|");
                display.println(valve[1].state);
                display.setCursor(0,48);
                display.print("Sensor=");
                display.print(sensor[0].state);
                display.print("|");
                display.println(sensor[1].state);
                display.display();
            }
            display_timeout = 10;
        }
    }
    
    if (currentMillis - previous_counter_read >= 500 || previous_counter_read > currentMillis)
    {
        previous_counter_read = currentMillis;
        for (int i=0; i < 2; i++) 
        {
            byte count_state = digitalRead(counter[i].pin);
            if (count_state != counter[i].state)
            {
                counter[i].state = count_state;
                if (count_state == 0)
                {
                    counter[i].value += 0.01;
                    char buf18[30];
                    sprintf(buf18,"home/water_count/counter/%d", i);
                    publish_message(buf18 , counter[i].value, true);
                }
            }
            
            byte sensor_state = digitalRead(sensor[i].pin);
            if (sensor_state != sensor[i].state) 
            {
                sensor[i].state = sensor_state;
                sensor[i].timeout = SENSOR_TIMEOUT; 
            }
            if (sensor[i].timeout > 0) 
            {
                sensor[i].timeout -= 1;
                if (sensor[i].timeout == 0)
                {
                    char buf18[30];
                    sprintf(buf18,"home/water_count/sensor/%d", i);
                    publish_message(buf18 , sensor[i].state, true);
                    if (sensor[i].state  == 0)
                    {
                        set_valve(0, 1); 
                        set_valve(1, 1); 
                    }
                }
            }
        }
    }
    
    if (currentMillis - start_temp_timer >= 299000 || start_temp_timer > currentMillis)
    { 
        start_temp_timer = currentMillis;
        presense0 = start_temp0();
        presense1 = start_temp1();
    }
    if (currentMillis - read_temp_timer >= 300000 || read_temp_timer > currentMillis)
    {
        read_temp_timer = currentMillis;
        start_temp_timer = currentMillis;
        if (presense0) read_temp0();
        if (presense1) read_temp1();
        
        char buf18[30]; 
        for (int i=0; i < 2; i++)
        {
            sprintf(buf18,"home/water_count/pressure/%d", i);
            float read_val = analogRead(pressure[i]);
            float value = (read_val - 600.0) / 300.0 ;
            publish_message(buf18 , value, false);
        }
    }
    
    client.loop();
}
void mqtt_connect()
{
    if (client.connect("water_count"))
    { 
        client.subscribe("home/water_count/spark/set");
        publish_message("home/water_count/spark", Particle.connected() ? 1 : 0, true);
        client.subscribe("home/water_count/valve/+/set");
        client.subscribe("home/water_count/counter/+/set");
        
    }
}
bool start_temp0()
{
    
    if ( !ds0.search(addr0)) { ds0.reset_search(); return false;}
    ds0.reset_search();
    if (OneWire::crc8(addr0, 7) != addr0[7]) { return false;}
    
    ds0.reset();
    ds0.select(addr0);
    ds0.write(0x44, 0);
    return true; 
}
bool start_temp1()
{
    
    if ( !ds1.search(addr1)) { ds1.reset_search(); return false;}
    ds1.reset_search();
    if (OneWire::crc8(addr1, 7) != addr1[7]) { return false;}
    
    ds1.reset();
    ds1.select(addr1);
    ds1.write(0x44, 0);
    return true; 
}
bool read_temp0()
{
    
    ds0.reset();
    ds0.select(addr0);
    ds0.write(0xBE, 0);
    for (int i = 0; i < 9; i++) 
    {
        data[i] = ds0.read();
    }
    int16_t raw = (data[1] << 8) | data[0];
    float celsius = (float)raw * 0.0625;
    if (celsius < 0 || celsius > 100) return false;
    publish_message("home/water_count/temp/0", celsius, false);
    
    ds0.reset_search();
    return true;
}
bool read_temp1()
{
    
    ds1.reset();
    ds1.select(addr1);
    ds1.write(0xBE, 0);
    for (int i = 0; i < 9; i++) 
    {
        data[i] = ds1.read();
    }
    int16_t raw = (data[1] << 8) | data[0];
    float celsius = (float)raw * 0.0625;
    if (celsius < 0 || celsius > 100) return false;
    publish_message("home/water_count/temp/1", celsius, false);
    
    ds1.reset_search();
    return true;
}
void set_valve(int vlv, byte state)
{
    valve[vlv].state = state;
    digitalWrite(valve[vlv].pin, state);
    char buf26[26];
    sprintf(buf26,"home/water_count/valve/%d", vlv);
    publish_message(buf26 , state , true);
}