/////////////////////////////////////////
// HXbot HXpower firmware v4a           /
// EoF 2016 EoF@itphx.ru                /
/////////////////////////////////////////

#include <Wire.h>

// DEBUG
#define DEBUG 0

// DEFINE
#define SLAVE_ADDRESS 0x05
#define XOR_SEQ 0xFF
#define EXT_COM 0xAA

#define AIN_PIN A9
#define ABT_PIN A8
#define ABA_PIN A7
#define AHX_PIN A6

#define VIN_PIN A5
#define VDC_PIN A4
#define VBT_PIN A3
#define VBA_PIN A2
#define VZU_PIN A1
#define VPF_PIN A0

#define R_BT 11
#define R_BA 7
#define R_MV 5
#define R_US 1
//#define RED_PIN 10
//#define GRN_PIN 12
#define INC_PIN 0
#define LED_PIN 13

#define REP_COUNT 5

#define TIMEOUT 5000
#define BUTTON_REACTION_TIME 5000

// COMMANDS
// VOLTMETERS
#define COM_GET_VIN 0x01
#define COM_GET_VDC 0x02
#define COM_GET_VBT 0x03
#define COM_GET_VBA 0x04
#define COM_GET_VPF 0x05
#define COM_GET_VZU 0x06
#define COM_GET_VCC 0x07

// AMPERMETERS
#define COM_GET_ABT 0x21
#define COM_GET_ABA 0x22
#define COM_GET_AIN 0x23
#define COM_GET_AHX 0x24

// STATUS
#define COM_GET_STAT1 0x08
#define COM_GET_STAT2 0x09
#define COM_GET_STAT3 0x20

// TEMP
#define COM_GET_TEMP 0x10

// RELAYS
#define COM_ENABLE_BA   0x11 // 17
#define COM_ENABLE_MV   0x12 // 18
#define COM_ENABLE_BT   0x13 // 19
#define COM_ENABLE_US   0x14 // 20

#define COM_DISABLE_BA  0x15 // 21
#define COM_DISABLE_MV  0x16 // 22
#define COM_DISABLE_BT  0x17 // 23
#define COM_DISABLE_US  0x18 // 24

#define COM_RUN      0x1B // 27
#define COM_STOP     0x1C // 28
#define COM_RESET_US 0x1D // 29

#define OK_RSP  0x00
#define NO_RSP  0xFF
#define ERR_RSP 0x01
#define BLK_RSP 0x02
#define CSE_RSP 0x03
#define IOE_RSP 0x04
#define TMO_RSP 0x05

// CONST
#define BT_BLOCK_TIMEOUT 30000
#define BT_FULL_CHECK_CYCLES 20
#define BA_BLOCK_TIMEOUT 30000
#define BA_FULL_CHECK_CYCLES 20
#define V_CHECK_CYCLES 10
#define AREF 5

#define VIN_MIN 14.0
#define VIN_MAX 25.0

#define VDC_MIN 5.5
#define VDC_MAX 6.5

#define VBT_MIN 6.6
#define VBT_MAX 8.6

#define VBA_MIN 9.0
#define VBA_MAX 13.0
#define VBA_FULL 12.0

#define VPF_MIN 7.2
#define VPF_MAX 10.0

#define VZU_MIN 5.0
#define VZU_MAX 6.3

#define VCC_MIN 4.8
#define VCC_MAX 5.5

#define ABT_MIN 0.1
#define ABT_MAX 5.0

#define ABA_MIN 0.1
#define ABA_MAX 5.0

#define AIN_MIN 0.1
#define AIN_MAX 5.0

#define AHX_MIN 0.1
#define AHX_MAX 5.0

// VAR
// резисторы делителя напряжения
const float r1_1 = 100000; // 100K
const float r1_2 = 10000;  // 10K
const float r2_1 = 10380;  // 10K
const float r2_2 = 10000;  // 10K
const float r3_1 = 20000;  // 10K
const float r3_2 = 10000;  // 10K
const float r4_1 = 20330;  // 20K
const float r4_2 = 10000;  // 10K
const float r5_1 = 1060;   // 1K
const float r5_2 = 1000;   // 1K
const float r6_1 = 1000;   // 1K
const float r6_2 = 1000;   // 1K

