#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
// The pins for I2C are defined by the Wire-library. 
// On an arduino UNO:       A4(SDA), A5(SCL)
// On an arduino MEGA 2560: 20(SDA), 21(SCL)
// On an arduino LEONARDO:   2(SDA),  3(SCL), ...
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3D ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

float R1 = 120000;    // !! resistance of R1 !! (voltage divider);
float R2 = 6800.0;    // !! resistance of R2 !! (Voltage divider);
float maxV = 740;     //sets the voltage rating of the caps
float cycle = 0;      //set the number of times for charge/discharge cycle

float volts = 0;
float voltage = 0;
int chargepin = (2);    //arduino pin for charger relay input;
int dischargepin = (4); //arduino pin to discharge resistor relay;
int state = 0;
int oldstate = 0;
unsigned long hold = 0;


void setup() {
  
Serial.begin(9600);
// SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
Serial.println(F("SSD1306 allocation failed"));
  for(;;); // Don't proceed, loop forever
  }
display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
display.clearDisplay();
pinMode(chargepin,OUTPUT);
pinMode(dischargepin,OUTPUT);
digitalWrite(2,LOW);
digitalWrite(4,LOW);
analogRead(A0);
delay(2000);
display.clearDisplay();
display.fillRect(0, 0, 128, 15, SSD1306_INVERSE);
display.setTextSize(1);             
display.setTextColor(SSD1306_INVERSE); 
display.setCursor(3,4);
display.println(F("Capacitor Forming"));
}

void printVolts() {
  if (volts > -1) {
    display.setCursor(0,45);
    display.println(F("Cap:"));
    display.setCursor(24,45);
    display.setTextColor(WHITE, BLACK); // clearing the text field
    display.println(F("   "));
    display.setCursor(24,45);
    display.setTextColor(WHITE, BLACK); // clearing the text field
    display.println(volts,0);
    display.setCursor(45,45);
    display.println(F("Vdc"));
    display.display();
  }
}

void printCycle(){
  if (cycle > -1) {
    display.setCursor(0,55);
    display.println(F("Cycle:"));
    display.setCursor(36,55);
    display.setTextColor(WHITE, BLACK); // clearing the text field
    display.println(cycle,0);
    display.setCursor(50,55);
    display.println(F("x"));
    display.display();
  }
}

void printState(){ 

if (state != oldstate){
  oldstate = state;     
  switch(state){

    case 1:
      display.setCursor(0,20);
      display.setTextColor(WHITE, BLACK);  // clearing the text field
      display.println(F("Charging....       "));
      display.display();
      break;

    case 2:
      display.setCursor(0,20);
      display.setTextColor(WHITE, BLACK);  // clearing the text field
      display.println(F("Discharging....    "));
      display.display();
      break;

    case 3:
      display.setCursor(0,20);
      display.setTextColor(WHITE, BLACK);  // clearing the text field
      display.println(F("OVERVOLTAGE!       "));
      display.setCursor(0,30);
      display.setTextColor(WHITE, BLACK);  // clearing the text field
      display.println(F("Emergency Discharge"));
      display.display();
      delay(1000);
      break;

    case 4:
      display.setCursor(0,20);
      display.setTextColor(WHITE, BLACK);  // clearing the text field
      display.println(F("Wait till Caps     "));
      display.setCursor(0,30);
      display.setTextColor(WHITE, BLACK);  // clearing the text field
      display.println(F("are discharged     "));
      break;

    case 5:
      display.setCursor(0,20);
      display.setTextColor(WHITE, BLACK);  // clearing the text field
      display.println(F("Save to remove  "));
      display.setCursor(0,30);
      display.setTextColor(WHITE, BLACK);  // clearing the text field
      display.println(F("the Capacitors  "));
      break;

    default:
      display.setCursor(2,20);
      display.setTextColor(WHITE, BLACK); // clearing the text field 
      display.println(F("Check System for Faults..."));
      display.display();
      break;
    }
  }
}

void loop() {
    
printVolts();
printCycle();
printState();   
volts = analogRead(A0);
volts = (volts * 5) / 1024;
volts = volts / (R2/(R1+R2))*10;  

 if (cycle < 10) 
  {

      if (millis() < hold + 900ul){
        if (volts < maxV){
          delay(100);
          digitalWrite(dischargepin, LOW);
          delay(100);
          digitalWrite(chargepin, HIGH);
 
          state = 1;
          }

        else {
          if (volts > maxV){
          digitalWrite(chargepin, LOW);
          delay(100);
          digitalWrite(dischargepin, HIGH);
          state = 3;
          cycle = 10;
          }
        }
      }

      else {
        delay(100);
        digitalWrite(chargepin, LOW);
        delay(100);
        digitalWrite(dischargepin, HIGH);
        state = 2;
        
        if (millis() > hold + 1800ul){
          hold = millis();
          cycle ++;

        }
      }
    }
  
  else {
    digitalWrite(chargepin, LOW);
    delay(100);
    digitalWrite(dischargepin, HIGH);
      if (volts < 3) 
      {
        delay(100);
        state = 5;
      }  

      else 
      {
        delay(100);
        state = 4;
      }
  }
}