#include <Arduino.h>
#include <Adafruit_SSD1306.h>
#include <Wire.h>
//#include <avr/pgmspace.h>
#include <ezButton.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

//Constants
const String displayVoltage = "3.3";

//Set volts and amps
float maxVolts = 0;
float minVolts = 0;
float maxAmps = 0;

//Define I/O
int analogInputs[]={A0, A1};  //Array of Analog Inputs
int analogOutputs[]={}; //Array of Analog Outputs
int digitalInputs[]={4,12,13};  //Array of Digital Inputs
int digitalOutputs[]={5,2,3};  //Array of Digital Outputs

//  Digital Inputs
//  4 = Button
//  12 = Voltage Selector Bit 1
//  13 = Voltage Selector Bit 2

//  Digital Outputs
//  5 = Relay Output
//  6 = Green LED
//  7 = Red LED

ezButton btn1(digitalInputs[0]);

//Timers and clocks
const unsigned long n250ms = 250;
const unsigned long n500ms = 500;
const unsigned long n1s = 1000;
const unsigned long n2s = 2000;
const unsigned long n5s = 5000;
const unsigned long n10s = 10000;

unsigned long T1PrevTime = 0;
unsigned long T2PrevTime = 0;

//Output states of Digital Outputs
int RedLedState = LOW;
int GreenLedState = LOW;
int outputRelay = LOW;

//Boolean values
bool faultActive = false;

void writeDataToScreen(float voltage, float amperage, int status){
  //Print voltage
  oled.fillRect(0,11,62,20,SSD1306_BLACK);
  oled.setTextColor(SSD1306_WHITE);
  oled.setCursor(0,15);
  oled.setTextSize(2);
  if(voltage > maxVolts){
    oled.setTextSize(1);
    oled.print(F("TOO HIGH"));
    oled.setTextSize(2);
  }else if(voltage < minVolts && outputRelay == HIGH){
    oled.setTextSize(1);
    oled.print(F("TOO LOW"));
    oled.setTextSize(2);
  }else{
    if(voltage < 10){
      oled.print(voltage);
    }else{
      oled.print(voltage,1);
    }
  }
  oled.setCursor(50,15);
  oled.print(F("V"));

  //Print Amps
  oled.fillRect(65,11,128,20,SSD1306_BLACK);
  oled.setTextColor(SSD1306_WHITE);
  oled.setCursor(66,15);
  oled.setTextSize(2);
  if(amperage > maxAmps){
    oled.setTextSize(1);
    oled.print(F("OVR CRNT"));
    oled.setTextSize(2);
  }else{
    if(amperage < 1){
      oled.setCursor(66,15);
      oled.print(amperage*1000,0);
      oled.setCursor(103,15);
      oled.print(F("mA"));
    }
    else if(amperage < 10){
      oled.setCursor(66,15);
      oled.print(amperage);
      oled.setCursor(115,15);
      oled.print(F("A"));
    }else{
      oled.setCursor(66,15);
      oled.print(amperage,1);
      oled.setCursor(115,15);
      oled.print(F("A"));
    }
  }
  //oled.setCursor(116,15);
  //oled.print(F("A"));

  //Print Watts
  float wattage = (amperage/voltage);
  oled.fillRect(0,34,62,63,SSD1306_BLACK);
  oled.setTextColor(SSD1306_WHITE);
  oled.setCursor(0,40);
  oled.setTextSize(2);
  if(wattage > 0){
    if(wattage < 10){
      oled.print(wattage);
    }else{
      oled.print(wattage,1);
    }
  }else{
    oled.print(F("0.00"));
  }
  oled.setCursor(50,40);
  oled.print(F("W"));

  
  oled.fillRect(SCREEN_WIDTH/2,SCREEN_HEIGHT/2,SCREEN_HEIGHT,SCREEN_WIDTH,SSD1306_WHITE);
  oled.setTextSize(3);
  oled.setTextColor(SSD1306_BLACK);
  if(status == HIGH){
    oled.setCursor(SCREEN_WIDTH/2+14,SCREEN_HEIGHT/2+6);
    oled.print(F("ON"));
  }else{
    oled.setCursor(SCREEN_WIDTH/2+6,SCREEN_HEIGHT/2+6);
    oled.print(F("OFF"));
  }
  

  //Display on screen
  oled.display();
}

float ScaleValues(float input, float minIn, float maxIn, float minOut, float maxOut) {
  float output;
  output = ((((maxOut-minOut)*(input-minIn))/(maxIn-minIn))+minOut);
  if(input > minIn && input < maxIn){
    return output;
  }else if(input <= minIn){
    return minOut;
  }else if(input >= maxIn){
    return maxOut;
  }
  
}