// эту константу (typVbg) необходимо откалибровать индивидуально
const float typVbg = 1.08; // 1.0 -- 1.2

float vcc = 0.0, tcc;
float kin, kdc, kbt, kba, kpf, kzu;
float vin, vdc, vbt, vba, vpf, vzu;
float tin, tdc, tbt, tba, tpf, tzu;
float abt, aba, ain, ahx;
float uus, uba, uin, udc;

byte cmd = 0x00;
byte ext = 0x00;
byte sum = 0x00;

unsigned long lastCommandTime = 0;
byte autoResponse = 0x00;

byte byteResponse = 0x00;
boolean needByte = false;
boolean needSumm = false;

float floatResponse = 0.0;
boolean needFloat = false;
byte floatByte = 0;

unsigned long bt_time, ba_time;
boolean bt_blocked = false, ba_blocked = false;

byte ba_full_count = 0, bt_full_count = 0;

unsigned long inc_time;

boolean vin_ok = false;
byte vin_count = 0;

boolean vdc_ok = false;
byte vdc_count = 0;

boolean vbt_ok = false, vbt_fail = false;
byte vbt_count = 0, vbt_fail_count = 0;

boolean vba_ok = false;
byte vba_count = 0;

boolean vpf_ok = false;
byte vpf_count = 0;

boolean vzu_ok = false;
byte vzu_count = 0;

boolean vcc_ok = false;
byte vcc_count = 0;

boolean abt_ok = false;
byte abt_count = 0;

boolean aba_ok = false;
byte aba_count = 0;

boolean ain_ok = false;
byte ain_count = 0;

boolean ahx_ok = false;
byte ahx_count = 0;

boolean ba_enabled = false;
boolean bt_enabled = true;
boolean mv_enabled = false;
boolean us_enabled = true;

boolean bt_full = false;
boolean bt_charge = false;
boolean ba_full = false;
boolean ba_charge = false;

byte status_1 = 0x00;
byte status_2 = 0x00;
byte status_3 = 0x00;

/****************************************************************************
 * Главная программа
 ****************************************************************************/

