#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Arduino.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 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define PRESSURESENSOR 0x48
#define WATERTEMP 2
#define WATERFLOW 3
#define PSUTEMP 4
#define DOORSTATUS 5
#define EX1 6
#define EX2 7
#define AIRDEMAND 8
#define AIRRUN 9
#define STATUSOUTPUT 10
//Safe is unarmed / Fault
#define STATUSSAFE 11
// Armed is no fault, Laser Armed
#define STATUSARMED 12
#define AIRPUMPRUN 13



//10-15psi open airrun(9), less than 10psi 
int iAirPressure = 0;
bool bIsAirDemanded = false;



void setup() {
pinMode(WATERTEMP, INPUT);
pinMode(WATERFLOW, INPUT);
pinMode(PSUTEMP, INPUT);
pinMode(DOORSTATUS, INPUT);
pinMode(EX1, INPUT);
pinMode(EX2, INPUT);
pinMode(AIRDEMAND, INPUT);
pinMode(AIRRUN, OUTPUT);
pinMode(STATUSOUTPUT, OUTPUT);
pinMode(STATUSSAFE, OUTPUT);
pinMode(STATUSARMED, OUTPUT);
pinMode(AIRPUMPRUN, OUTPUT);


//set inital states
digitalWrite(STATUSSAFE, HIGH);
digitalWrite(STATUSOUTPUT, HIGH);
digitalWrite(AIRPUMPRUN, HIGH);
Wire.begin();
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
  }
  // Clear the buffer
  display.clearDisplay();
}

void loop() {

//int iWaterTempState = digitalRead(WATERTEMP);
//int iWaterFlowState = digitalRead(WATERFLOW);
//int iPSUTempState = digitalRead(PSUTEMP);
//int iDoorStatusState = digitalRead(DOORSTATUS);
//int iEX1State = digitalRead(EX1);
//int iEX2State = digitalRead(EX2);
//int iAirDemandState = digitalRead(AIRDEMAND);
bool bFault = false;
bool bAirDemand = false;   

//check for air demand
if(digitalRead(AIRDEMAND) == HIGH)
{
  bAirDemand = true;
  digitalWrite(AIRRUN, HIGH); 
  Serial.println(F("Air Demand - Air On"));   
}
else
{
  bAirDemand = false;
  digitalWrite(AIRRUN, LOW);
  Serial.println(F("Air Demand - Air Off"));   
}

if(digitalRead(WATERTEMP) == LOW)
{
  bFault = true;  
  Serial.println(F("Warning - Water Temperature High"));
  Serial.println(F("FAULT RAISED"));
  screenstate(1,0,0,0,0,0,"Water Temp High");
}
if (digitalRead(WATERFLOW) == HIGH)
{
   bFault = true;
  Serial.println(F("Warning - Water Flow Low"));
  Serial.println(F("FAULT RAISED"));
  screenstate(0,1,0,0,0,0,"Water Flow Low");
}
if (digitalRead(PSUTEMP) == LOW)
{
  bFault = true;
  Serial.println(F("Warning - PSU Temperature High"));
  Serial.println(F("FAULT RAISED"));
  screenstate(0,0,1,0,0,0,"PSU Temp High");
}
if (digitalRead(DOORSTATUS) == HIGH)
{
  bFault = true;
  Serial.println(F("Warning - Door Opened"));
  Serial.println(F("FAULT RAISED"));
  screenstate(0,0,0,1,0,0,"Door Open");
}
if (digitalRead(EX1) == LOW)
{
  bFault = true;
  Serial.println(F("Warning - External Input 1 Fault"));
  Serial.println(F("FAULT RAISED"));
  screenstate(0,0,0,0,1,0,"EX 1 Fault");
}
if (digitalRead(EX2) == HIGH)
{
  bFault = true;
  Serial.println(F("Warning - External Input 2 Fault"));
  Serial.println(F("FAULT RAISED"));
  screenstate(0,0,0,0,1,0,"EX 2 Fault");
}
if(bFault)
{
  digitalWrite(STATUSSAFE, HIGH);
  digitalWrite(STATUSARMED, LOW);
  digitalWrite(STATUSOUTPUT, HIGH);
  digitalWrite(AIRRUN, LOW);
  display.invertDisplay(true);
  display.display();
  delay(1000);
  display.invertDisplay(false);
  display.display();
  delay(1000);  
}
else
{
  digitalWrite(STATUSARMED, HIGH);
  digitalWrite(STATUSSAFE, LOW);
  digitalWrite(STATUSOUTPUT, LOW);
  Serial.println(F("NO FAULT"));

  if(bAirDemand == true)
  {
    screenstate(0,0,0,0,0,0,"Air On - Ready");
  }
  else
  {
    screenstate(0,0,0,0,0,0,"Ready");
  }
}

preasureread();
delay(1000);
}

void screenstate(int iWT, int iWF, int iPSU, int iDoor, int iEX1, int iEX2, char* cText)
{
  display.clearDisplay();
  display.setTextSize(1);      // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE); // Draw white text
  display.setCursor(0, 0);     // Start at top-left corner
  display.cp437(true);         // Use full 256 char 'Code Page 437' font
  display.write(cText);
  display.drawFastHLine(0, 15, 128, SSD1306_WHITE);
  //display.drawRect(14, 16, 100, 48, SSD1306_WHITE);
  display.drawRoundRect(14, 16, 100, 48, 3,SSD1306_WHITE );

if(iWT == 1 || iWF == 1)
{
  //display.fillRect(16, 18, 96, 8, SSD1306_WHITE);
  display.fillRoundRect(16, 18, 96, 8, 3, SSD1306_WHITE);
}
else
{
  display.drawRoundRect(16, 18, 96, 8, 3, SSD1306_WHITE);

}
if(iPSU == 1)
{
  display.fillRoundRect(82, 27, 30, 35, 3, SSD1306_WHITE);
}
else
{
  display.drawRoundRect(82, 27, 30, 35, 3, SSD1306_WHITE);
}
if (iDoor == 1)
{
  display.fillRoundRect(16, 27, 65, 35, 3, SSD1306_WHITE); 
}
else
{
  display.drawRoundRect(16, 27, 65, 35, 3, SSD1306_WHITE); 
}
display.display();
}

unsigned char reverse(unsigned char iVal)
{
 unsigned char XR = 0;
      for(int i=0; i<8; i++)
      {
        if(iVal&(1<<i))
        {
          XR |= 1<<(7-i);
        }
        else
        {
          XR |= 0<<(7-i);
        }
        
      }

      return XR;
}



void preasureread()
{
  unsigned char X0 = 0;
  unsigned char X1 = 0;
  int iResult = 0;
   Wire.requestFrom(PRESSURESENSOR,2);

  if(Wire.available ()>0)
  {
      X0 = Wire.read();
      Serial.print("X0=   ");
      Serial.print(X0);   
      Serial.println();
      X0 = reverse(X0);       
      Serial.print("X0R=   ");
      Serial.print(X0);   
      Serial.println();
      X1 = Wire.read();
      Serial.print(F("X1=  "));
      Serial.print(X1);
      Serial.println();
    //  X1 = reverse(X1);
//Serial.print(F("X1R=  "));
     // Serial.print(X1);
     // Serial.println();
      //iResult = ((int)(X0 <<8)) | X1;
      iResult = X1;
      Serial.print(F("FINAL=  "));
      Serial.print(iResult);
      Serial.println();
      float fPSI = (float)iResult * 0.145;
      Serial.print(F("PSI=  "));
      Serial.print(fPSI);
      Serial.println();
            
  }      

  
}