#include <SPI.h>
#include <ESP8266WiFi.h>
#include <PubSubClient.h>
#include <Wire.h>
#include <Adafruit_INA219.h>
#include "DHT.h"
#include <max6675.h>

Adafruit_INA219 ina219IN(0x040);
Adafruit_INA219 ina219OUT1(0x44);
Adafruit_INA219 ina219OUT2(0x41);

float shuntvoltageIN = 0;
float busvoltageIN = 0;
float current_mAIN = 0;
float current_AIN = 0;
float loadvoltageIN = 0;
float power_WIN = 0;

float shuntvoltageOUT1 = 0;
float busvoltageOUT1 = 0;
float current_mAOUT1 = 0;
float current_AOUT1 = 0;
float loadvoltageOUT1 = 0;
float power_WOUT1 = 0;

float shuntvoltageOUT2 = 0;
float busvoltageOUT2 = 0;
float current_mAOUT2 = 0;
float current_AOUT2 = 0;
float loadvoltageOUT2 = 0;
float power_WOUT2 = 0;

const char* ssid = "SSID";
const char* password = "Password";
const char* mqtt_server = "mqtt server ip addres";
const char* mqttUser = "user";            
const char* mqttPassword = "password";        
WiFiClient espClient;
PubSubClient client(espClient);
long lastMsg = 0;
char msg[50];


#define DHTPIN 2    // modify to the pin we connected
#define DHTTYPE DHT21   // AM2301 
DHT dht(DHTPIN, DHTTYPE);

const int pinSO  = 13;
int pinCS  = 12;
int pinSCK = 14;
MAX6675 termocoupler(pinSCK, pinCS, pinSO);

void callback(char* topic, byte* payload, unsigned int length) {
  Serial.print("Message arrived [");
  Serial.print(topic);
  Serial.print("] ");
  for (int i=0;i<length;i++) {
    Serial.print((char)payload[i]);
  }
  Serial.println();
}

void reconnect() {
  // Loop until we're reconnected
  while (!client.connected()) {
    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (client.connect("ESP8266Client", mqttUser, mqttPassword )) {  
      Serial.println("connected");
      client.subscribe("CayenneInput");
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Sleep for 5 minutes before retrying
      Serial.println("Failed to connect to MQTT server - sleeping for 5 minutes");
      delay(100);
      //ESP.deepSleep(10000, WAKE_RF_DEFAULT); //300000000
      delay(100);
    }
  }
}

void setup()
{
  Serial.begin(115200);
  
  //Wathdog timer
  pinMode(15, OUTPUT);
  digitalWrite(15, HIGH);
  
  int timeoutcounter = 0;
  WiFi.begin(ssid, password);
  Serial.println();
  Serial.println();
  Serial.print("Connecting to ");
  Serial.println(ssid); 
  while (WiFi.status() != WL_CONNECTED) {
    timeoutcounter++;
    delay(500);
    Serial.print(".");
    if (timeoutcounter >= 30){
      Serial.println("Failed to connect to wireless - sleeping for 5 minutes");
      delay(100);
      //ESP.deepSleep(10000, WAKE_RF_DEFAULT);
      delay(100);
    }
  }
  Serial.println("");
  Serial.println("WiFi connected");
  client.setServer(mqtt_server, 1883);
  client.setCallback(callback);
  
  delay(100);
  
  Wire.begin(5, 4); // SDA, SCL
  uint32_t currentFrequency;
  ina219IN.begin();
  ina219OUT1.begin();
  ina219OUT2.begin();
  dht.begin();
}

