#include "EEPROM.h"

#include <ZUNO_OneWire.h>
#include <ZUNO_DS18B20.h>

#define PIN_RELAY_BURNER 12
#define PIN_RELAY_WATER_LOAD 11
#define PIN_RELAY_WATER_RECYCLING 10
#define PIN_RELAY_HEAT_PUMP_1 9

#define PIN_INPUT_BURNER_RUNNING 16
#define PIN_INPUT_BURNER_ALARM 17
#define PIN_INPUT_BOILER_ALARM 18

#define PIN_ONEWIRE 15

#define EEPROM_MAGIC_VALUE 0x4242
#define EEPROM_MAGIC_ADDR 0
#define EEPROM_REQUESTED_TEMP_ADDR_BASE (EEPROM_MAGIC_ADDR + 2)
#define EEPROM_THERMOSTAT_MODE_BASE (EEPROM_REQUESTED_TEMP_ADDR_BASE + 4) // req temp is 2 bytes per thermostat

#define ZW_PARAM_BASE 64
#define ZW_PARAM_UPDATE_UNSOLICITED_TIME_BASE (ZW_PARAM_BASE) // 64, 65
#define ZW_PARAM_UPDATE_THRESHOLD_TEMP_BASE (ZW_PARAM_UPDATE_UNSOLICITED_TIME_BASE + 2) // 66, 67
#define ZW_PARAM_MIN_CYCLE_DURATION_BASE (ZW_PARAM_UPDATE_THRESHOLD_TEMP_BASE + 2) // 68, 69
#define ZW_PARAM_TOLERANCE_TEMP_BASE (ZW_PARAM_MIN_CYCLE_DURATION_BASE + 2) // 70, 71

#define LOG_INIT() Serial.begin(9600)
#define LOG_WRITE(x) Serial.print(x)
#define LOG_WRITELN(x) Serial.println(x)

//#define LOG_INIT()
//#define LOG_WRITE(x)
//#define LOG_WRITELN(x)

#define DALLAS_ADDR_SIZE 8

struct thermostat_t {
  byte dallasAddress[DALLAS_ADDR_SIZE];
  word currentTemp;
  
  byte requestedMode;
  word requestedTemp;
  
  unsigned long lastZwaveReadTime;
  word lastZwaveReadTemp;
  
  word updateUnsolicitedTime;
  word updateThresholdTemp;
  
  word minCycleDuration;
  word toleranceTemp;

  bool requestHeat;
};

struct output_t {
  int pin;
  bool currentValue;
  unsigned long lastChangedTime;
};

OneWire oneWire(PIN_ONEWIRE);
DS18B20Sensor dallas(&oneWire);


#define THERMOSTATS_COUNT 2
#define THERMOSTAT_HEAT_IDX 0
#define THERMOSTAT_WATER_IDX 1
thermostat_t thermostats[THERMOSTATS_COUNT];

#define OUTPUTS_COUNT 4
#define OUTPUT_BURNER_IDX 0
#define OUTPUT_WATER_LOAD_IDX 1
#define OUTPUT_WATER_RECYCLING_IDX 2
#define OUTPUT_HEAT_PUMP_1_IDX 3
output_t outputs[OUTPUTS_COUNT];