void setup() {
  if (DEBUG) {
    Serial.begin(9600);
    Serial.println("---");
    delay(1000);
  }
  
  // определение опорного напряжения
  analogReference(DEFAULT);  // DEFAULT INTERNAL использовать vcc как AREF
  
  kin = r1_2 / (r1_1 + r1_2);
  kdc = r2_2 / (r2_1 + r2_2);
  kbt = r3_2 / (r3_1 + r3_2);
  kba = r4_2 / (r4_1 + r4_2);
  kpf = r5_2 / (r5_1 + r5_2);
  kzu = r6_2 / (r6_1 + r6_2);
  
  if (DEBUG) {
    Serial.print("Vcc= ");
    Serial.println(vcc);
    Serial.println("---");
  }
  
  // Initialize i2c as slave
  Wire.begin(SLAVE_ADDRESS);
 
  // Define callbacks for i2c communication
  Wire.onReceive(receiveData);
  Wire.onRequest(answer);
  
  pinMode(R_BT, OUTPUT);
  pinMode(R_BA, OUTPUT);
  pinMode(R_MV, OUTPUT);
  pinMode(R_US, OUTPUT);
  
  pinMode(VIN_PIN, INPUT);
  pinMode(VDC_PIN, INPUT);
  pinMode(VBT_PIN, INPUT);
  pinMode(VBA_PIN, INPUT);
  pinMode(VPF_PIN, INPUT);
  pinMode(VZU_PIN, INPUT);
  
  pinMode(ABT_PIN, INPUT);
  pinMode(ABA_PIN, INPUT);
  pinMode(AIN_PIN, INPUT);
  pinMode(AHX_PIN, INPUT);

  pinMode(INC_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
    
  bt_time = millis();
}

void loop() {
  // Timeout protection
  if ((lastCommandTime + TIMEOUT < millis())) {
    floatResponse = 0.0;
    floatByte = 0;
    needFloat = false;
    autoResponse = TMO_RSP;
  }

  // BLOCK OF MAGIC 
  readvcc();
  analogRead(VIN_PIN);
  readvcc();
  analogRead(VIN_PIN);
  readvcc();
  analogRead(VIN_PIN);
  // MAGIC

  // Get voltages
  tin = 0.0;
  tdc = 0.0;
  tbt = 0.0;
  tba = 0.0;
  tpf = 0.0;
  tzu = 0.0;
  
  tin += readvcc() * analogRead(VIN_PIN);
  tdc += readvcc() * analogRead(VDC_PIN);
  tbt += readvcc() * analogRead(VBT_PIN);
  tba += readvcc() * analogRead(VBA_PIN);
  tpf += readvcc() * analogRead(VPF_PIN);
  tzu += readvcc() * analogRead(VZU_PIN);

  tcc = readvcc();
  tin = tin / 1024.0 / kin;// / REP_COUNT;
  tdc = tdc / 1024.0 / kdc;// / REP_COUNT;
  tbt = tbt / 1024.0 / kbt;// / REP_COUNT;
  tba = tba / 1024.0 / kba;// / REP_COUNT;
  tpf = tpf / 1024.0 / kpf;// / REP_COUNT;
  tzu = tzu / 1024.0 / kzu;// / REP_COUNT;

  if (tcc > VCC_MIN) vcc = tcc;
  vin = tin;
  vdc = tdc;
  vbt = tbt;
  vba = tba;
  vpf = tpf;
  vzu = tzu;  
  
  // Get currents
  abt = (analogRead(ABT_PIN) - 512) * 0.0264;
  //if (abt < 0) abt *= -1;
  aba = (analogRead(ABA_PIN) - 512) * 0.0264;
  //if (aba < 0) aba *= -1;
  ain = (analogRead(AIN_PIN) - 512) * 0.0264;
  //if (ain < 0) ain *= -1;
  ahx = (analogRead(AHX_PIN) - 512) * 0.0264;
  //if (ahx < 0) ahx *= -1;

  if (DEBUG) {
    Serial.print("Vcc: ");
    Serial.println(vcc);
    Serial.print("Vin: ");
    Serial.println(vin);
    Serial.print("Vdc: ");
    Serial.println(vdc);
    Serial.print("Vbt: ");
    Serial.println(vbt);
    Serial.print("Vba: ");
    Serial.println(vba);
    Serial.print("Vpf: ");
    Serial.println(vpf);
    Serial.print("Vzu: ");
    Serial.println(vzu);
    Serial.println("---");
  }
  
  // Vin check
  if (vin >= VIN_MIN && vin <= VIN_MAX) {
    if (! vin_ok) vin_count++;
  }
  else {
    vin_count = 0;
    vin_ok = false;
  }
  
  if (vin_count >= V_CHECK_CYCLES) vin_ok = true;
  
  // Vdc check
  if (vdc >= VDC_MIN && vdc <= VDC_MAX) {
    if (! vdc_ok) vdc_count++;
  }
  else {
    vdc_count = 0;
    vdc_ok = false;
  }
  
  if (vdc_count >= V_CHECK_CYCLES) vdc_ok = true;
  
  // Vbt check
  if (vbt >= VBT_MIN && vbt <= VBT_MAX) {
    if (! vbt_ok) vbt_count++;
  }
  else {
    vbt_count = 0;
    vbt_ok = false;
  }
  
  if (vbt_count >= V_CHECK_CYCLES) vbt_ok = true;
  
  // Vba check
  if (vba >= VBA_MIN && vba <= VBA_MAX) {
    if (! vba_ok) vba_count++;
  }
  else {
    vba_count = 0;
    vba_ok = false;
  }
  
  if (vba_count >= V_CHECK_CYCLES) vba_ok = true;

  // Vpf check
  if (vpf >= VPF_MIN && vpf <= VPF_MAX) {
    if (! vpf_ok) vpf_count++;
  }
  else {
    vpf_count = 0;
    vpf_ok = false;
  }
  
  if (vpf_count >= V_CHECK_CYCLES) vpf_ok = true;

  // Vzu check
  if (vzu >= VZU_MIN && vzu <= VZU_MAX) {
    if (! vzu_ok) vzu_count++;
  }
  else {
    vzu_count = 0;
    vzu_ok = false;
  }
  
  if (vzu_count >= V_CHECK_CYCLES) vzu_ok = true;
  
  // Vcc check
  if (vcc >= VCC_MIN && vcc <= VCC_MAX) {
    if (! vcc_ok) vcc_count++;
  }
  else {
    vcc_count = 0;
    vcc_ok = false;
  }
  
  if (vcc_count >= V_CHECK_CYCLES) vcc_ok = true;

  // Abt check
  if (abt >= ABT_MIN && abt <= ABT_MAX) {
    if (! abt_ok) abt_count++;
  }
  else {
    abt_count = 0;
    abt_ok = false;
  }
  
  if (abt_count >= V_CHECK_CYCLES) abt_ok = true;

  // Aba check
  if (aba >= ABA_MIN && aba <= ABA_MAX) {
    if (! aba_ok) aba_count++;
  }
  else {
    aba_count = 0;
    aba_ok = false;
  }
  
  if (aba_count >= V_CHECK_CYCLES) aba_ok = true;

  // Ain check
  if (ain >= AIN_MIN && ain <= AIN_MAX) {
    if (! ain_ok) ain_count++;
  }
  else {
    ain_count = 0;
    ain_ok = false;
  }
  
  if (ain_count >= V_CHECK_CYCLES) ain_ok = true;

  // Ahx check
  if (ahx >= AHX_MIN && ahx <= AHX_MAX) {
    if (! ahx_ok) ahx_count++;
  }
  else {
    ahx_count = 0;
    ahx_ok = false;
  }
  
  if (ahx_count >= V_CHECK_CYCLES) ahx_ok = true;
  
//  // BA full?
//  if (vba >= VBA_FULL) {
//    if (! ba_full && ! ba_enabled) ba_full_count++;
//  }
//  else {
//    ba_full_count = 0;
//    //ba_full = false;
//  }
//
//  if (ba_full_count >= BA_FULL_CHECK_CYCLES) {
//    ba_full = true;
//  }
//
//  if (ba_enabled && ! in_plugged()) {
//    ba_full = false;
//  }

//  // Auto disable EX
//  if (! vdc_ok) { // || ((! vin_ok || ! in_plugged()) && ! ex_requested)) {
//    disableEX();
//  }
//
//  // Disable EX block
//  if (ex_blocked && ex_time + EX_BLOCK_TIMEOUT < millis()) {
//    ex_blocked = false;
//  }
//  
//  // Auto enable EX
//  if (vdc_ok && ! ex_blocked) {
//    enableEX();
//  }
//    
//  // Auto enable BA (for charge)
//  if (! ba_enabled && vin_ok && in_plugged() && ! ba_full && ! ba_blocked) {
//    enableBA();
//  }
//
//  // Auto disable BA (if charge complete)
//  if (ba_enabled && ba_full && in_plugged()) { //(ba_full || ! in_plugged())) {
//    disableBA();
//  }
//
//  // Auto enable BA (for support)
//  if (! ba_enabled && ! ba_blocked && (! vin_ok || ! in_plugged())) {
//    enableBA();
//  }
//
//  // Auto disable BA (if discharged)
//  if (ba_enabled && ! in_plugged() && ! vba_ok) {
//    disableBA();
//  }
//
//  // Disable BA block
//  if (ba_blocked && ba_time + BA_BLOCK_TIMEOUT < millis()) {
//    ba_blocked = false;
//  }

  // Compile status
  status_1 = 0x00;
  if (vcc_ok)       status_1 |= 0x01;
  if (vin_ok)       status_1 |= 0x02;
  if (vdc_ok)       status_1 |= 0x04;
  if (vbt_ok)       status_1 |= 0x08;
  if (vba_ok)       status_1 |= 0x10;
  if (bt_enabled)   status_1 |= 0x20;
  if (ba_enabled)   status_1 |= 0x40;
  if (in_plugged()) status_1 |= 0x80;
  
  status_2 = 0x00;
  if (bt_blocked) status_2 |= 0x01;
  if (ba_blocked) status_2 |= 0x02;
  if (bt_full)    status_2 |= 0x04;
  if (ba_full)    status_2 |= 0x08;
  if (vpf_ok)     status_2 |= 0x10;
  if (vzu_ok)     status_2 |= 0x20;
  if (mv_enabled) status_2 |= 0x40;
  if (us_enabled) status_2 |= 0x80;

  status_3 = 0x00;
  if (abt_ok)     status_3 |= 0x01;
  if (aba_ok)     status_3 |= 0x02;
  if (ain_ok)     status_3 |= 0x04;
  if (ahx_ok)     status_3 |= 0x08;
}

/****************************************************************************
 * Функции
 ****************************************************************************/

float readvcc() {
  byte i;
  float result = 0.0;
  float tmp = 0.0;  

  //for (i = 0; i < REP_COUNT; i++) {
    // Read 1.1V reference against Avcc
    // set the reference to vcc and the measurement to the internal 1.1V reference
    #if defined(__AVR_ATmega32U4__) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
        ADMUX = _BV(REFS0) | _BV(MUX4) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
    #elif defined (__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__)
        ADMUX = _BV(MUX5) | _BV(MUX0);
    #elif defined (__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__)
        ADMUX = _BV(MUX3) | _BV(MUX2);
    #else
        // works on an Arduino 168 or 328
        ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
    #endif

    delay(3); // Wait for Vref to settle
    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

    tmp = (high<<8) | low;
    tmp = (typVbg * 1023.0) / tmp;
    //result = result + tmp;
  //  delay(5);
  //}

  //result = result / REP_COUNT;
  return tmp;//result;
}

// Callback for received data
void receiveData(int byteCount) {  
  
  while(Wire.available()) {
    // Get command
    ext = Wire.read();
    if (ext == EXT_COM && byteCount == 3) {
      cmd = 0x00;
      sum = 0x00;
      
      if (Wire.available()) sum = Wire.read();
      if (Wire.available()) cmd = Wire.read();
      
      if ((cmd ^ XOR_SEQ) != sum) {
        autoResponse = CSE_RSP;
        return;
      }
    }
    else {
      autoResponse = ERR_RSP;
      while(Wire.available()) {
        ext = Wire.read();
      }
      return;
    }
    
    // Process command
    switch (cmd) {
      case COM_GET_VIN:
        sendFloat(vin);
        break;
      case COM_GET_VDC:
        sendFloat(vdc);
        break;
      case COM_GET_VBT:
        sendFloat(vbt);
        break;
      case COM_GET_VBA:
        sendFloat(vba);
        break;
      case COM_GET_VPF:
        sendFloat(vpf);
        break;
      case COM_GET_VZU:
        sendFloat(vzu);
        break;
      case COM_GET_VCC:
        sendFloat(vcc);
        break;

      case COM_GET_ABT:
        sendFloat(abt);
        break;
      case COM_GET_ABA:
        sendFloat(aba);
        break;
      case COM_GET_AIN:
        sendFloat(ain);
        break;
      case COM_GET_AHX:
        sendFloat(ahx);
        break;
          
      case COM_GET_TEMP:
        sendFloat(getInternalTemp());
        break;
      case COM_GET_STAT1:
        sendByte(status_1);
        break;
      case COM_GET_STAT2:
        sendByte(status_2);
        break;     
      case COM_GET_STAT3:
        sendByte(status_3);
        break; 
        
      case COM_ENABLE_BA:
        commandResponse(enableBA());
        break;
      case COM_DISABLE_BA:
        commandResponse(disableBA());
        break;
        
      case COM_ENABLE_BT:        
        commandResponse(enableBT());
        break;
      case COM_DISABLE_BT:
        commandResponse(disableBT());
        break;
      
      case COM_ENABLE_MV:
        enableMV();
        commandResponse();
        break;
        
      case COM_DISABLE_MV:
        disableMV();
        commandResponse();
        break;

      case COM_ENABLE_US:
        enableUS();
        commandResponse();
        break;
        
      case COM_DISABLE_US:
        disableUS();
        commandResponse();
        break;

      case COM_RESET_US:
        resetUS();
        commandResponse();
        break;
                    
      default:
        autoResponse = ERR_RSP;
        break;
    }
  }
}

void commandResponse() {
  lastCommandTime = millis();
  autoResponse = OK_RSP;
}

void commandResponse(byte response) {
  lastCommandTime = millis();
  autoResponse = response;  
}

void sendByte(byte value) {
  lastCommandTime = millis();
  byteResponse = value;
  needSumm = false;
  needByte = true;
}

void sendFloat(float value) {
  lastCommandTime = millis();
  floatResponse = value;
  floatByte = 0;
  needFloat = true;
}

byte cSum(byte value) {
  return value ^ XOR_SEQ; 
}

byte cSum(byte *data, byte dataSize) {
  byte tmp = 0x00;
  for (byte i = 0; i < dataSize; i++) {
    tmp = tmp ^ data[i];
  }
  return tmp ^ XOR_SEQ; 
}

void answer() {
  // Want float value?
  if (needFloat) {
    // Get access to the float as a byte-array:
    byte *data = (byte *) &floatResponse;
    
    if (floatByte < sizeof(floatResponse)) {
      // Send byte
      Wire.write(data[floatByte]);
      floatByte++;
    }
    else {
      // Send control sum
      Wire.write(cSum(data, sizeof(floatResponse)));
      needFloat = false;
    }
  }
  else {
    // Want byte value?
    if (needByte) {
      if (!needSumm) {
        // Send byte
        Wire.write(byteResponse);
        needSumm = true;
      }
      else {
        // Send control sum
        Wire.write(cSum(byteResponse));
        needSumm = false;
        needByte = false;
      }
    }
    else {
      // Want something else?
      Wire.write(autoResponse);
    }
  }
  // Nothing more to send
  autoResponse = NO_RSP;
}

byte enableBT() {
  if (vbt_ok) {
    digitalWrite(R_BT, LOW);
    bt_enabled = true;
    bt_blocked = false;
      
    return OK_RSP;
  }
  else return BLK_RSP;
}

byte disableBT() {
  if ((vin_ok && in_plugged()) || (vba_ok && ba_enabled)) {
    digitalWrite(R_BT, HIGH);
    bt_enabled = false;
    bt_time = millis();
    bt_blocked = true;
  
    return OK_RSP;
  }
  else return BLK_RSP;
}

byte enableBA() {
  if (vba_ok) {
    digitalWrite(R_BA, HIGH);
    ba_enabled = true;
    ba_blocked = false;

    return OK_RSP;
  }
  else return BLK_RSP;
}

byte disableBA() {
  if ((vin_ok && in_plugged()) || (vbt_ok && bt_enabled)) {
    digitalWrite(R_BA, LOW);
    ba_enabled = false;
    ba_time = millis();
    ba_blocked = true;

    return OK_RSP;
  }
  else return BLK_RSP;
}

void enableMV() {
  digitalWrite(R_MV, HIGH);
  mv_enabled = true;
}

void disableMV() {
  digitalWrite(R_MV, LOW);
  mv_enabled = false;
}

void enableUS() {
  digitalWrite(R_US, LOW);
  us_enabled = true;
}

void disableUS() {
  digitalWrite(R_US, HIGH);
  us_enabled = false;
}

void resetUS() {
  disableUS();
  delay(500);
  enableUS();
}

// Get the internal temperature
float getInternalTemp() {
  unsigned int wADC;
  float t;
  ADMUX = (_BV(REFS1) | _BV(REFS0) | _BV(MUX3));
  ADCSRA |= _BV(ADEN); // enable the ADC
  delay(20); // wait for voltages to become stable.
  ADCSRA |= _BV(ADSC); // Start the ADC
  while (bit_is_set(ADCSRA,ADSC));
  wADC = ADCW;
  t = (wADC - 324.31 ) / 1.22;
  return(t);
}

bool in_plugged() {
  if (digitalRead(INC_PIN) == HIGH) {
    return true;
  } else {
    return false;
  }
}