/////////////////////////////////////////
// HXbot HXpower firmware               /
// 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 ADC_PIN A8
#define AHX_PIN A7
#define ABA_PIN A6

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

#define R_BL 11
#define R_PF 5
#define R_HX 1
#define R_IS 7
#define RED_PIN 10
#define GRN_PIN 12
#define BTN_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_VIS 0x05
#define COM_GET_VZU 0x06
#define COM_GET_VCC 0x07

// AMPERMETERS
#define COM_GET_AHX 0x21
#define COM_GET_ABA 0x22
#define COM_GET_AIN 0x23
#define COM_GET_ADC 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_POWER_ON_BL  0x11  // 17
#define COM_POWER_ON_PF  0x12  // 18
#define COM_SWITCH_TO_IS 0x13  // 19
#define COM_SWITCH_TO_IN 0x14  // 20

#define COM_SHUTDOWN_BL  0x15  // 21
#define COM_SHUTDOWN_PF  0x16  // 22
#define COM_SWITCH_TO_LR 0x17  // 23
#define COM_SWITCH_TO_BA 0x18  // 24

#define COM_ENABLE_PF  0x1B  // 27
#define COM_DISABLE_PF 0x1C  // 28

#define COM_ENABLE_BL  0x1D  // 29
#define COM_DISABLE_BL 0x1E  // 30

#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 DELAY_TIME 0
#define BLINK_DELAY 300

#define BA_TIMEOUT 5000
#define BA_BLOCK_TIMEOUT 30000
#define BA_FULL_CHECK_CYCLES 20
#define V_CHECK_CYCLES 10
#define MAX_VBT_FAIL 15
#define AREF 5

#define VIN_MIN 10.0
#define VIN_MAX 14.0

#define VDC_MIN 4.8
#define VDC_MAX 5.5

#define VBT_MIN 3.65
#define VBT_MAX 4.25

#define VBA_MIN 6.0
#define VBA_MAX 8.6

#define VIS_MIN 4.8
#define VIS_MAX 5.5

#define VZU_MIN 5.0
#define VZU_MAX 6.3

#define VCC_MIN 4.3
#define VCC_MAX 5.3

#define AHX_MIN 0.1
#define AHX_MAX 5.0

#define ABA_MIN 0.1
#define ABA_MAX 5.0

#define AIN_MIN 0.1
#define AIN_MAX 5.0

#define ADC_MIN 0.1
#define ADC_MAX 5.0

// VAR
// резисторы делителя напряжения
const float r1_1 = 20000;  // 20K
const float r1_2 = 10000;  // 10K
const float r2_1 = 10380;  // 10K
const float r2_2 = 10000;  // 10K
const float r3_1 = 10000;  // 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, kis, kzu;
float vin, vdc, vbt, vba, vis, vzu;
float tin, tdc, tbt, tba, tis, tzu;
float ahx, aba, ain, adc;
float uhx, 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;

byte ba_count = 0;
unsigned long ba_time;
boolean ba_blocked = false;
byte ba_full_count = 0;
boolean ba_charge_blocked = false;

boolean is_requested = false;

unsigned long button_time;
//byte button_count = 0;

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 vis_ok = false;
byte vis_count = 0;

boolean vzu_ok = false;
byte vzu_count = 0;

boolean vcc_ok = false;
byte vcc_count = 0;

boolean ahx_ok = false;
byte ahx_count = 0;

boolean aba_ok = false;
byte aba_count = 0;

boolean ain_ok = false;
byte ain_count = 0;

boolean adc_ok = false;
byte adc_count = 0;

boolean ba_enabled = true;
boolean lr_enabled = true;
boolean pf_enabled = false;
boolean bl_enabled = false;

boolean bl_powered = false;
boolean bl_error = false;
boolean ba_full = false;
boolean ba_charge = false;

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

// Blink detection
  // 0 - LOW
  // 1 - HIGH
  // 2 - BLINK
