#include <Wire.h>
#include <Servo.h>
#include <Adafruit_INA219.h>

#include <DallasTemperature.h>

#define ONE_WIRE_BUS 2

OneWire oneWire(ONE_WIRE_BUS);
DallasTemperature sensors(&oneWire);

Adafruit_INA219 ina219;
Servo myservo;
int pos = 0;
int counter = 0;
int startedCounter = 0;

bool isTurningOff = false;
bool hasStarted = false;

const int POWER_BUTTON = 7;
const int POWER_OFF_PIN = 8;

void setup(void)
{
  Serial.begin(115200);
  myservo.attach(9);
  while (!Serial) {
      // will pause Zero, Leonardo, etc until serial console opens
      delay(1);
  }

  sensors.begin();

  uint32_t currentFrequency;

  Serial.println("Hello!");

  pinMode(POWER_BUTTON, INPUT);
  pinMode(POWER_OFF_PIN, OUTPUT);

  ina219.begin();

  Serial.println("Measuring voltage and current with INA219 ...");
}

void loop(void)
{

  if (startedCounter > 2000) {
    startedCounter = 0;
    hasStarted = true;
  }

  if (Serial.available() > 0) {
    processMessage();
  } else if (counter > 2000) {
    counter = 0;
    float shuntvoltage = 0;
    float busvoltage = 0;
    float current_mA = 0;
    float loadvoltage = 0;

    shuntvoltage = ina219.getShuntVoltage_mV();
    busvoltage = ina219.getBusVoltage_V();
    current_mA = ina219.getCurrent_mA();
    loadvoltage = busvoltage + (shuntvoltage / 1000);
//    power = busvoltage * (current_mA/1000);


    // 1.2 x (ncellx - 1) = 6V = cut off voltage
    Serial.print("B:");
    Serial.print(loadvoltage); Serial.print(",");
    Serial.print(current_mA);
    Serial.println("");

    sensors.requestTemperatures();

    Serial.print("Temperature is: ");
    Serial.println(sensors.getTempCByIndex(0));
    Serial.println(sensors.getTempCByIndex(1));

  } else {

    if (hasStarted && !isTurningOff && digitalRead(POWER_BUTTON) == HIGH) {
      isTurningOff = true;

      Serial.println("Turning it off");
//      turnOff();
    }

  }

  delay(1);
  counter += 1;
  startedCounter += 1;
}

void turnOff() {
  digitalWrite(POWER_OFF_PIN, HIGH);
}

void processMessage() {
  while (Serial.available() > 0) {

    char c = Serial.peek();
    // ignore newlines
    if (c == 'S') {
      Serial.println("OK");
      Serial.read();
      int pos = Serial.parseInt();
      myservo.write(pos);
    } else {
      Serial.read();
    }
  }
}