void loop()
{
  if (!client.connected()) {
    reconnect();
  }
  client.loop();
 
  digitalWrite(15, HIGH);
  delay(500);
  //Input power from solar panel or external power supply
  shuntvoltageIN = ina219IN.getShuntVoltage_mV();
  busvoltageIN = ina219IN.getBusVoltage_V();
  current_mAIN = ina219IN.getCurrent_mA();
  loadvoltageIN = busvoltageIN + (shuntvoltageIN / 1000);
  current_AIN = current_mAIN / 1000;
  power_WIN = current_AIN * loadvoltageIN; 
  
  Serial.print("Input:    ");
  Serial.print(loadvoltageIN); Serial.print("V  ");
  Serial.print(current_AIN); Serial.print("A  ");
  Serial.print(power_WIN); Serial.print("W ");
  Serial.println();
  
  String mqtt_message = String(loadvoltageIN); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/ina219IN/voltage/", msg);
   mqtt_message = String(current_AIN); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/ina219IN/current/", msg);
   mqtt_message = String(power_WIN); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/ina219IN/power/", msg);

  //Output to IoT devices (switchboard, Rasp.Pi, solar box)
  shuntvoltageOUT1 = ina219OUT1.getShuntVoltage_mV();
  busvoltageOUT1 = ina219OUT1.getBusVoltage_V();
  current_mAOUT1 = ina219OUT1.getCurrent_mA();
  loadvoltageOUT1 = busvoltageOUT1 + (shuntvoltageOUT1 / 1000);
  current_AOUT1 = current_mAOUT1 / 1000;
  power_WOUT1 = current_AOUT1 * loadvoltageOUT1; 
  
  Serial.print("Output 1:    ");
  Serial.print(loadvoltageOUT1); Serial.print("V  ");
  Serial.print(current_AOUT1); Serial.print("A  ");
  Serial.print(power_WOUT1); Serial.print("W ");
  Serial.println();
  
  mqtt_message = String(loadvoltageOUT1); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/ina219OUT1/voltage/", msg);
   mqtt_message = String(current_AOUT1); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/ina219OUT1/current/", msg);
   mqtt_message = String(power_WOUT1); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/ina219OUT1/power/", msg);

  //Output to LED strips and charging port
  shuntvoltageOUT2 = ina219OUT2.getShuntVoltage_mV();
  busvoltageOUT2 = ina219OUT2.getBusVoltage_V();
  current_mAOUT2 = ina219OUT2.getCurrent_mA();
  loadvoltageOUT2 = busvoltageOUT2 + (shuntvoltageOUT2 / 1000);
  current_AOUT2 = current_mAOUT2 / 1000;
  power_WOUT2 = current_AOUT2 * loadvoltageOUT2; 
  
  Serial.print("Output 2:  ");
  Serial.print(loadvoltageOUT2); Serial.print("V  ");
  Serial.print(current_AOUT2); Serial.print("A  ");
  Serial.print(power_WOUT2); Serial.print("W ");
  Serial.println();
  
  mqtt_message = String(loadvoltageOUT2); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/ina219OUT2/voltage/", msg);
   mqtt_message = String(current_AOUT2); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/ina219OUT2/current/", msg);
   mqtt_message = String(power_WOUT2); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/ina219OUT2/power/", msg);

  //All output power
  float allPower = power_WOUT1 + power_WOUT2;
   mqtt_message = String(allPower); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/ina219/allPower/", msg);

  //Status message every 1s
  mqtt_message = String("1"); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("device/solarbox/status/", msg);

  //Temperature and humidity indide a solar box
  float insideHUM = dht.readHumidity();
  Serial.println(insideHUM);
  delay(100);
   mqtt_message = String(insideHUM); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/inside/humidity/", msg);
  delay(100);
  float insideTEM = dht.readTemperature();
  Serial.println(insideTEM);
  delay(100);
   mqtt_message = String(insideTEM); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/inside/temperature/", msg);
  delay(100);

  //Temperature house heating system
  float heating = termocoupler.readCelsius();
  Serial.println(heating);
  delay(100);
   mqtt_message = String(heating); 
  mqtt_message.toCharArray(msg, 50);
  client.publish("solarbox/sensors/heating/temperature/", msg);
  delay(100);
  
  digitalWrite(15, LOW);
  delay(1000);
}