/////////////////////////////////////////
// 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 VIN_PIN 0
#define VDC_PIN 1
#define VBT_PIN 2
#define VBA_PIN 3

#define R_BL 7
#define R_PF 6
#define R_HX 5
#define R_IS 4
#define BL_RED_PIN 8
#define BL_GRN_PIN 9
#define BTN_PIN 2
#define LED_PIN 3

#define REP_COUNT 5

#define TIMEOUT 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_VCC 0x07

// TEMP
#define COM_GET_TEMP 0x06

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

// 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 900
#define BA_TIMEOUT 3000
#define BA_BLOCK_TIMEOUT 30000
#define BUTTON_CYCLES 4
#define V_CHECK_CYCLES 3

#define VIN_MIN 10.0
#define VIN_MAX 14.0

#define VDC_MIN 4.8
#define VDC_MAX 6.0

#define VBT_MIN 3.0
#define VBT_MAX 4.25

#define VBA_MIN 6.0
#define VBA_MAX 8.4

#define VCC_MIN 4.5
#define VCC_MAX 5.3

// VAR
// резисторы делителя напряжения
const float r1_1 = 200000;  // 200K
const float r1_2 = 100000;  // 100K
const float r2_1 = 100000;  // 100K
const float r2_2 = 100000;  // 100K
const float r3_1 = 100000;  // 100K
const float r3_2 = 100000;  // 100K
const float r4_1 = 200000;  // 200K
const float r4_2 = 100000;  // 100K

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

float vcc = 0.0;
float Vmax1, Vmax2, Vmax3, Vmax4;
float kin, kdc, kbt, kba;
float vin, vdc, vbt, vba;
float tin, tdc, tbt, tba;

//char *stime;

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 button_count = 0;

boolean vin_ok = false;
byte vin_count = 0;

boolean vdc_ok = false;
byte vdc_count = 0;

boolean vbt_ok = false;
byte vbt_count = 0;

boolean vba_ok = false;
byte vba_count = 0;

boolean vcc_ok = false;
byte vcc_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;

// Loop delay
int delay_time = DELAY_TIME;

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

