//#include <math.h>         //loads the more advanced math functions
//#include <SPI.h>
//#include <Wire.h>
//#include <Adafruit_GFX.h>


//Global Variables
////PINS////
//IN
byte gbliIncreasePin = 3;
byte gbliDecreasePin = 4;
byte gbliHeatingPin = 5;
byte gbliPowerPin = 6;
byte gbliCurrentOut = A7;

//OUT
byte gbliPowerLED = A0; //Green
byte gbliTempLED = A1; //Orange
byte gbliHeatingLED = A2; //Yellow
byte gbliUnitHASPowerLED = A3; //Red ENABLE
byte gbliUnitBuzzer = A6;  //Buzzer
byte gbliPOWEROutPIN = 11;

//Variables
volatile unsigned long last_interrupt_time;
double gbldKettleTemp = 0;
boolean gblboolControl = true;
double gbldCurrentOut = 0;

// Display Configuration
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 OLEDLeft(4);
Adafruit_SSD1306 OLEDRight(5);

void DisplaySetup() {
  OLEDLeft.begin(SSD1306_SWITCHCAPVCC, 0x3D);
  OLEDLeft.clearDisplay();
  OLEDLeft.setTextColor(WHITE);
  OLEDLeft.setCursor(0, 0);
  OLEDLeft.setTextSize(1);
  OLEDLeft.println("Loading...");
  OLEDLeft.display();

  OLEDRight.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  OLEDRight.clearDisplay();
  OLEDRight.setTextColor(WHITE);
  OLEDRight.setCursor(0, 0);
  OLEDRight.setTextSize(1);
  OLEDRight.println("Loading...");
  OLEDRight.display();
}

// BlueTooth Configuration
#include <SoftwareSerial.h>
SoftwareSerial BluetoothSerial(12, 13); //TX = 12 //RX = 13

void BlueToothSetup() {
  //Setup BlueTooth
  /* Set the baud rate for the software serial port */
  BluetoothSerial.begin(9600);
  delay(1000);
  // Should respond with OK
  BluetoothSerial.print("AT");
  waitForResponse();
  // Should respond with its version
  BluetoothSerial.print("AT+VERSION");
  waitForResponse();
  // Set pin to 1234
  BluetoothSerial.print("AT+PIN1234");
  waitForResponse();
  // Set the name
  BluetoothSerial.print("AT+NAMEBLUBurner");
  waitForResponse();
  // Set baudrate from 9600 (default) to 57600
  // * Note of warning * - many people report issues after increasing JY-MCU
  // baud rate upwards from the default 9,600bps rate (e.g. 'AT+BAUD4')
  // so you may want to leave this and not alter the speed!!
  BluetoothSerial.print("AT+BAUD4");
  waitForResponse();
  Serial.println("BlueTooth Setup is Finished!");
}

//RTD Configuration
#include <Adafruit_MAX31865.h>
// Use software SPI: CS, DI, DO, CLK
Adafruit_MAX31865 max = Adafruit_MAX31865(10, 9, 8, 7);
// The value of the Rref resistor. Use 430.0!
#define RREF 430.0

// PID Configuration
#include <PID_v1.h>
//Define Variables we'll be connecting to
double Setpoint, dPOWEROut;
//Define the aggressive and conservative Tuning Parameters
double consKp = 75, consKi = 0.5, consKd = 0.25;
//Specify the links and initial tuning parameters
PID myPID(&gbldKettleTemp, &dPOWEROut, &Setpoint, consKp, consKi, consKd, DIRECT); //DIRECT or REVERSE



void setup()
{
  // Debugging Setup
  Serial.begin(9600);

  //Display Setup
  DisplaySetup();

  //BlueTooth Setup
  BlueToothSetup();

  //Set Pins
  pinMode(gbliPOWEROutPIN, OUTPUT);   // sets the pin as output
  pinMode(gbliPowerLED, OUTPUT);
  pinMode(gbliHeatingLED, OUTPUT);
  pinMode(gbliTempLED, OUTPUT);
  pinMode(gbliUnitHASPowerLED, OUTPUT);
  pinMode(gbliUnitBuzzer, OUTPUT);
  //analogReference(EXTERNAL);
  pinMode(gbliCurrentOut, INPUT_PULLUP);
  pinMode(gbliIncreasePin, INPUT_PULLUP);
  pinMode(gbliDecreasePin, INPUT_PULLUP);
  pinMode(gbliHeatingPin, INPUT_PULLUP);
  pinMode(gbliPowerPin, INPUT_PULLUP);
  pinMode(2, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(2), CheckButtons, LOW);

  //RTD Setup
  max.begin(MAX31865_4WIRE);

  //turn the PID on
  myPID.SetMode(AUTOMATIC);

  Setpoint = 152;

}

