#include "DHT.h"

#define DHTTYPE DHT22

#define DHTPIN_1 9

#define DHTPIN_2 10

#define CO_PIN_GREEN 3
#define CWU_PIN_RED 5
#define D3_PIN_BLUE 8

int MAX_CO_TEMPERATURE = 26;
int MIN_CWU_TEMPERATURE = 42;
int MAX_CWU_TEMPERATURE = 46; // Nagrzany zbiornik

DHT CO_thermometer(DHTPIN_1, DHTTYPE);
DHT CWU_thermometer(DHTPIN_2, DHTTYPE);

/*
Mozliwe stany:
0 - ogrzewanie domu CO (przy starcie)
1 - gdy CWU jest ponizej danej wartosci, ogrzewanie wody CWU, zawór 3D tak, CO nie
2 - CWU osiagnelo odpowiednia temperature, najpierw 3D, poczekac 3min i potem CO
*/
int STATE = 0;
int HEAT_CWU = -1;
int HEAT_CO = -1;

float CO_temperature;
float CWU_temperature;

/* HELPERS */
String* split(String command, String* tokens) {
  int i;
  char delimiter[] = " ";
  char *p;
  char str[128];

  command.toCharArray(str, sizeof(str));
  i = 0;
  p = strtok(str, delimiter);
  while(p && i < 4)
  {
    tokens[i] = p;
    p = strtok(NULL, delimiter);
    ++i;
  }
}

boolean contains(String str1, String str2) {
  if (str1.indexOf(str2) >= 0) {
    return true;
  }

  return false;
}
/* END HELPERS */

void setup() {
  Serial.begin(9600);
  Serial.println("DHT initialized");
  CO_thermometer.begin();
  CWU_thermometer.begin();

  CO_temperature = CO_thermometer.readTemperature();
  CWU_temperature = CWU_thermometer.readTemperature();

  pinMode(CO_PIN_GREEN, OUTPUT);
  pinMode(CWU_PIN_RED, OUTPUT);
  pinMode(D3_PIN_BLUE, OUTPUT);
}

void heatCO() {
  digitalWrite(CO_PIN_GREEN, HIGH);
  if (CO_temperature > MAX_CO_TEMPERATURE || HEAT_CO == 0) {
    digitalWrite(CO_PIN_GREEN, LOW);
    STATE = 1;
  }
}

void heatCWU() {
  digitalWrite(CWU_PIN_RED, HIGH);
  if (CWU_temperature < MIN_CWU_TEMPERATURE) {
    // Zwiekszamy temperature poniewaz jest ponizej minimalnej
    digitalWrite(D3_PIN_BLUE, HIGH);
  }

  if (CWU_temperature == MAX_CWU_TEMPERATURE) {
    // Nagrzany zbiornik
    digitalWrite(CWU_PIN_RED, LOW);
    digitalWrite(D3_PIN_BLUE, LOW);
    STATE = 2;
  }
}

float d3_time = 1000000;

void turn3D() {
  Serial.println("Przelaczamy zawor 3D i czekamy 3 sekundy");
  digitalWrite(D3_PIN_BLUE, HIGH);

  d3_time--;

  if (CWU_temperature != MAX_CWU_TEMPERATURE && d3_time == 0) {
    digitalWrite(D3_PIN_BLUE, LOW);
    STATE = 0;
    d3_time = 1000000;
  }
}

void handleCWUCommand(String* tokens, int numOfCommands) {
  if (contains(tokens[1], "start")) {
    // hotwater start
    if (numOfCommands == 2) {
      Serial.println("Wymuszenie włączenia CWU");
      HEAT_CWU = 1;
    }
    // hotwater start temp
    else if (numOfCommands == 3) {
      Serial.print("Temperatura załączenia CWU: ");
      Serial.println(MIN_CWU_TEMPERATURE);
    }
    // hotwater start temp n
    else if (numOfCommands == 4) {
      Serial.println("Zmiana temperatury złączenia CWU!");
      MIN_CWU_TEMPERATURE = tokens[3].toInt();
    }
  } else if (contains(tokens[1], "stop")) {
    // hotwater stop
    if (numOfCommands == 2) {
      Serial.println("Wymuszanie wyłączenia CWU");
      HEAT_CWU = 0;
    }
    // hotwater stop temp
    else if (numOfCommands == 3) {
      Serial.print("Temperatura wyłączenia CWU: ");
      Serial.println(MAX_CWU_TEMPERATURE);
    }
    // hotwater stop temp n
    else if (numOfCommands == 4) {
      Serial.println("Zmiana temperatury wyłączenia CWU!");
      MAX_CWU_TEMPERATURE = tokens[3].toInt();
    }
  } else if (contains(tokens[1], "temp")) {
    Serial.print("Bieżąca temperatura CWU: ");
    Serial.println(CWU_temperature);
  }
}

void handleCOCommand(String* tokens, int numOfCommands) {
  if (contains(tokens[1], "start")) {
        // hotwater start
    if (numOfCommands == 2) {
      Serial.println("Wymuszenie włączenia CO");
      HEAT_CO = 1;
    }
  } else if (contains(tokens[1], "stop")) {
    if (numOfCommands == 2) {
      Serial.println("Wymuszenie wyłączenia CO");
      HEAT_CO = 0;
    }
  } else if (contains(tokens[1], "temp")) {
    Serial.print("Bieżąca temperatura CO: ");
    Serial.println(CO_temperature);
  }
}

void handleCommand() {
  String command = Serial.readString();
  command.trim();

  if (command) {
    String tokens[4];
    split(command, tokens);

    int numOfCommands = 0;

    for (int i = 0; i < 4; i++) {
      if (tokens[i].length() > 0) {
        numOfCommands += 1;
      }
    }

    if (contains(tokens[0], "hotwater")) {
      Serial.println("KOMENDA hotwater(CWU)");
      handleCWUCommand(tokens, numOfCommands);
    } else if (contains(tokens[0], "heating")) {
      Serial.println("KOMENDA heating(CO)");
      handleCOCommand(tokens, numOfCommands);
    }
  }
}

void loop() {
  Serial.print("CO temperatura: ");
  Serial.println(CO_temperature);
  Serial.print("CWU temperatura: ");
  Serial.println(CWU_temperature);

  handleCommand();

  CO_temperature = CO_thermometer.readTemperature();
  CWU_temperature = CWU_thermometer.readTemperature();

  // Stan poczatkowy, ogrzewamy DOM
  if (STATE == 0) {
    heatCO();
  }
  // Stan w ktorym ogrzewamy wode
  else if (STATE == 1) {
    heatCWU();
  }

  else if (STATE == 2) {
    turn3D();
  }

  if (HEAT_CWU == 1) {
    digitalWrite(CWU_PIN_RED, HIGH);
    digitalWrite(D3_PIN_BLUE, HIGH);
  } else if (HEAT_CWU == 0) {
    digitalWrite(CWU_PIN_RED, LOW);
    digitalWrite(D3_PIN_BLUE, LOW);
  }

  if (HEAT_CO == 1) {
    digitalWrite(CO_PIN_GREEN, HIGH);
  } else if (HEAT_CO == 0) {
    digitalWrite(CO_PIN_GREEN, LOW);
  }

  // delay(1000);
}