//Built-in
#include <Arduino.h>
#include <SPI.h>

// Additoinal Libraries
#include <LiquidCrystal_I2C.h>
#include <LiquidMenu.h>  // LiquidMenu_config.h needs to be modified to use I2C.
#include <MD_REncoder.h>
#include <Adafruit_MAX31865.h>
//#include <EEPROM-Storage.h>

// My Includes
#include "config.h"
#include "src/button/button.h"
#include "src/slowPWM/slowPWM.h"
#include "src/thermoControl/thermoControl.h"
//#include "src/Arduino-PID-Library/PID_v1.h"

double KettleDuty = 0;
double KettleSetpoint = 70;
double CurrentTemp;
bool tuning = false;

//EEPROMStorage<double> eepromKp(0, 2.0); // 9 bytes for doubles 8 + 1 for checksum
//EEPROMStorage<double> eepromKi(9, 5.0); // Initialize at zero.
//EEPROMStorage<double> eepromKd(18, 1.0);

// User I/O objects.
Button Enter;
slowPWM boilPWM;
MD_REncoder rotary = MD_REncoder(encoderDT, encoderCLK);
LiquidCrystal_I2C lcd(0x27, 20, 4);
Adafruit_MAX31865 thermoRTD = Adafruit_MAX31865(KettleRTD);
thermoControl Controller(&CurrentTemp, &KettleSetpoint, &KettleDuty, AUTOMATIC);

//PID ControllerPID(&CurrentTemp, &KettleDuty, &KettleSetpoint, eepromKp, eepromKi, eepromKd, P_ON_M, DIRECT);

unsigned long lastRun = 0;

// Return a character array to represent the
// On/Off state of the kettle.
char* KettleState() {
  switch (Controller.Mode()) {
    case OFF: return (char*)"OFF";
    case AUTOMATIC: return (char*)"AUT";
    case MANUAL: return (char*)"MAN";
    default: return (char*)"OFF";
  }
}

byte setpoint() {
  return (byte)KettleSetpoint;
}

byte duty() {
  return (byte)KettleDuty;
}

byte current() {
  return (byte)CurrentTemp;
}

// LCD menu setup.
LiquidLine KettleState_line(0, 0,        "Boil Kettle      ", KettleState);
LiquidLine kettle_temp_line(0, 1,        "Setpoint (F)     ", setpoint);
LiquidLine kettle_power_line(0, 2,       "Power (%)        ", duty);
LiquidLine kettle_currenttemp_line(0, 3, "Actual Temp (F)  ", current);
LiquidScreen home_screen(KettleState_line, kettle_temp_line, kettle_power_line,kettle_currenttemp_line);
LiquidMenu menu(lcd);

// Interrupt function to run when encoder is turned.
//
// Increases/decreases the kettle output to a max
// of 100% and minimum of 0%.
void doEncoder() {
  uint8_t result = rotary.read();
  uint8_t inc = 1;

  if (result) {
    uint8_t speed = rotary.speed();
    speed >= 10 ? inc = 5 : inc = 1;
  }

  if (Controller.Mode() == AUTOMATIC) {
    if (result == DIR_CW) {
      KettleSetpoint = (KettleSetpoint / inc) * inc + inc;
    } else if (result == DIR_CCW) {
      KettleSetpoint = (KettleSetpoint / inc) * inc - inc;
    }

    if (KettleSetpoint > 212) KettleSetpoint=212;
    if (KettleSetpoint < 0) KettleSetpoint=0;

  } else if (Controller.Mode() == MANUAL) {
    if (result == DIR_CW) {
      KettleDuty = (KettleDuty / inc) * inc + inc;
    } else if (result == DIR_CCW) {
      KettleDuty = (KettleDuty / inc) * inc - inc;
    }

    if (KettleDuty > 100) KettleDuty = 100;
    if (KettleDuty < 0) KettleDuty = 0;

  }
  //menu.update();
}