void DisplayTemps(double powerOut) {
  OLEDLeft.clearDisplay();
  OLEDLeft.setCursor(0, 0);
  OLEDLeft.setTextSize(1);
  OLEDLeft.println(F("Kettle SetPoint:"));
  OLEDLeft.setTextSize(2);
  OLEDLeft.print(Setpoint);
  OLEDLeft.print(" ");
  OLEDLeft.print((char)0367);
  OLEDLeft.println("F");

  OLEDLeft.setTextSize(1);
  OLEDLeft.print(F("Power Out: "));
  OLEDLeft.setTextSize(1);
  OLEDLeft.print(powerOut);
  OLEDLeft.display();

  OLEDRight.clearDisplay();
  OLEDRight.setCursor(0, 0);
  OLEDRight.setTextSize(1);
  OLEDRight.println(F("Kettle Temp:"));
  OLEDRight.setTextSize(2);
  OLEDRight.print(gbldKettleTemp);
  OLEDRight.print(" ");
  OLEDRight.print((char)0367);
  OLEDRight.println("F");

  OLEDRight.setTextSize(1);
  OLEDRight.print(F("Current Out: "));
  OLEDRight.setTextSize(1);
  OLEDRight.print(gbldCurrentOut);
  OLEDRight.display();
}

// Function to pass BlueTooth output through to serial port output
void waitForResponse() {
  delay(1000);
  while (BluetoothSerial.available()) {
    Serial.write(BluetoothSerial.read());
  }
  Serial.write("\n");
}

void ReadSettings() {
  String reading;
  char inChar;
  boolean bNewSetting = false;
  while (BluetoothSerial.available()) {
    inChar = (char)BluetoothSerial.read();
    reading += inChar;
    if (reading.substring(0, 13) == F("New Setpoint:")) {
      reading = "";
      while (BluetoothSerial.available()) {
        inChar = (char)BluetoothSerial.read();
        reading += inChar;
      }
      Setpoint = reading.toDouble();
    }
    else if (reading.substring(0, 17) == F("New PID Settings:")) {
      consKp = BluetoothSerial.readStringUntil('\n').toDouble();
      consKi = BluetoothSerial.readStringUntil('\n').toDouble();
      consKd = BluetoothSerial.readStringUntil('\n').toDouble();
      bNewSetting = true;
    }
    else if (reading.substring(0, 16) == F("New PID Control:")) {
      reading =  BluetoothSerial.readStringUntil('\n');
      OLEDRight.clearDisplay();
      OLEDRight.setCursor(0, 0);
      OLEDRight.setTextSize(1);
      OLEDRight.println(F("PID Control"));
      if (reading == "ON") {
        gblboolControl = true;
        OLEDRight.print(reading);
      }
      else {
        gblboolControl = false;
        OLEDRight.print(reading);
      }
      OLEDRight.display();
      delay(2500);
    }
    else if (reading.substring(0, 10) == F("New Power:")) {
      reading = "";
      while (BluetoothSerial.available()) {
        inChar = (char)BluetoothSerial.read();
        reading += inChar;
      }
      if (reading == "ON") {
        digitalWrite(gbliPowerLED, HIGH);
      }
      else {
        digitalWrite(gbliPowerLED, LOW);
      }
    }
  }


  BluetoothSerial.print(F("Setpoint: "));
  BluetoothSerial.print(Setpoint);
  BluetoothSerial.println(F(";"));
  BluetoothSerial.print(F("Proportional: "));
  BluetoothSerial.print(consKp);
  BluetoothSerial.println(F(";"));
  BluetoothSerial.print(F("Integral: "));
  BluetoothSerial.print(consKi);
  BluetoothSerial.println(F(";"));
  BluetoothSerial.print(F("Derivative: "));
  BluetoothSerial.print(consKd);
  BluetoothSerial.println(F(";"));

  if (bNewSetting == true) {

    OLEDRight.clearDisplay();
    OLEDRight.setCursor(0, 0);
    OLEDRight.setTextSize(1);
    OLEDRight.print(F("Proportional:"));
    OLEDRight.setTextSize(1);
    OLEDRight.println(consKp);
    OLEDRight.print(F("Integral:"));
    OLEDRight.setTextSize(1);
    OLEDRight.println(consKi);
    OLEDRight.print(F("Derivative:"));
    OLEDRight.setTextSize(1);
    OLEDRight.println(consKd);
    OLEDRight.display();
    delay(2500);
  }

}