void setup() {
  if (DEBUG) {
    Serial.begin(9600);
    Serial.println("---");
    delay(1000);
  }
  
  // определение опорного напряжения
  analogReference(DEFAULT);  // DEFAULT INTERNAL использовать vcc как AREF
  delay(100);
  
  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);
  
  vcc = readvcc();
  Vmax1 = vcc / kin;
  Vmax2 = vcc / kdc;
  Vmax3 = vcc / kbt;  
  Vmax4 = vcc / kba;

  if (DEBUG) {
    Serial.print("Vcc= ");
    Serial.println(vcc);
    Serial.print("Vmax1= ");
    Serial.println(Vmax1);
    Serial.print("Vmax2= ");
    Serial.println(Vmax2);
    Serial.print("Vmax3= ");
    Serial.println(Vmax3);
    Serial.print("Vmax4= ");
    Serial.println(Vmax4);
    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(BTN_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(BL_RED_PIN, INPUT);
  pinMode(BL_GRN_PIN, INPUT);
    
  ba_time = millis();
}

void loop() {
  // Timeout protection
  if ((lastCommandTime + TIMEOUT < millis())) {
    floatResponse = 0.0;
    floatByte = 0;
    needFloat = false;
    autoResponse = TMO_RSP;
  }
  
  //vcc = readvcc();
  
  // stime = TimeToString(millis()/1000);
  // считываем точное напряжение с A0, где будет находиться наш вольтметр с делителем напряжения
  tin = 0.0;
  tdc = 0.0;
  tbt = 0.0;
  tba = 0.0;
  
  for (byte i = 0; i < REP_COUNT; i++) {
    tin = tin * readvcc() + analogRead(VIN_PIN);
    tdc = tdc * readvcc() + analogRead(VDC_PIN);
    tbt = tbt * readvcc() + analogRead(VBT_PIN);
    tba = tba * readvcc() + analogRead(VBA_PIN);
    delay(10);
  }
  
  // tin = (tin * vcc) / 1024.0 / kin / REP_COUNT;
  // tdc = (tdc * vcc) / 1024.0 / kdc / REP_COUNT;
  // tbt = (tbt * vcc) / 1024.0 / kbt / REP_COUNT;
  // tba = (tba * vcc) / 1024.0 / kba / REP_COUNT;

  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;
  
  vin = tin;
  vdc = tdc;
  vbt = tbt;
  if (ba_enabled) vba = tba;

//  if (DEBUG && count >= 5) {
//    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("Vpf= ");
//    Serial.println(vpf); 
//    Serial.println("---");
//    count = 0;
//  }
//  else count++;
  
  // 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 = 0.0;
    vba_ok = false;    
  }
  
  // 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;
  
  // Auto switch to BA
  if (! vin_ok) {
    switchToBA();
  }
  
  // BL status
  bl_powered = false;
  bl_error = false;
  ba_full = false;
  ba_charge = false;
  
  if (vin_ok && bl_enabled) {
    delay_time = 0;

//    if (DEBUG) {
//      unsigned long tmp1, tmp2;
//      
//      tmp1 = millis();
//      while(digitalRead(BL_GRN_PIN) == LOW) {
//        
//      }
//      tmp2 = millis();
//      
//      Serial.print("time1: ");
//      Serial.println(tmp2 - tmp1);
//
//      tmp1 = millis();
//      while(digitalRead(BL_GRN_PIN) == HIGH) {
//        
//      }
//      tmp2 = millis();
//      Serial.print("time2: ");
//      Serial.println(tmp2 - tmp1);
//      
//    }

    switch (detectBlink(BL_GRN_PIN)) {
      case 0:
        ba_full = false;
        ba_charge = false;
        break;
      case 1:
        ba_full = true;
        ba_charge = false;
        bl_powered = true;
        break;
      case 2:
        ba_full = false;
        ba_charge = true;
        bl_powered = true;
        break;
    }

    if (! ba_charge && ! ba_full) {
      switch (detectBlink(BL_RED_PIN)) {
        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;
      }
    }
    
  } else delay_time = DELAY_TIME;

  // Button reaction
  if (digitalRead(BTN_PIN) == HIGH) {
    if (button_count >= BUTTON_CYCLES) {
      if (ba_enabled && vin_ok) {
        switchToIN();
      }
      else {
        switchToBA();        
        ba_blocked = true;
      }
      button_count = 0;
    }
    else button_count++;
  }
  else button_count = 0;
  
  // Disable BA block
  if (ba_blocked && ba_time + BA_BLOCK_TIMEOUT < millis()) {
    ba_blocked = false;
  }
  
  // Auto switch to IN
  if (vin_ok && ba_enabled && ! ba_blocked && ba_time + BA_TIMEOUT < millis()) {
    switchToIN();
  }
  
  // Auto enable BL
  if (! pf_enabled && ! ba_enabled && vin_ok) {
    enableBL();
  }

  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 (vpf_low)    status_2 |= 0x10;
//  if (vpf_high)   status_2 |= 0x20;
  if (pf_enabled)  status_2 |= 0x40;
  if (bl_enabled)  status_2 |= 0x80;
  
  // Delay
  delay(delay_time);
}

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

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 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_VCC:
        sendFloat(vcc);
        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_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:
        commandResponse(switchToIS());
        break;
      case COM_SWITCH_TO_LR:
        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;
  }
  
  return OK_RSP;
}

byte switchToIN() {
  if (vin_ok && ba_enabled) {
    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;
}

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

byte enableBL() {
  if (! ba_enabled && ! pf_enabled) {
    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);
}

byte detectBlink(byte pin) {
  // Return codes:
    // 0 - LOW
    // 1 - HIGH
    // 2 - BLINK
  byte low_count = 0;
  byte high_count = 0;
  byte i = 0;
  byte pin_level;
  
  for (i = 0; i < 3; i++) {
    pin_level = digitalRead(pin);
    if (pin_level == HIGH) high_count++;
    if (pin_level == LOW) low_count++;
    delay(300);
  }
  
  if (low_count > 0 && high_count == 0)
    return 0;
  else
    if (high_count > 0 && low_count == 0)
      return 1;
    else
      return 2;
}