//Compiled in Arduino IDE v1.8.10

#include "SPI.h"
#include "Wire.h"
#include "Adafruit_GFX.h"
#include "Adafruit_SSD1306.h"

Adafruit_SSD1306 oledDisplay = Adafruit_SSD1306(128, 32, &Wire);

#define BUTTON_A 9
#define BUTTON_B 6
#define BUTTON_C 5

const int pinFETgate = 10;
const int pinVBATplus = A4;
const int pinVLOADplus = A3;
const int pinVLOADminus = A2;
const int SAMPLE_SIZE = 5;

//default AREF = 3.3vdc
const float actualAREF = 3.306; //measured
const float VINcal = 13.000;  //calibration voltage value input
const float actualADCcount = 4073.0;  //ADC count value recorded at "VINcal" value
const float R2 = 10e3;  //10K ohms for R2 of input voltage divider

const float maxADCcount = 4095.0; //ADC count computed
const float voltsPerCount = actualAREF/maxADCcount;
const float Xcal = voltsPerCount*(actualADCcount/VINcal);
const float R1 = (R2*Xcal)/(1-Xcal); //calculated R1 for calibration adjustment
const float vScaleInput = R1/(R1+R2);
const float vScale = voltsPerCount/vScaleInput;

float VBAToc;
float SOC12v;
float RBATint;
float RBATintrms;

int countVBAToc;
int countVBATloaded;
int countVLOADplus;
int countVLOADminus;
int count;

/** BATTERY INTERNAL RESISTENCE CALCULATION DERIVATION
Rint = (VBAToc - VBATloaded) / Iload
Iload = (VLOADplus - VLOADminus) / Rload
Where Rload = 1 and using A/D counts becomes:
Rint = (countVBAToc - countVBATloaded) / (countVLOADplus - countVLOADminus)

**/
void setup() {
//set ADC for 12-bit (4095 counts) resolution
  analogReadResolution(12); 

//set ADC reference to 3.3v (NOTE: Feather M4 has 3.3V jumpered to 'AREF' pin)
  analogReference(AR_EXTERNAL); 

//ensure FET is OFF
  digitalWrite(pinFETgate,LOW);
  pinMode(pinFETgate,OUTPUT);

//enable button pin pull-up resistor
  pinMode(BUTTON_A, INPUT_PULLUP);
  pinMode(BUTTON_B, INPUT_PULLUP);
  pinMode(BUTTON_C, INPUT_PULLUP);

/*delay to ensure the display is powered up before trying to communicate otherwise
nothing will be displayed until you manually press reset; there seems to not be any 'display.ready' signal
NOTE: 50ms delay corrected the issue for 9 out of 10 power-on/-off cycles; 100ms seems solid, so far*/
  delay(100);   
 
// SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
oledDisplay.begin(SSD1306_SWITCHCAPVCC, 0x3C); // Address 0x3C for 128x32

// Clear the display buffer contents
  oledDisplay.clearDisplay();

//config/display constant text
  oledDisplay.setTextSize(1);
  oledDisplay.setCursor(0,0);
  oledDisplay.setTextColor(WHITE,BLACK); //BLACK sets background color otherwise is transparent; a trick to overwrite old data
  oledDisplay.println(F("Vbat = "));
  oledDisplay.println(F("Rint = "));
  oledDisplay.println(F("SOC = "));
  oledDisplay.println(F("SOH = ")); 
 // oledDisplay.println(F("CCA = "));   

//refresh display
  oledDisplay.display();
}

void loop() {
   
//while no buttons pressed
  while (digitalRead(BUTTON_B)) {
  //refresh display
      oledDisplay.display();
   
  //get battery open circuit voltage
    countVBAToc = analogRead(pinVBATplus);
 
  //display open circuit battery voltage value
    VBAToc = countVBAToc * vScale;
    oledDisplay.setCursor(40,0);
    oledDisplay.print(VBAToc,2); oledDisplay.print(F(" Vdc  "));

  //calculate 12v battery State Of Charge (SOC)
    if (VBAToc >= 12.78 ){
      SOC12v = 100.0;
    }
    else if (VBAToc >= 12.66 ){
      SOC12v = fmap(VBAToc,12.66,12.78,90.0,100.0);
    }
    else if (VBAToc >= 12.54 ){
      SOC12v = fmap(VBAToc,12.54,12.66,80.0,90.0);
    }
    else if (VBAToc >= 12.48 ){
      SOC12v = fmap(VBAToc,12.48,12.54,75.0,80.0);
    }
    else if (VBAToc >= 12.42 ){
      SOC12v = fmap(VBAToc,12.42,12.48,70.0,75.0);
    }
    else if (VBAToc >= 12.30 ){
      SOC12v = fmap(VBAToc,12.30,12.42,60.0,70.0);
    }
    else if (VBAToc >= 12.18 ){
      SOC12v = fmap(VBAToc,12.18,12.30,50.0,60.0);
    }
    else if (VBAToc >= 12.06 ){
      SOC12v = fmap(VBAToc,12.06,12.18,40.0,50.0);
    }
    else if (VBAToc >= 11.94 ){
      SOC12v = fmap(VBAToc,11.94,12.06,30.0,40.0);
    }
    else if (VBAToc >= 11.90 ){
      SOC12v = fmap(VBAToc,11.90,11.94,25.0,30.0);
    }
    else if (VBAToc >= 11.82 ){
      SOC12v = fmap(VBAToc,11.82,11.90,20.0,25.0);
    }
    else if (VBAToc >= 11.70 ){
      SOC12v = fmap(VBAToc,11.70,11.82,10.0,20.0);
    }                   
    else if (VBAToc >= 11.58 ){
      SOC12v = fmap(VBAToc,11.58,11.70,0.0,10.0);
    }                   
    else if (VBAToc < 11.58 ){
      SOC12v = 0.0;
    }                   

  //display battery state of charge
    oledDisplay.setCursor(40,16);
    oledDisplay.print(SOC12v,0); oledDisplay.print(F(" %     "));
  }

//calculate battery internal resistance and sum of squares
  RBATintrms=0.0;
  count=0;

  while (++count <= SAMPLE_SIZE){
  //get battery open circuit voltage
    countVBAToc = analogRead(pinVBATplus);
       
  //get analog values under load
  digitalWrite(pinFETgate,HIGH);
    delayMicroseconds(5);
    countVBATloaded = analogRead(pinVBATplus);
    countVLOADplus = analogRead(pinVLOADplus);
    countVLOADminus = analogRead(pinVLOADminus);
  digitalWrite(pinFETgate,LOW); 

  //calculate battery internal resistance and sum of squares
    RBATint = float(countVBAToc - countVBATloaded)/float(countVLOADplus - countVLOADminus);
    RBATintrms += sq(RBATint);
    delayMicroseconds(50);
  }

//display battery internal resistance rms value
  RBATintrms = sqrt(RBATintrms/SAMPLE_SIZE);
  oledDisplay.setCursor(40,8);
  oledDisplay.print(RBATintrms,4); oledDisplay.print(F(" Ohms   "));

//500mS wait time to repeat load test - stay within thermal response, energy pulse limits of load resistor
    delay(500);
}

float fmap(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}