ZUNO_SETUP_CHANNELS(
  ZUNO_THERMOSTAT(THERMOSTAT_FLAGS_OFF | THERMOSTAT_FLAGS_HEAT, THERMOSTAT_UNITS_CELSIUS, THERMOSTAT_RANGE_POS, 10, getHeaterMode, setHeaterMode, getHeaterTemp, setHeaterTemp), // Heater
  ZUNO_SENSOR_MULTILEVEL(ZUNO_SENSOR_MULTILEVEL_TYPE_WATER_TEMPERATURE, SENSOR_MULTILEVEL_SCALE_CELSIUS, METER_SIZE_TWO_BYTES, SENSOR_MULTILEVEL_PRECISION_TWO_DECIMALS, getHeaterCurrentTemp), // Heater temp
  ZUNO_THERMOSTAT(THERMOSTAT_FLAGS_OFF | THERMOSTAT_FLAGS_HEAT, THERMOSTAT_UNITS_CELSIUS, THERMOSTAT_RANGE_POS, 10, getWaterMode, setWaterMode, getWaterTemp, setWaterTemp), // Water
  ZUNO_SENSOR_MULTILEVEL(ZUNO_SENSOR_MULTILEVEL_TYPE_WATER_TEMPERATURE, SENSOR_MULTILEVEL_SCALE_CELSIUS, METER_SIZE_TWO_BYTES, SENSOR_MULTILEVEL_PRECISION_TWO_DECIMALS, getWaterCurrentTemp), // Water temp
  ZUNO_SWITCH_BINARY(getRelayBurner, setRelayBurner), // Burner
  ZUNO_SWITCH_BINARY(getRelayWaterLoad, setRelayWaterLoad), // Water load pump
  ZUNO_SWITCH_BINARY(getRelayWaterRecycling, setRelayWaterRecycling), // Water recycling pump
  ZUNO_SWITCH_BINARY(getRelayHeatPump1, setRelayHeatPump1), // Heater pump Floor 1
  ZUNO_SENSOR_BINARY(ZUNO_SENSOR_BINARY_TYPE_GENERAL_PURPOSE, getInputBurnerRunning), // Burner running
  ZUNO_SENSOR_BINARY(ZUNO_SENSOR_BINARY_TYPE_GENERAL_PURPOSE, getInputBurnerAlarm), // Burner alarm
  ZUNO_SENSOR_BINARY(ZUNO_SENSOR_BINARY_TYPE_GENERAL_PURPOSE, getInputBoilerAlarm)  // Boiler alarm
);
ZUNO_SETUP_CFGPARAMETER_HANDLER(config_parameter_changed);

byte EEPROM_put(dword address, void * value, word val_size) {
  byte res = EEPROM.put(address, value, val_size);
  if (!res) {
    LOG_WRITELN("EEPROM put: FAIL");
  }
  return res;
}

byte EEPROM_get(dword address, void * value, word val_size) {
  byte res = EEPROM.get(address, value, val_size);
  if (!res) {
    LOG_WRITELN("EEPROM put: FAIL");
  }
  return res;
}

word abs_diff(word v1, word v2) {
  if (v1 > v2) {
    return v1 - v2;
  }
  return v2 - v1;
}

