/* Pin settings
 * A0: battery 0 voltage
 * D2: battery 0 drain control
 * A1: battery 1 voltage
 * D3: battery 1 drain control
 */

#define PIN_VOLTAGE_0 A0
#define PIN_CONTROL_0 2
#define PIN_VOLTAGE_1 A1
#define PIN_CONTROL_1 3
#define PIN_LED 13

uint8_t status = 0;
uint32_t next = 0;

/*
uint32_t readRef() {
  // Read 1.1V reference against AVcc
  // set the reference to Vcc and the measurement to the internal 1.1V reference
  ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
  delay(2); // Wait for Vref to settle
  uint32_t total=0;
  ADCSRA |= _BV(ADSC); // Start conversion
  while (bit_is_set(ADCSRA,ADSC)); // measuring
  uint8_t low  = ADCL; // must read ADCL first - it then locks ADCH
  uint8_t high = ADCH; // unlocks both
  uint32_t result = (high<<8) | low;
  return result;
}
*/

uint32_t readVcc() {
  // Read 1.1V reference against AVcc
  // set the reference to Vcc and the measurement to the internal 1.1V reference
  ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
  delay(2); // Wait for Vref to settle
  uint32_t total=0;
  for (int i=0; i< 1000; i++) {
    ADCSRA |= _BV(ADSC); // Start conversion
    while (bit_is_set(ADCSRA,ADSC)); // measuring
    uint8_t low  = ADCL; // must read ADCL first - it then locks ADCH
    uint8_t high = ADCH; // unlocks both
    uint32_t result = (high<<8) | low;
    total += result;
  }
  // VREF = 1.089 +/- 0.001 V
  //     1115136000 = 1024 * VREF * 1000 (loop) * 1000 (millivolts)
  return 1115136000L/total; // in millivolts
}

uint32_t readBattery(uint8_t pin) {
  uint32_t total = 0;
  for(int i=0; i<1000; i++) {
    total += analogRead(pin);
  }
  return total * readVcc() / 1024000;
}

void setup() {
  Serial.begin(57600);
  pinMode(PIN_CONTROL_0, OUTPUT);
  pinMode(PIN_CONTROL_1, OUTPUT);
  digitalWrite(PIN_CONTROL_0, LOW);
  digitalWrite(PIN_CONTROL_1, LOW);
  digitalWrite(PIN_LED, LOW);
}

void print() {
  uint32_t v0, v1, s0, s1;
  v0 = readBattery(PIN_VOLTAGE_0);
  v1 = readBattery(PIN_VOLTAGE_1);
  s0 = status & 0x1;
  s1 = status & 0x2;
  Serial.print(s0 ? "1" : "0");
  Serial.print(" ");
  Serial.print(v0, DEC);
  Serial.print(" ");
  Serial.print(s0 ? v0 * 1000 / 3300 : 0, DEC);
  Serial.print(" ");
  Serial.print(s1 ? "1" : "0");
  Serial.print(" ");
  Serial.print(v1, DEC);
  Serial.print(" ");
  Serial.print(s1 ? v1 * 1000 / 3300 : 0, DEC);
  Serial.println();
}

void drain_control() {
  digitalWrite(PIN_CONTROL_0, (status & 0x1) ? HIGH : LOW);
  digitalWrite(PIN_CONTROL_1, (status & 0x2) ? HIGH : LOW);

  digitalWrite(PIN_LED, status ? HIGH : LOW);
}

void loop() {
  if (Serial.available() > 0) {
    char c = Serial.read();
    if (c >= '0' && c <= '3') {
      status = c - '0';
      drain_control();
    }
  }
  if (millis() >= next) {
    print();
    next = millis() + 1000;
  }
}