byte red_state = 0, grn_state = 0;
byte red_array[3] = {0, 0, 0}, grn_array[3] = {0, 0, 0};
byte blink_i;
unsigned long blink_time = 0;

// Loop delay
//int delay_time = DELAY_TIME;

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

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);
  kis = 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_BL, OUTPUT);
  pinMode(R_PF, OUTPUT);
  pinMode(R_HX, OUTPUT);
  pinMode(R_IS, OUTPUT);
  pinMode(R_IS, OUTPUT);
  
  pinMode(VIN_PIN, INPUT);
  pinMode(VDC_PIN, INPUT);
  pinMode(VBT_PIN, INPUT);
  pinMode(VBA_PIN, INPUT);
  pinMode(VIS_PIN, INPUT);
  pinMode(VZU_PIN, INPUT);
  
  pinMode(AHX_PIN, INPUT);
  pinMode(ABA_PIN, INPUT);
  pinMode(AIN_PIN, INPUT);
  pinMode(ADC_PIN, INPUT);

  pinMode(BTN_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(RED_PIN, INPUT);
  pinMode(GRN_PIN, INPUT);
    
  ba_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;
  tis = 0.0;
  tzu = 0.0;
  
  //for (byte i = 0; i < REP_COUNT; i++) {
    tin += readvcc() * analogRead(VIN_PIN);
    tdc += readvcc() * analogRead(VDC_PIN);
    tbt += readvcc() * analogRead(VBT_PIN);
    tba += readvcc() * analogRead(VBA_PIN);
    tis += readvcc() * analogRead(VIS_PIN);
    tzu += readvcc() * analogRead(VZU_PIN);
    //delay(10);
  //}

  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;
  tis = tis / 1024.0 / kis;// / REP_COUNT;
  tzu = tzu / 1024.0 / kzu;// / REP_COUNT;

  if (tcc > VCC_MIN) vcc = tcc;
  vin = tin;
  vdc = tdc;
  vbt = tbt;
  if (ba_enabled) vba = tba;
  vis = tis;
  vzu = tzu;  
  
  // Get currents
  ahx = (analogRead(AHX_PIN) - 512) * 0.0264;
  if (ahx < 0) ahx *= -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;
  adc = (analogRead(ADC_PIN) - 512) * 0.0264;
  if (adc < 0) adc *= -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("Vis: ");
    Serial.println(vis);
    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 (ba_enabled) {
    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;
  }
  else {
    vba_ok = false;
  }

  // Vis check
  if (vis >= VIS_MIN && vis <= VIS_MAX) {
    if (! vis_ok) vis_count++;
  }
  else {
    vis_count = 0;
    vis_ok = false;
  }
  
  if (vis_count >= V_CHECK_CYCLES) vis_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;

  // 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;

  // 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;

  // Adc check
  if (adc >= ADC_MIN && adc <= ADC_MAX) {
    if (! adc_ok) adc_count++;
  }
  else {
    adc_count = 0;
    adc_ok = false;
  }
  
  if (adc_count >= V_CHECK_CYCLES) adc_ok = true;
  
  // Auto disable IN
  if (! vin_ok || ! in_connected()) {
    switchToBA();
  }

  // Auto disable IS
  if (! vis_ok || ((! vin_ok || ! in_connected()) && ! is_requested)) {
    switchToLR();
  }
    
  // BL status  
  bl_powered = false;
  bl_error = false;
  if (! ba_charge_blocked) ba_full = false;
  ba_charge = false;

  if (bl_enabled) {
    detectBlink();
    
    switch (grn_state) {
      case 0:
        ba_full = false;
        ba_charge = false;
        break;
      case 1:
        ba_full = true;
        ba_charge = false;
        break;
      case 2:
        ba_full = false;
        ba_charge = true;
        break;
    }
    
    switch (red_state) {
      case 0:
        bl_powered = false;
        bl_error = false;
        break;
      case 1:
        bl_powered = true;
        bl_error = false;
        break;
      case 2:
        bl_powered = false;
        bl_error = true;
        break;    
    }

    if (ba_full) {
      if (! ba_charge_blocked) ba_full_count++;
    }
    else {
      ba_full_count = 0;
    }

    if (ba_full_count >= BA_FULL_CHECK_CYCLES) ba_charge_blocked = true;
  }

  // Button reaction
//  if (digitalRead(BTN_PIN) == LOW) {
//    button_time = millis();
//  }
//  else {
//    if (button_time + BUTTON_REACTION_TIME < millis()) {
//      switchToBA();        
//    }
//  }
  
  // Disable BA block
  if (ba_blocked && ba_time + BA_BLOCK_TIMEOUT < millis()) {
    ba_blocked = false;
  }
  
  // Auto switch to IN
  if (vin_ok && in_connected() && ba_enabled && ! ba_blocked && ba_time + BA_TIMEOUT < millis()) {
    switchToIN();
  }

  // Auto enable IS
  if (vis_ok && ! ba_enabled && vin_ok) {
    switchToIS();
  }
  
  // Auto enable BL
  if (! pf_enabled && ! ba_enabled && vin_ok && in_connected() && ! ba_charge_blocked) {
  //if (! pf_enabled && ! ba_enabled && vin_ok && ! ba_charge_blocked) {
    enableBL();
  }

  // Auto disable BL
  if ((bl_enabled && ba_charge_blocked) || ! in_connected()) {
    disableBL();
  }

  // Auto enable IS on BT fail
//  if (! vbt_ok) {
//    if (! vbt_fail) vbt_fail_count++;
//  }  
//  else {
//    vbt_fail_count = 0;
//    vbt_fail = false;
//  }

//  if (vbt_fail_count >= MAX_VBT_FAIL && vis_ok) switchToIS();

  // 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 (lr_enabled) status_1 |= 0x20;
  if (ba_enabled) status_1 |= 0x40;
  if (ba_blocked) status_1 |= 0x80;
  
  status_2 = 0x00;
  if (bl_powered) status_2 |= 0x01;
  if (bl_error)   status_2 |= 0x02;
  if (ba_full)    status_2 |= 0x04;
  if (ba_charge)  status_2 |= 0x08;
  if (vis_ok)     status_2 |= 0x10;
  if (vzu_ok)     status_2 |= 0x20;
  if (pf_enabled) status_2 |= 0x40;
  if (bl_enabled) status_2 |= 0x80;

  status_3 = 0x00;
  if (ahx_ok)     status_3 |= 0x01;
  if (aba_ok)     status_3 |= 0x02;
  if (ain_ok)     status_3 |= 0x04;
  if (adc_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_VIS:
        sendFloat(vis);
        break;
      case COM_GET_VZU:
        sendFloat(vzu);
        break;
      case COM_GET_VCC:
        sendFloat(vcc);
        break;

      case COM_GET_AHX:
        sendFloat(ahx);
        break;
      case COM_GET_ABA:
        sendFloat(aba);
        break;
      case COM_GET_AIN:
        sendFloat(ain);
        break;
      case COM_GET_ADC:
        sendFloat(adc);
        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_POWER_ON_BL:
        digitalWrite(R_BL, HIGH);
        bl_enabled = true;
        commandResponse();
        break;
      case COM_SHUTDOWN_BL:
        digitalWrite(R_BL, LOW);
        bl_enabled = false;
        commandResponse();
        break;
        
      case COM_POWER_ON_PF:
        digitalWrite(R_PF, HIGH);
        pf_enabled = true;
        commandResponse();
        break;
      case COM_SHUTDOWN_PF:
        digitalWrite(R_PF, LOW);
        pf_enabled = false;
        commandResponse();
        break;
      
      case COM_SWITCH_TO_IS:
        is_requested = true;
        commandResponse(switchToIS());
        break;
      case COM_SWITCH_TO_LR:
        is_requested = false;
        commandResponse(switchToLR());
        break;
      
      case COM_SWITCH_TO_IN:
        commandResponse(switchToIN());       
        break;
      case COM_SWITCH_TO_BA:
        commandResponse(switchToBA());
        break;
        
      case COM_ENABLE_PF:
        enablePF();
        commandResponse();
        break;
        
      case COM_DISABLE_PF:
        disablePF();
        commandResponse();
        break;
        
      case COM_ENABLE_BL:
        enableBL();
        commandResponse();
        break;
        
      case COM_DISABLE_BL:
        disableBL();
        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 switchToLR() {
  if (vbt_ok) {
    digitalWrite(R_HX, LOW);
    lr_enabled = true;
    
    return OK_RSP;
  }
  else return BLK_RSP;
}

byte switchToIS() {
  digitalWrite(R_HX, HIGH);
  lr_enabled = false;

  return OK_RSP;
}

byte switchToBA() {
  if (! ba_enabled) {

    // Disable BL
    if (bl_enabled) {
      digitalWrite(R_BL, LOW);
      bl_enabled = false;
      //delay(500);
    }
    
    digitalWrite(R_IS, LOW);
    ba_enabled = true;
    
    digitalWrite(LED_PIN, LOW);
    ba_time = millis();
    ba_blocked = true;
    ba_charge_blocked = false;
  }
  
  return OK_RSP;
}

byte switchToIN() {
  if (vin_ok && ba_enabled && in_connected()) {
    digitalWrite(R_IS, HIGH);
    ba_enabled = false;
    
    digitalWrite(LED_PIN, HIGH);
    
    return OK_RSP;
  }
  else return BLK_RSP;
}

void enablePF() {
  if (bl_enabled) {
    digitalWrite(R_BL, LOW);
    bl_enabled = false;
    //delay(500);
  }
  digitalWrite(R_PF, HIGH);
  pf_enabled = true;
  ba_charge_blocked = false;
}

void disablePF() {
  digitalWrite(R_PF, LOW);
  pf_enabled = false;
}

byte enableBL() {
  if (! ba_enabled && ! pf_enabled && in_connected()) {
    digitalWrite(R_BL, HIGH);
    bl_enabled = true;
    
    return OK_RSP;
  }
  else return BLK_RSP;
}

void disableBL() {
  digitalWrite(R_BL, LOW);
  bl_enabled = false;
}

// 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);
}

void detectBlink() {
    // 0 - LOW
    // 1 - HIGH
    // 2 - BLINK
  byte red_low_count = 0, grn_low_count = 0;
  byte red_high_count = 0, grn_high_count = 0;
  byte pin_level;

  if (millis() - blink_time < BLINK_DELAY) return;
  
  red_array[blink_i] = digitalRead(RED_PIN);
  grn_array[blink_i] = digitalRead(GRN_PIN);
  
  blink_i++;

  if (blink_i == 3) {    
    for (byte i = 0; i < 3; i++) {
      if (red_array[i] == HIGH) red_high_count++;
      if (red_array[i] == LOW) red_low_count++;
      if (grn_array[i] == HIGH) grn_high_count++;
      if (grn_array[i] == LOW) grn_low_count++;
    }
    
    if (red_low_count > 0 && red_high_count == 0)
      red_state = 0;
    else
      if (red_high_count > 0 && red_low_count == 0)
        red_state = 1;
      else
        red_state = 2;

    if (grn_low_count > 0 && grn_high_count == 0)
      grn_state = 0;
    else
      if (grn_high_count > 0 && grn_low_count == 0)
        grn_state = 1;
      else
        grn_state = 2;

    blink_i = 0;
  }
  
  blink_time = millis();
}

bool in_connected() {
  if (digitalRead(BTN_PIN) == HIGH) {
    return true;
  } else {
    return false;
  }
}