void setup() {
  delay(2000);
  LOG_INIT();
  LOG_WRITELN("setup BEGIN");
  
  pinMode(PIN_INPUT_BURNER_RUNNING, INPUT);
  pinMode(PIN_INPUT_BURNER_ALARM, INPUT);
  pinMode(PIN_INPUT_BOILER_ALARM, INPUT);

  word magic_value[1] = {0};
  EEPROM_get(EEPROM_MAGIC_ADDR, magic_value, sizeof(magic_value));
  if (magic_value[0] != EEPROM_MAGIC_VALUE) {
    LOG_WRITE("First init: ");
    LOG_WRITELN((int)magic_value[0]);

    zunoSaveCFGParam(ZW_PARAM_UPDATE_UNSOLICITED_TIME_BASE + 0, 600); // 10 min
    zunoSaveCFGParam(ZW_PARAM_UPDATE_UNSOLICITED_TIME_BASE + 1, 600); // 10 min

    zunoSaveCFGParam(ZW_PARAM_UPDATE_THRESHOLD_TEMP_BASE + 0, 50); // 0.5 °C
    zunoSaveCFGParam(ZW_PARAM_UPDATE_THRESHOLD_TEMP_BASE + 1, 50); // 0.5 °C

    zunoSaveCFGParam(ZW_PARAM_MIN_CYCLE_DURATION_BASE + 0, 60); // 1 min
    zunoSaveCFGParam(ZW_PARAM_MIN_CYCLE_DURATION_BASE + 1, 60); // 1 min

    zunoSaveCFGParam(ZW_PARAM_TOLERANCE_TEMP_BASE + 0, 2000); // 20 °C
    zunoSaveCFGParam(ZW_PARAM_TOLERANCE_TEMP_BASE + 1, 1000); // 10 °C

    word requestedTemps[2] = {8000, 3000}; // 80 °C; 30 °C
    EEPROM_put(EEPROM_REQUESTED_TEMP_ADDR_BASE, requestedTemps, sizeof(requestedTemps));

    byte modes[2] = {THERMOSTAT_MODE_OFF, THERMOSTAT_MODE_OFF};
    EEPROM_put(EEPROM_THERMOSTAT_MODE_BASE, modes, sizeof(modes));

    magic_value[0] = EEPROM_MAGIC_VALUE;
    EEPROM_put(EEPROM_MAGIC_ADDR, magic_value, sizeof(magic_value));
  }

  byte dallasAddresses[DALLAS_ADDR_SIZE * 2];
  byte dallasAddressesCount = dallas.findAllSensors(dallasAddresses); // TODO retry if not 2
  LOG_WRITE("Found ");
  LOG_WRITE(dallasAddressesCount);
  LOG_WRITELN(" sensors");

  word requestedTemps[2] = {8000, 3000};
  EEPROM_get(EEPROM_REQUESTED_TEMP_ADDR_BASE, requestedTemps, sizeof(requestedTemps));
  byte modes[2] = {THERMOSTAT_MODE_OFF, THERMOSTAT_MODE_OFF};
  EEPROM_get(EEPROM_THERMOSTAT_MODE_BASE, modes, sizeof(modes));

  for (int ti = 0; ti < THERMOSTATS_COUNT; ++ti) {
    memcpy(thermostats[ti].dallasAddress, &dallasAddresses[ti * DALLAS_ADDR_SIZE], DALLAS_ADDR_SIZE);
    thermostats[ti].currentTemp = BAD_TEMP;
    
    thermostats[ti].requestedMode = modes[ti];
    thermostats[ti].requestedTemp = requestedTemps[ti];
    
    thermostats[ti].lastZwaveReadTime = 0;
    thermostats[ti].lastZwaveReadTemp = BAD_TEMP;
    
    thermostats[ti].updateUnsolicitedTime = zunoLoadCFGParam(ZW_PARAM_UPDATE_UNSOLICITED_TIME_BASE + ti);
    thermostats[ti].updateThresholdTemp = zunoLoadCFGParam(ZW_PARAM_UPDATE_THRESHOLD_TEMP_BASE + ti);
    
    thermostats[ti].minCycleDuration = zunoLoadCFGParam(ZW_PARAM_MIN_CYCLE_DURATION_BASE + ti);
    thermostats[ti].toleranceTemp = zunoLoadCFGParam(ZW_PARAM_TOLERANCE_TEMP_BASE + ti);

    thermostats[ti].requestHeat = false;
  }

  outputs[OUTPUT_BURNER_IDX].pin = PIN_RELAY_BURNER;
  outputs[OUTPUT_WATER_LOAD_IDX].pin = PIN_RELAY_WATER_LOAD;
  outputs[OUTPUT_WATER_RECYCLING_IDX].pin = PIN_RELAY_WATER_RECYCLING;
  outputs[OUTPUT_HEAT_PUMP_1_IDX].pin = PIN_RELAY_HEAT_PUMP_1;
  for (int oi = 0; oi < OUTPUTS_COUNT; ++oi) {
    outputs[oi].currentValue = 0;
    outputs[oi].lastChangedTime = 0;
    pinMode(outputs[oi].pin, OUTPUT);
    digitalWrite(outputs[oi].pin, 0);
  }
  LOG_WRITELN("setup END");
}

