   Serial.println();
 
   // Only proceed if incoming message's topic matches
-  if (String(topic) == topicLed) {
-    ledStatus = !ledStatus;
-    digitalWrite(LED_PIN, ledStatus);
-    mqtt.publish(topicLedStatus, ledStatus ? "1" : "0");
-  }
-}
+  if (String(topic) == topicHeater) {
+    HeaterStatus = payload[0] == '0' ? LOW : HIGH; 
+    digitalWrite(HEATER_PIN, HeaterStatus);
+    mqtt.publish(topicHeaterStatus, HeaterStatus ? "1" : "0");
+  }
+}
+
+void senddht22(int pinDHT22, char* topic) {
+  float temperature = 0;
+  float humidity = 0;
+  int err = SimpleDHTErrSuccess;
+  size_t len = strlen(topic);
+
+  if ((err = dht22.read2(pinDHT22, &temperature, &humidity, NULL)) != SimpleDHTErrSuccess) {
+    delay(2000);
+    Serial.println("DHT init failed");
+    return;
+  }
+
+  strcpy(topicbuffer,topic);
+  
+  strcpy(topicbuffer + len, tempprefix);
+  dtostrf(temperature, 0, 1, valbuffer);
+  mqtt.publish(topicbuffer, valbuffer);
+  
+  strcpy(topicbuffer + len, humprefix);
+  dtostrf(humidity, 0, 1, valbuffer);
+  mqtt.publish(topicbuffer, valbuffer);
+}
+  
+void sendbme(char* topic) {
+  float temperature = 0;
+  float humidity = 0;
+  size_t len = strlen(topic);
+
+  if (bme_status) {
+    strcpy(topicbuffer,topic);
+  
+    strcpy(topicbuffer + len, tempprefix);
+    dtostrf(bme.readTemperature(), 0, 1, valbuffer);
+    mqtt.publish(topicbuffer, valbuffer);
+
+    strcpy(topicbuffer + len, humprefix);
+    dtostrf(bme.readHumidity(), 0, 1, valbuffer);
+    mqtt.publish(topicbuffer, valbuffer);
+
+    strcpy(topicbuffer + len, pressprefix);
+    dtostrf(bme.readPressure(), 0, 2, valbuffer);
+    mqtt.publish(topicbuffer, valbuffer);
+  }
+}
+// foo
+
+  //  mqtt.publish(topic, xxx);

 **************************************************************/

// Select your modem:
// #define TINY_GSM_MODEM_SIM800
// #define TINY_GSM_MODEM_SIM900
// #define TINY_GSM_MODEM_A6
// #define TINY_GSM_MODEM_A7
#define TINY_GSM_MODEM_M590
// #define TINY_GSM_MODEM_ESP8266

#include <TinyGsmClient.h>
#include <PubSubClient.h>
#include <OneWire.h>
#include <SimpleDHT.h>

// Your GPRS credentials
// Leave empty, if missing user or pass
const char apn[]  = "";
const char user[] = "";
const char pass[] = "";

// Use Hardware Serial on Mega, Leonardo, Micro
#define SerialAT Serial1

// or Software Serial on Uno, Nano
//#include <SoftwareSerial.h>
//SoftwareSerial SerialAT(2, 3); // RX, TX

TinyGsm modem(SerialAT);
TinyGsmClient client(modem);
PubSubClient mqtt(client);

const char* broker = "test.mosquitto.org";

const char* topicLed = "GsmClientTest/led";
const char* topicInit = "GsmClientTest/init";
const char* topicLedStatus = "GsmClientTest/ledStatus";

#define LED_PIN 13
int ledStatus = LOW;

long lastReconnectAttempt = 0;

void setup() {
  pinMode(LED_PIN, OUTPUT);

  // Set console baud rate
  Serial.begin(115200);
  delay(10);

  // Set GSM module baud rate
  SerialAT.begin(115200);
  delay(3000);

  // Restart takes quite some time

    while (true);
  }
  Serial.println(" OK");
  
  // MQTT Broker setup
  mqtt.setServer(broker, 1883);
  mqtt.setCallback(mqttCallback);

  bme_status = bme.begin();
  Watchdog.reset();
}

boolean mqttConnect() {
  Serial.print("Connecting to ");
  Serial.print(broker);
  if (!mqtt.connect("Karttula", mqtt_user, mqtt_pass)) {
    Serial.println(" fail");
    return false;
  }
  Serial.println(" OK");
  mqtt.publish(topicInit, "Karttula started");
  mqtt.subscribe(topicHeater);
  return mqtt.connected();
}

void loop() {

  if (mqtt.connected()) {
    //  if (1) {
    if (millis() > (lastrun + PERIOD)) {
      switch(state) {
      case 0:
	senddht22(Makuuhuone, topicMakuuhuone);
	state++;
	break;

      case 1:
	sendbme(topicOlohuone);
	state++;
	break;

      case 2:
	senddht22(WC, topicWC);
	state++;
	break;

      case 3:
	// Foo
	state = 0;
	lastrun = millis();
      }
    }
    delay(1000);
    mqtt.loop();
  } else {
    // Reconnect every 10 seconds