void setup() {

  lastRun = millis() - UpdateInterval;
  Serial.begin(115200);
  rotary.begin();
  thermoRTD.begin(MAX31865_3WIRE);

  attachInterrupt(digitalPinToInterrupt(encoderCLK), doEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderDT), doEncoder, CHANGE);


  pinMode(encoderCLK, INPUT_PULLUP);
  pinMode(encoderDT, INPUT_PULLUP);
  Enter.begin(encoderBTN);
  boilPWM.begin(kettlePWM, PeriodPWM);
  Controller.Mode(OFF);

  lcd.init();
  lcd.backlight();

  menu.init();
  menu.add_screen(home_screen);
  menu.update();
};

void run_kettle() {
  if (Enter.pressed()) {
    Serial.println("Enter Pressed");
    if (Controller.CycleMode() == OFF) {
      KettleDuty = 0;
    }
    menu.update();
  }

  Controller.Compute();

}
/*
void run_tuning() {
  if (Enter.pressed()) {
    stopAutoTune();
  }
  int result = ControllerPID.ComputeTune();
    if (result!=0)
    {
      tuning = false;
    }
    if(!tuning)
    { //we're done, set the tuning parameters
      ControllerPID.SetTunings(ControllerPID.TunedKp(),ControllerPID.TunedKi(),ControllerPID.TunedKd());
      ControllerPID.Mode(OFF);
    }
}
*/
void loop() {
  unsigned long now = millis();
  static unsigned long lastTime=now-2000;
  unsigned long timeChange = (now - lastTime);
  static byte lastoutput = LOW;
  static double lastKettleDuty = 0;
  static double lastKettleSetpoint = 0;
  static byte lastTemperature;

  if(timeChange >= 2000) {
    CurrentTemp = 32 + 1.8 * thermoRTD.temperature(RNOMINAL_KETTLE, RREF_KETTLE);
    lastTime = now;
  }

  if (!tuning) {
    run_kettle();
  } else {
    //run_tuning();
  }

  byte output = boilPWM.Compute(KettleDuty);

  if (output != lastoutput) {
    digitalWrite(kettlePWM, output);
    lastoutput = output;
  }

  if (lastKettleDuty != KettleDuty || lastKettleSetpoint != KettleSetpoint || lastTemperature != (byte)CurrentTemp) {
    menu.update();
    lastKettleDuty = KettleDuty;
    lastKettleSetpoint = KettleSetpoint;
    lastTemperature = (byte)CurrentTemp;
  }
  
}
/*
void startAutoTune() {
  if(!tuning) {
    //Set the output to the desired starting frequency.
    KettleDuty=50;
    ControllerPID.SetNoiseBand(1);
    ControllerPID.SetOutputStep(30);
    ControllerPID.SetLookbackSec(20);
    ControllerPID.Mode(AUTOMATIC);
    tuning=true;
  }
}

void stopAutoTune() {
  if(tuning) { //cancel autotune
    ControllerPID.Cancel();
    ControllerPID.Mode(OFF);
    tuning=false;
  }
}

void SaveTunings() {
  eepromKp = ControllerPID.GetKp();
  eepromKi = ControllerPID.GetKi();
  eepromKd = ControllerPID.GetKd();
}

void SerialSend()
{
  Serial.print("setpoint: ");Serial.print(KettleSetpoint); Serial.print(" ");
  Serial.print("input: ");Serial.print(CurrentTemp); Serial.print(" ");
  Serial.print("output: ");Serial.print(KettleDuty); Serial.print(" ");
  if(tuning){
    Serial.println("tuning mode");
  } else {
    Serial.print("kp: ");Serial.print(ControllerPID.GetKp());Serial.print(" ");
    Serial.print("ki: ");Serial.print(ControllerPID.GetKi());Serial.print(" ");
    Serial.print("kd: ");Serial.print(ControllerPID.GetKd());Serial.println();
  }
}

void SerialReceive()
{
  if(Serial.available()) {
    String myinput;
    myinput = Serial.readString();
    myinput.trim();
    if(myinput=="start"){
      startAutoTune();  
    } else if(myinput=="stop") {
      stopAutoTune();
    } else if(myinput=="save") {
      SaveTunings();
    }
  }
}
*/