void updateValues() {
  // Temp sensors
  for (int ti = 0; ti < THERMOSTATS_COUNT; ++ti) {
    word newValue = BAD_TEMP;
    for (int j = 0; j < 3 && (newValue == BAD_TEMP || newValue == 0); ++j) {
      newValue = dallas.getTemperature(thermostats[ti].dallasAddress) * 100;
    }
    thermostats[ti].currentTemp = newValue;

    if (thermostats[ti].requestedMode == THERMOSTAT_MODE_HEAT) {
      if (thermostats[ti].currentTemp <= thermostats[ti].requestedTemp - thermostats[ti].toleranceTemp && !thermostats[ti].requestHeat) { // TODO Check overflow
        LOG_WRITE("Thermostat ");
        LOG_WRITE(ti);
        LOG_WRITE(" is too low: ");
        LOG_WRITE((int)thermostats[ti].currentTemp);
        LOG_WRITE(" << ");
        LOG_WRITELN((int)thermostats[ti].requestedTemp);
        thermostats[ti].requestHeat = true;
      }
      else if (thermostats[ti].currentTemp >= thermostats[ti].requestedTemp && thermostats[ti].requestHeat) {
        LOG_WRITE("Thermostat ");
        LOG_WRITE(ti);
        LOG_WRITE(" is acceptable: ");
        LOG_WRITE((int)thermostats[ti].currentTemp);
        LOG_WRITE(" >= ");
        LOG_WRITELN((int)thermostats[ti].requestedTemp);
        thermostats[ti].requestHeat = false;
      }
    }
    else {
      if (thermostats[ti].requestHeat) {
        LOG_WRITE("Thermostat ");
        LOG_WRITE(ti);
        LOG_WRITELN(" has been turned OFF");
        thermostats[ti].requestHeat = false;
        setRelayManual(OUTPUT_BURNER_IDX, 0);
        setRelayManual(OUTPUT_WATER_LOAD_IDX, 0);
      }
    }
  }
}

void updateZwave() {
  for (int ti = 0; ti < THERMOSTATS_COUNT; ++ti) {
    const word oldValue = thermostats[ti].lastZwaveReadTemp;
    const word newValue = thermostats[ti].currentTemp;
    if (abs_diff(newValue, oldValue) > thermostats[ti].updateThresholdTemp) {
      LOG_WRITE("Sensor ");
      LOG_WRITE(ti);
      LOG_WRITE(" value changed from ");
      LOG_WRITE((int)oldValue);
      LOG_WRITE(" to ");
      LOG_WRITELN((int)newValue);
      zunoSendReport(2 + (ti * 2)); // 2 or 4
    }
    else if (millis() - thermostats[ti].lastZwaveReadTime > (unsigned long)thermostats[ti].updateUnsolicitedTime * 1000) {
      LOG_WRITE("Sensor ");
      LOG_WRITE(ti);
      LOG_WRITE(" unsolicited update to ");
      LOG_WRITELN((int)newValue);
      zunoSendReport(2 + (ti * 2)); // 2 or 4
    }
  }
}

void updateOutputs() {

  // TODO Handle min cycle
  bool burnerRequested = thermostats[THERMOSTAT_HEAT_IDX].requestHeat || (thermostats[THERMOSTAT_WATER_IDX].requestHeat && thermostats[THERMOSTAT_HEAT_IDX].currentTemp < thermostats[THERMOSTAT_WATER_IDX].requestedTemp);
  bool waterLoadRequested = thermostats[THERMOSTAT_WATER_IDX].requestHeat && thermostats[THERMOSTAT_HEAT_IDX].currentTemp > thermostats[THERMOSTAT_WATER_IDX].currentTemp;

  if (thermostats[THERMOSTAT_HEAT_IDX].requestedMode == THERMOSTAT_MODE_HEAT || thermostats[THERMOSTAT_WATER_IDX].requestedMode == THERMOSTAT_MODE_HEAT) {
    setRelay(OUTPUT_BURNER_IDX, burnerRequested);
  }
  if (thermostats[THERMOSTAT_WATER_IDX].requestedMode == THERMOSTAT_MODE_HEAT) {
    setRelay(OUTPUT_WATER_LOAD_IDX, waterLoadRequested);
  }
}