void CheckButtons() {
  //static unsigned long last_interrupt_time = 0;
  unsigned long interrupt_time = millis();
  // If interrupts come faster than 200ms, assume it's a bounce and ignore
  if (interrupt_time - last_interrupt_time > 200) {
    int PowerPin = digitalRead(gbliPowerPin);
    int HeatingPin = digitalRead(gbliHeatingPin);

    if (PowerPin == LOW ) {
      if ( digitalRead(gbliPowerLED) == LOW) {
        //gblboolPower = true;
        digitalWrite(gbliPowerLED, HIGH);
        //Turn Beeper ON!!!
      }
      else if (digitalRead(gbliPowerLED) == HIGH) {
        //gblboolPower = false;
        digitalWrite(gbliPowerLED, LOW);
        //Turn Beeper ON!!!
      }
    }
    else if (HeatingPin == LOW) {
      if (digitalRead(gbliHeatingLED) == HIGH) {
        digitalWrite(gbliHeatingLED, LOW);
      }
      else {
        digitalWrite(gbliHeatingLED, HIGH);
      }
    }

  }

  if (interrupt_time - last_interrupt_time > 5) {
    int IncreasePin = digitalRead(gbliIncreasePin);
    int DecreasePin = digitalRead(gbliDecreasePin);

    if (IncreasePin == LOW) {
      Setpoint += 1;
    }
    else if (DecreasePin == LOW) {
      Setpoint -= 1;
    }
  }

  last_interrupt_time = interrupt_time;

}


void  ReadRTD() {
  byte i;
  byte NUMSAMPLES = 10;
  float average;
  float samples[NUMSAMPLES];

  // take N samples in a row, with a slight delay
  for (i = 0; i < NUMSAMPLES; i++) {
    samples[i] = max.temperature(100, RREF);
    delay(5);
  }

  // average all the samples out
  average = 0;
  for (i = 0; i < NUMSAMPLES; i++) {
    average += samples[i];
  }
  average /= NUMSAMPLES;

  gbldKettleTemp = (average * 9.0) / 5.0 + 32.0; // Convert Celcius to Fahrenheit
  Serial.print(F("Kettle Temp = ")); Serial.println(gbldKettleTemp);
  BluetoothSerial.print(F("Kettle Temp = "));
  BluetoothSerial.print(gbldKettleTemp);
  BluetoothSerial.println(F(";"));
}

void ReadCurrentOut() {
  gbldCurrentOut =   analogRead(gbliCurrentOut) * (5.0 / 1023.0);
}




/////////Start of the LOOP//////////////

void loop()
{

  byte PIDLoop  = 0;
  digitalWrite(gbliUnitHASPowerLED, HIGH);
  digitalWrite(gbliTempLED, LOW);
  digitalWrite(gbliHeatingLED, LOW);

  ReadRTD();
  ReadSettings();
  ReadCurrentOut();
  DisplayTemps(0);
  delay(5);

  //Power Button Must Be ON
  while (digitalRead(gbliPowerLED) == HIGH) {
    if  (gbldCurrentOut > 0.3) {
      digitalWrite(gbliHeatingLED, HIGH);
    }
    else {
      digitalWrite(gbliHeatingLED, LOW);
    }
    ReadRTD();
    ReadSettings();
    ReadCurrentOut();
    DisplayTemps(dPOWEROut);

    //USE PID Control
    if (gblboolControl == true) {
      digitalWrite(gbliTempLED, HIGH);

      //Calculate PID
       myPID.SetTunings(consKp, consKi, consKd);
        myPID.Compute();
   /*   if (PIDLoop >= 2) {
        myPID.SetTunings(consKp, consKi, consKd);
        myPID.Compute();
        PIDLoop = 0;
     } else {
        PIDLoop += 1;
      }*/


      //Write Analog IO
      dPOWEROut = map(dPOWEROut, 0, 255, 66, 220); //66 = 1.05 Volts //220 = 3.5 Volts
      //dPOWEROut = 70;
      analogWrite(gbliPOWEROutPIN, dPOWEROut);  // analogRead values go from 0 to 1023, analogWrite values from 0 to 255
      dPOWEROut = map(dPOWEROut, 66, 220, 0, 100); // Convert to Percent out!!!
      Serial.print(F("Power Out (Mapped): "));
      Serial.println(dPOWEROut);
      BluetoothSerial.print(F("Power Out: "));
      BluetoothSerial.print(dPOWEROut);
      BluetoothSerial.println(F(";"));
      Serial.println();
      delay(1);
    }
    else {
      digitalWrite(gbliTempLED, LOW);
      //Write Analog IO
      dPOWEROut = map(Setpoint, 0, 100, 0, 255); // Convert Percent to 0-255 out!!!
      dPOWEROut = map(dPOWEROut, 0, 255, 66, 220); //66 = 1.05 Volts //220 = 3.5 Volts
      analogWrite(gbliPOWEROutPIN, dPOWEROut);  // analogRead values go from 0 to 1023, analogWrite values from 0 to 255
      dPOWEROut = map(dPOWEROut, 58, 185, 0, 100); // Convert to Percent out!!!
      Serial.print(F("Power Out (Mapped): "));
      Serial.println(dPOWEROut);
      BluetoothSerial.print(F("Power Out: "));
      BluetoothSerial.print(dPOWEROut);
      BluetoothSerial.println(F(";"));
      Serial.println();
      delay(1);

    }
  }

  BluetoothSerial.print(F("Power Out: "));
  BluetoothSerial.print(0);
  BluetoothSerial.println(F(";"));

}