   cmdMessenger.sendCmd(kERR,"Unknown command");
 }
 
+int measureTemperature() {
+  int index = analogRead(A0) - adcSubstract;
+  if (index < 0)
+  {
+    index = 0;
+  }
+  if (index>adcMax)
+  {
+    index = adcMax;
+  }
+  int temperature = pgm_read_word(&temperatureTable[index]);
+  return temperature;
+}
+
+unsigned long windowStartTime;
+boolean inDutyCycleMode;
+boolean heaterIsOn;
+
+void heaterOn() {
+  digitalWrite(heaterPin, HIGH);
+  heaterIsOn = HIGH;
+}
+
+void heaterOff() {
+  digitalWrite(heaterPin, LOW);
+  heaterIsOn = LOW;
+}
+
+void controlHeater() {
+  if (dutyCycle == 0) {
+    heaterOff();
+    inDutyCycleMode = LOW;
+  }
+  else if (dutyCycle == 100) {
+    heaterOn();
+    inDutyCycleMode = LOW;
+  }
+  else {
+    // If you just entered duty cycle mode
+    // start first window
+    if (!inDutyCycleMode) {
+      windowStartTime = millis();
+    }
+
+    // 5min is 300'000 ms    
+    unsigned long windowSize = 300000;
+    
+    // The onTime is windowSize * dutyCycle and since
+    // dutyCycle is in percent we divide by 100
+    unsigned long onTime = dutyCycle * (windowSize / 100);
+    
+    // Shift window time if  needed
+    if (millis() - windowStartTime > windowSize)
+      windowStartTime += windowSize;
+
+    if (millis() - windowStartTime < onTime) {
+      /* if (!heaterIsOn) */
+      /* 	Serial << "Heater on " << millis()/1000 << "\n"; */
+      heaterOn();
+    }
+    else {
+      /* if (heaterIsOn) */
+      /* 	Serial << "Heater off" << millis()/1000 << "\n"; */
+      heaterOff();
+    }
+    
+    inDutyCycleMode = HIGH;
+  }
+}
+
 void setup() {
   pinMode(pumpPin, OUTPUT);
+  pinMode(heaterPin, OUTPUT);
+
+  dutyCycle = 0;
 
   Serial.begin(57600);
   cmdMessenger.print_LF_CR();

  kACK = 001,
  kARDUINO_READY = 002, 
  kERR = 003,

  kSEND_CMDS_END, // Mustnt delete this line
};


  }
}

unsigned long windowSize;
int dutyCycle; 

void heater_msg() {
  cmdMessenger.sendCmd(kACK, "Heater msg received");
  while (cmdMessenger.available()) {
    char buf[350] = {'\0'};
    cmdMessenger.copyString(buf, 350);
    // Its of the form int, where int is percent
    // duty cycle
    dutyCycle = dutyCycle = atoi(buf);
    Serial.print(dutyCycle);
    // Check bounds
    if (dutyCycle > 100)
      dutyCycle = 100;
    if (dutyCycle < 0)
      dutyCycle = 0;
  }
}

void temp_msg() {
  cmdMessenger.sendCmd(kACK, "Request for temperature received");
  char temp[5];
  itoa(temperature, temp, 10);
  cmdMessenger.sendCmd(kACK, temp);
}

void arduino_ready() {
  // In response to ping. We just send a throw-away ack to say "im alive"
  cmdMessenger.sendCmd(kACK,"Arduino ready");