void loop() {
  updateValues();
  updateZwave();
  updateOutputs();
  delay(100);
}

// ZWave callbacks
void config_parameter_changed(byte param, word value) {
    LOG_WRITE("Zwave param ");
    LOG_WRITE((int)param);
    LOG_WRITE(": ");
    LOG_WRITELN((int)value);

    if (param >= ZW_PARAM_UPDATE_UNSOLICITED_TIME_BASE && param < ZW_PARAM_UPDATE_UNSOLICITED_TIME_BASE + THERMOSTATS_COUNT) {
      thermostats[param - ZW_PARAM_UPDATE_UNSOLICITED_TIME_BASE].updateUnsolicitedTime = value;
    }
    else if (param >= ZW_PARAM_UPDATE_THRESHOLD_TEMP_BASE && param < ZW_PARAM_UPDATE_THRESHOLD_TEMP_BASE + THERMOSTATS_COUNT) {
      thermostats[param - ZW_PARAM_UPDATE_THRESHOLD_TEMP_BASE].updateThresholdTemp = value;
    }
    else if (param >= ZW_PARAM_MIN_CYCLE_DURATION_BASE && param < ZW_PARAM_MIN_CYCLE_DURATION_BASE + THERMOSTATS_COUNT) {
      thermostats[param - ZW_PARAM_MIN_CYCLE_DURATION_BASE].minCycleDuration = value;
    }
    else if (param >= ZW_PARAM_TOLERANCE_TEMP_BASE && param < ZW_PARAM_TOLERANCE_TEMP_BASE + THERMOSTATS_COUNT) {
      thermostats[param - ZW_PARAM_TOLERANCE_TEMP_BASE].toleranceTemp = value;
    }
}

// Thermostats
void setMode(int ti, byte mode) {
  LOG_WRITE("Thermostat ");
  LOG_WRITE(ti);
  LOG_WRITE(" update: mode: ");
  LOG_WRITELN((int)mode);
  thermostats[ti].requestedMode = mode;
  EEPROM_put(EEPROM_THERMOSTAT_MODE_BASE + (sizeof(thermostats[ti].requestedMode) * ti), &thermostats[ti].requestedMode, sizeof(thermostats[ti].requestedMode));
}
byte getMode(int ti) {
  return thermostats[ti].requestedMode;
}
void setTemp(int ti, byte mode, word temp) {
  LOG_WRITE("Thermostat ");
  LOG_WRITE(ti);
  LOG_WRITE(" update: mode: ");
  LOG_WRITE((int)mode);
  LOG_WRITE(" temp: ");
  LOG_WRITELN((int)temp);
  thermostats[ti].requestedMode = mode;
  EEPROM_put(EEPROM_THERMOSTAT_MODE_BASE + (sizeof(thermostats[ti].requestedMode) * ti), &thermostats[ti].requestedMode, sizeof(thermostats[ti].requestedMode));
  if (mode == THERMOSTAT_MODE_HEAT) {
    thermostats[ti].requestedTemp = temp * 10;// Z-Wave thermostat precision is 0.1
    EEPROM_put(EEPROM_REQUESTED_TEMP_ADDR_BASE + (sizeof(thermostats[ti].requestedTemp) * ti), &thermostats[ti].requestedTemp, sizeof(thermostats[ti].requestedTemp));
  }
}
word getTemp(int ti, byte mode) {
  if (mode == THERMOSTAT_MODE_HEAT) {
    return thermostats[ti].requestedTemp / 10;// Z-Wave thermostat precision is 0.1
  }
  return 0;
}
word getCurrentTemp(int ti) {
  LOG_WRITE("Sensor ");
  LOG_WRITE(ti);
  LOG_WRITELN(" Zwave update");
  thermostats[ti].lastZwaveReadTime = millis();
  thermostats[ti].lastZwaveReadTemp = thermostats[ti].currentTemp;
  return thermostats[ti].lastZwaveReadTemp;
}