void setup() {
  Serial.begin(9600);
  oled.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
  
  for (int din = 0; din < (sizeof(digitalInputs)/sizeof(digitalInputs[0])); din++){
      pinMode(digitalInputs[din],INPUT_PULLUP);
    }

  for (int dout = 0; dout < (sizeof(digitalOutputs)/sizeof(digitalOutputs[0])); dout++){
      pinMode(digitalOutputs[dout],OUTPUT);
      digitalWrite(digitalOutputs[dout], LOW);
  }
  
  btn1.setDebounceTime(50);

  oled.clearDisplay();
  oled.fillRect(0,0,SCREEN_WIDTH,SCREEN_HEIGHT,SSD1306_WHITE);
  oled.setTextColor(SSD1306_BLACK);
  oled.setTextSize(3);
  oled.setCursor(32,4);
  delay(50);

  // Set the current voltage and amps. Selectable by jumper links/DIP switches
  if(digitalRead(digitalInputs[1]) == LOW && digitalRead(digitalInputs[2]) == HIGH){
    oled.setCursor(30,3);
    oled.print(F("3.3V"));
    maxVolts = 3.6;
    minVolts = 3.0;
    maxAmps = 10;

  }else if(digitalRead(digitalInputs[1]) == HIGH && digitalRead(digitalInputs[2]) == LOW){
    oled.setCursor(30,3);
    oled.print(F(" 5V "));
    maxVolts = 5.8;
    minVolts = 4.8;
    maxAmps = 15;

  }else if(digitalRead(digitalInputs[1]) == LOW && digitalRead(digitalInputs[2]) == LOW){
    oled.setCursor(35,3);
    oled.print(F("12V"));
    maxVolts = 12.8;
    minVolts = 11.8;
    maxAmps = 20;

  }else if(digitalRead(digitalInputs[1]) == HIGH && digitalRead(digitalInputs[2]) == HIGH){
    oled.setCursor(30,3);
    oled.print(F("VSEL"));
    maxVolts = 0.0;
    minVolts = 0.0;
    maxAmps = 0;
  }

  oled.setCursor(5,36);
  oled.setTextSize(2);
  oled.print(F("PSU Module"));

  oled.display();
  delay(5000);

  oled.clearDisplay();

  //Draw the initial layout
  oled.setTextSize(1);
  oled.setTextColor(SSD1306_WHITE);

  if(digitalRead(digitalInputs[1]) == LOW && digitalRead(digitalInputs[2]) == HIGH){
    oled.setCursor(15,0);
    oled.print(F("3.3V Power Supply"));
  }else if(digitalRead(digitalInputs[1]) == HIGH && digitalRead(digitalInputs[2]) == LOW){
    oled.setCursor(15,0);
    oled.print(F("5V Power Supply"));
  }else if(digitalRead(digitalInputs[1]) == LOW && digitalRead(digitalInputs[2]) == LOW){
    oled.setCursor(15,0);
    oled.print(F("12V Power Supply"));
  }else if(digitalRead(digitalInputs[1]) == HIGH && digitalRead(digitalInputs[2]) == HIGH){
    oled.setCursor(15,0);
    oled.print(F("NO VOLTAGE SELECT"));
  }

  oled.drawLine(0,9,128,9,SSD1306_WHITE);

  oled.drawLine(0,32,128,32,SSD1306_WHITE);

  oled.drawLine(SCREEN_WIDTH/2-1,10,SCREEN_WIDTH/2-1,SCREEN_HEIGHT,SSD1306_WHITE); //Draw the line below the text

  oled.fillRect(SCREEN_WIDTH/2,SCREEN_HEIGHT/2,SCREEN_HEIGHT,SCREEN_WIDTH,SSD1306_WHITE);
  oled.setTextSize(3);
  oled.setTextColor(SSD1306_BLACK);
  oled.setCursor(SCREEN_WIDTH/2+6,SCREEN_HEIGHT/2+6);
  oled.print(F("OFF"));
  

  oled.display();
  
}



void loop() {
  //Start button check
  btn1.loop();

  //Accurately scale voltage / amps
  int VoltsRaw = analogRead(analogInputs[0]);
  int AmpsRaw = analogRead(analogInputs[1]);
  
  float Volts = ScaleValues(VoltsRaw,0,1023,0,20);
  float Amps = ScaleValues(AmpsRaw,511,1023,0,30);

  //Timers for LED's and OLED display
  unsigned long T1Time = millis();
  if(T1Time - T1PrevTime >= n250ms){
    writeDataToScreen(Volts,Amps,outputRelay);
    T1PrevTime = T1Time;
  }

  unsigned long T2Time = millis();
  if(T2Time - T2PrevTime >= n500ms){
    if(outputRelay == LOW){
      if(GreenLedState == LOW){
        GreenLedState = HIGH;
      }else{
        GreenLedState = LOW;
      }
    }else{
      GreenLedState = HIGH;
    }
    
    if(faultActive == true){
      if(RedLedState == LOW){
        RedLedState = HIGH;
      }else{
        RedLedState = LOW;
      }
    }else{
      RedLedState = LOW;
    }



    /*
    //Write data to Serial monitor
    Serial.print(Volts);
    Serial.print(F(";"));
    Serial.print(Amps);
    Serial.print(F(";"));
    Serial.print(outputRelay);
    Serial.print(F(";"));
    Serial.print(faultActive);
    Serial.print('\n');
    */
    T2PrevTime = T2Time;
  }

  if(btn1.isPressed()){
    outputRelay = !outputRelay;
    GreenLedState = HIGH;
  }

  if(Amps > maxAmps){
    outputRelay = LOW;
  }
  
  //Outputs
  digitalWrite(digitalOutputs[1], GreenLedState);
  digitalWrite(digitalOutputs[2], RedLedState);
  digitalWrite(digitalOutputs[0],outputRelay);

  if(((Volts > maxVolts || Volts < minVolts)&&outputRelay==HIGH) || Amps > maxAmps){
    faultActive = true;
  }else{
    faultActive = false;
  }

}