// Heater
void setHeaterMode(byte mode) {
  setMode(THERMOSTAT_HEAT_IDX, mode);
}
byte getHeaterMode(){
  return getMode(THERMOSTAT_HEAT_IDX);
}
void setHeaterTemp(byte mode, word temp) {
  setTemp(THERMOSTAT_HEAT_IDX, mode, temp);
}
word getHeaterTemp(byte mode) {
  return getTemp(THERMOSTAT_HEAT_IDX, mode);
}
word getHeaterCurrentTemp() {
  return getCurrentTemp(THERMOSTAT_HEAT_IDX);
}

// Water
void setWaterMode(byte mode) {
  setMode(THERMOSTAT_WATER_IDX, mode);
}
byte getWaterMode(){
  return getMode(THERMOSTAT_WATER_IDX);
}
void setWaterTemp(byte mode, word temp) {
  setTemp(THERMOSTAT_WATER_IDX, mode, temp);
}
word getWaterTemp(byte mode) {
  return getTemp(THERMOSTAT_WATER_IDX, mode);
}
word getWaterCurrentTemp() {
  return getCurrentTemp(THERMOSTAT_WATER_IDX);
}

// Raw outputs

void setRelay(int oi, bool value) {
  if (value != outputs[oi].currentValue) {
    LOG_WRITE("Output ");
    LOG_WRITE(oi);
    LOG_WRITE(" update: value: ");
    LOG_WRITELN((int)value);
    outputs[oi].currentValue = value;
    outputs[oi].lastChangedTime = millis();
    digitalWrite(outputs[oi].pin, value);
    zunoSendReport(5 + oi); // 5 to 8
  }
}
byte getRelay(int oi) {
  return outputs[oi].currentValue;
}

void setRelayManual(int oi, byte value) {
  if (oi == OUTPUT_BURNER_IDX && (thermostats[THERMOSTAT_HEAT_IDX].requestedMode == THERMOSTAT_MODE_HEAT || thermostats[THERMOSTAT_WATER_IDX].requestedMode == THERMOSTAT_MODE_HEAT)) {
    return;
  }
  if (oi == OUTPUT_WATER_LOAD_IDX && thermostats[THERMOSTAT_WATER_IDX].requestedMode == THERMOSTAT_MODE_HEAT) {
    return;
  }

  setRelay(oi, value ? 1 : 0);
}
byte getRelayZwave(int oi) {
  return getRelay(oi) ? 255 : 0;
}

void setRelayBurner(byte value) {
  setRelayManual(OUTPUT_BURNER_IDX, value);
}
byte getRelayBurner() {
  return getRelayZwave(OUTPUT_BURNER_IDX);
}

void setRelayWaterLoad(byte value) {
  setRelayManual(OUTPUT_WATER_LOAD_IDX, value);
}
byte getRelayWaterLoad() {
  return getRelayZwave(OUTPUT_WATER_LOAD_IDX);
}

void setRelayWaterRecycling(byte value) {
  setRelayManual(OUTPUT_WATER_RECYCLING_IDX, value);
}
byte getRelayWaterRecycling() {
  return getRelayZwave(OUTPUT_WATER_RECYCLING_IDX);
}

void setRelayHeatPump1(byte value) {
  setRelayManual(OUTPUT_HEAT_PUMP_1_IDX, value);
}
byte getRelayHeatPump1() {
  return getRelayZwave(OUTPUT_HEAT_PUMP_1_IDX);
}

// Raw inputs

byte getInputBurnerRunning() {
  return !digitalRead(PIN_INPUT_BURNER_RUNNING);
}

byte getInputBurnerAlarm() {
  return !digitalRead(PIN_INPUT_BURNER_ALARM);
}

byte getInputBoilerAlarm() {
  return !digitalRead(PIN_INPUT_BOILER_ALARM);
}