#include <Arduino.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <AccelStepper.h>
#include <PID_v2.h>
#include <EEPROM.h>
#define ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h>

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

#define S_MAINSCREEN 0
#define S_MENU 1
#define S_SETTEMP 10
#define S_PRESETTEMP 20
#define S_CHANGETEMPS 21
#define S_CHANGETEMP 22
#define S_SETSPEED 30
#define S_PRESETSPEED 40
#define S_CHANGESPEEDS 41
#define S_CHANGESPEED 42
#define S_PID 50
#define S_KP 51
#define S_KI 52
#define S_KD 53
#define S_AUTOTUNE 54

// Declaration for SSD1306 display connected using I2C
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

double dRefresh = 100, dLastRefresh;
int iPage = 0, iItemMenu = 0, iItemMenuLast;

// --------------- Encoder -------------------------
#define ENCODER_CLK 2
#define ENCODER_DT  3
#define ENCODER_BTN 4
Encoder Enc(ENCODER_CLK, ENCODER_DT);

// -------------- Stepper --------------------------
#define MOTOR_STEPS 200
#define RPM 120

#define MICROSTEPS 16
#define DIR 8
#define STEP 9
#define SLEEP 13 // optional (just delete SLEEP from everywhere if not used)
#define motorInterfaceType 1

AccelStepper stepper = AccelStepper(motorInterfaceType, STEP, DIR);

// ----------------- PID ---------------------------
#define MAXTEMP 300
#define LED 5
double dSetTemp = 20;
double kp = 22, ki = 1, kd = 114;
double input, output; // PID_v1

PID myPID(&input, &output, &dSetTemp, kp, ki, kd, DIRECT);
// -------------------- Own ------------------------
#define MAXSPEED 1000

bool xOled = true;
bool xStepperRun = true;
bool xOnce;
int iSpeed = 0;

int iTempPreset[3] = {190, 200, 210};

int iSpeedPreset[6] = {-600, -400, -200, 200, 400, 600};

int ledState = HIGH;        // the current state of the output pin
int xEncBtn ,iEncLast;

int xEncBtnState = LOW, xEncBtnLast;
unsigned long uiEncBtnDebounceLastTime = 0;
unsigned long debounceDelay = 50;    // the debounce time; increase if the output flickers


const float BETA = 3950; // should match the Beta Coefficient of the thermistor


void setup() {

  // initialize the OLED object
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) 
  {
  //Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  // Clear the buffer.
  // display.clearDisplay();
  // display.setTextSize(1);
  // display.setTextColor(WHITE);

  stepper.setMaxSpeed(1000);

  pinMode(ENCODER_BTN, INPUT_PULLUP);
  pinMode(LED, OUTPUT);

  myPID.SetMode(AUTOMATIC); // Enable PID control
}

void loop() 
{

  xEncBtn = digitalRead(ENCODER_BTN);
  double analogValue = analogRead(A0);
  float celsius = 1 / (log(1 / (1023. / analogValue - 1)) / BETA + 1.0 / 298.15) - 273.15;
  input = double(celsius);

  myPID.Compute();
  
  switch (iPage) // Display Control
  {
    case S_MAINSCREEN: // Main Screen (Page 0)
      if((millis() - dLastRefresh) > dRefresh)
      {
        display.clearDisplay();
        display.setCursor(0,0);
        display.setTextColor(WHITE,BLACK);
        display.print("Speed: ");
        display.println(iSpeed,DEC);
    
        display.print("Meas. Temp: ");
        display.println(input,2);

        display.print("Set Temp: ");
        display.println(dSetTemp,0);

        display.print("PID Output: ");
        display.println(output,0);
      
        display.stopscroll();
        display.display();

        dLastRefresh = millis();
      }
      break;

    case S_MENU: // Menu (Page 1)
      if(xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(0,0);
        if(iItemMenu == 0)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Set Temp: ");
        display.println(dSetTemp,0);
        if(iItemMenu == 1)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("Preset Temp");
        if(iItemMenu == 2)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Set Speed: ");
        display.println(iSpeed, DEC);
        if(iItemMenu == 3)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("Preset Speed");
        if(iItemMenu == 4)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("PID");
        if(iItemMenu == 5)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("Shutdown All");
        if(iItemMenu == 6)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("Back");
      
        //display.startscrollright(iItemMenu,iItemMenu);

        display.display();
      }
      break;
    case S_SETTEMP: // Set Temp
      if(xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(20,10);
        display.setTextSize(4);
        display.print(dSetTemp,0);
        display.setTextSize(1);
        display.setCursor(0,56);
        display.print("Max Temperature: ");
        display.println(MAXTEMP);
        display.display();
      }
      break;
    case S_PRESETTEMP: // Preset Temp
      if (xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(0,0);
        if(iItemMenu == 0)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset1: ");
        display.println(iTempPreset[0]);
        if(iItemMenu == 1)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset2: ");
        display.println(iTempPreset[1]);
        if(iItemMenu == 2)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset3: ");
        display.println(iTempPreset[2]);
        if(iItemMenu == 3)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("Change Presets");
        if(iItemMenu == 4)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("Back");

        //display.startscrollright(iItemMenu,iItemMenu);
        display.display();
      }
      break;
    case S_CHANGETEMPS:
      if (xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(0,0);
        if(iItemMenu == 0)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset1: ");
        display.println(iTempPreset[0]);
        if(iItemMenu == 1)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset2: ");
        display.println(iTempPreset[1]);
        if(iItemMenu == 2)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset3: ");
        display.println(iTempPreset[2]);
        if(iItemMenu == 3)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("Back");

        //display.startscrollright(iItemMenu,iItemMenu);
        display.display();
      }
      break;
    case S_CHANGETEMP:
      if(xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(20,10);
        display.setTextSize(4);
        display.print(iTempPreset[iItemMenu],DEC);
        display.setTextSize(1);
        display.setCursor(0,56);
        display.print("Max Temperature: ");
        display.println(MAXTEMP);
        display.display();
      }
      break;
    case S_SETSPEED: // Set Speed
      if(xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(20,10);
        display.setTextSize(4);
        display.println(iSpeed,DEC);

        display.setTextSize(1);
        display.setCursor(0,56);
        display.print("Max Speed: +-");
        display.println(MAXSPEED);
        //display.stopscroll();
        display.display();
      }
      break;
    case S_PRESETSPEED: // Preset Speed
      if (xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(0,0);
        if(iItemMenu == 0)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset1: ");
        display.println(iSpeedPreset[0]);
        if(iItemMenu == 1)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset2: ");
        display.println(iSpeedPreset[1]);
        if(iItemMenu == 2)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset3: ");
        display.println(iSpeedPreset[2]);
        if(iItemMenu == 3)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset4: ");
        display.println(iSpeedPreset[3]);
        if(iItemMenu == 4)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset5: ");
        display.println(iSpeedPreset[4]);
        if(iItemMenu == 5)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset6: ");
        display.println(iSpeedPreset[5]);
        if(iItemMenu == 6)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("Change Presets");
        if(iItemMenu == 7)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("Back");

        //display.startscrollright(iItemMenu,iItemMenu);
        display.display();
      }
      break;
    case S_CHANGESPEEDS:
      if (xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(0,0);
        if(iItemMenu == 0)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset1: ");
        display.println(iSpeedPreset[0]);
        if(iItemMenu == 1)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset2: ");
        display.println(iSpeedPreset[1]);
        if(iItemMenu == 2)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset3: ");
        display.println(iSpeedPreset[2]);
        if(iItemMenu == 3)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset4: ");
        display.println(iSpeedPreset[3]);
        if(iItemMenu == 4)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset5: ");
        display.println(iSpeedPreset[4]);
        if(iItemMenu == 5)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Preset6: ");
        display.println(iSpeedPreset[5]);
        if(iItemMenu == 6)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("Back");
        display.display();
      }
      break;
    case S_CHANGESPEED:
      if(xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(20,10);
        display.setTextSize(4);
        display.println(iSpeedPreset[iItemMenu],DEC);
        display.setTextSize(1);
        display.setCursor(0,56);
        display.print("Max Speed: +-");
        display.println(MAXSPEED);
        //display.stopscroll();
        display.display();
      }
      break;
    case S_PID: // PID
      if (xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(0,0);
        if(iItemMenu == 0)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Kp: ");
        display.println(kp,2);
        if(iItemMenu == 1)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Ki: ");
        display.println(ki,2);
        if(iItemMenu == 2)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.print("Kd: ");
        display.println(kd,2);
        if(iItemMenu == 3)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("Autotune");
        if(iItemMenu == 4)
        {
          display.setTextColor(BLACK, WHITE);
        }
        else
        {
          display.setTextColor(WHITE,BLACK);
        }
        display.println("Back");

        //display.startscrollright(iItemMenu,iItemMenu);
        display.display();
      }
      break;
    case S_KP:
      if (xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(0,0);
        display.print("Kp: ");
        display.print(kp,3);
        //display.stopscroll();
        display.display();
      }
      break;
    case S_KI:
      if (xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(0,0);
        display.print("Ki: ");
        display.print(ki,3);
        //display.stopscroll();
        display.display();
      }
      break;
    case S_KD:
      if (xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(0,0);
        display.print("Kd: ");
        display.print(kd,3);
        //display.stopscroll();
        display.display();
      }
      break;
    case S_AUTOTUNE:
      if (xOled == true)
      {
        xOled = false;
        display.clearDisplay();
        display.setCursor(0,0);

        display.display();
      }
      break;
    default:
      break;
  }
  if(iEncLast != Enc.read())
    {
      iEncLast = Enc.read();
      xOled = true;
    }

  if (xEncBtn != xEncBtnState) 
  {
    uiEncBtnDebounceLastTime = millis();
  }

  if ((millis() - uiEncBtnDebounceLastTime) > debounceDelay)
  {
    if (xEncBtn != xEncBtnLast)
    {
      xEncBtnLast = xEncBtn;
      if (xEncBtnLast == LOW) 
      {
        switch(iPage) // Encoder Control
        {
          case S_MAINSCREEN: // Main Screen (Page 0)
            Enc.write(0);
            iPage = S_MENU;
            break;
          case S_MENU: // Main Screen -> Menu (Page 1)
            switch(iItemMenu)
            {
              case 0: // Main Screen -> Menu -> Set Temp (Page 1 Item 0)
                iPage = S_SETTEMP;
                Enc.write(dSetTemp * 4);
                break;
              case 1: // Main Screen -> Menu -> Preset Temp (Page 1 Item 1)
                iPage = S_PRESETTEMP;
                Enc.write(0);
                break;
              case 2: // Main Screen -> Menu -> Set Speed (Page 1 Item 2)
                iPage = S_SETSPEED;
                Enc.write(iSpeed * 4);
                break;
              case 3: // Main Screen -> Menu -> Preset Speed (Page 1 Item 3)
                iPage = S_PRESETSPEED;
                Enc.write(0);
                break;
              case 4: // Main Screen -> Menu -> PID (Page 1 Item 4)
                iPage = S_PID;
                Enc.write(0);
                break;
              case 5:
                iPage = S_MAINSCREEN;
                Enc.write(0);
                //iSpeed = 0;
                dSetTemp = 0;
                break;
              case 6: // Main Screen -> Menu -> Back Button
                iPage = S_MAINSCREEN;
                xOled = true;
                Enc.write(iSpeed*4);
                break;
            }
            break;
          case S_SETTEMP: // Main Screen -> Menu -> Set Temp (Page 2 Item 5)
            Enc.write(0);
            iPage = S_MENU;
            break;
          case S_PRESETTEMP: // Main Screen -> Menu -> Preset Temp (Page 3)
            switch(iItemMenu)
            {
              case 0: // Preset Temp 1
              case 1: // Preset Temp 2
              case 2: // Preset Temp 3
                dSetTemp = double(iTempPreset[iItemMenu]);
                Enc.write(4);
                iPage = S_MENU;
                break;
              case 3: // Config Temps
                Enc.write(0);
                iPage = S_CHANGETEMPS;
                break;
              case 4: // Back
                Enc.write(4);
                iPage = S_MENU;
                break;
            }
            break;
          case S_CHANGETEMPS:
            switch(iItemMenu)
            {
              case 0: // Preset Temp 1
              case 1: // Preset Temp 2
              case 2: // Preset Temp 3
                Enc.write(iTempPreset[iItemMenu] * 4);
                iPage = S_CHANGETEMP;
                break;
              case 3: // Back
                Enc.write(12);
                iPage = S_PRESETTEMP;
                break;
            }
            break;
          case S_CHANGETEMP:
            Enc.write(iItemMenu * 4);
            iPage = S_CHANGETEMPS;
            break;
          case S_SETSPEED:
            Enc.write(8);
            iPage = S_MENU;
            break;
          case S_PRESETSPEED:
            switch(iItemMenu)
            {
              case 0: // Preset Speed 1
              case 1: // Preset Speed 2
              case 2: // Preset Speed 3
              case 3: // Preset Speed 4
              case 4: // Preset Speed 5
              case 5: // Preset Speed 6
                iSpeed = double(iSpeedPreset[iItemMenu]);
                Enc.write(12);
                iPage = S_MENU;
                break;
              case 6: // Config Presets
                Enc.write(0);
                iPage = S_CHANGESPEEDS;
                break;
              case 7: // Back
                Enc.write(12);
                iPage = S_MENU;
                break;
            }
            break;
          case S_CHANGESPEEDS:
            switch(iItemMenu)
            {
              case 0: // Preset Speed 1
              case 1: // Preset Speed 2
              case 2: // Preset Speed 3
              case 3: // Preset Speed 4
              case 4: // Preset Speed 5
              case 5:
                Enc.write(iSpeedPreset[iItemMenu] * 4);
                iPage = S_CHANGESPEED;
                break;
              case 6: // Back
                Enc.write(24);
                iPage = S_PRESETSPEED;
                break;
            }
            break;
          case S_CHANGESPEED:
            Enc.write(iItemMenu * 4);
            iPage = S_CHANGESPEEDS;
            break;
          case S_PID:
            switch(iItemMenu)
            {
              case 0: // Kp
                iPage = S_KP;
                Enc.write(kp * 4 * 100);
                break;
              case 1: // Ki
                iPage = S_KI;
                Enc.write(ki * 4 * 100);
                break;
              case 2: // Kd
                iPage = S_KD;
                Enc.write(kd * 4 * 100);
                break;
              case 3: // Autotune
                iPage = S_AUTOTUNE;
                break;
              case 4: // Back
                iPage = S_MENU;
                Enc.write(16);
                break;
            }
            break;
          case S_KP:
            Enc.write(0);
            iPage = S_PID;
            break;
          case S_KI:
            Enc.write(4);
            iPage = S_PID;
            break;
          case S_KD:
            Enc.write(8);
            iPage = S_PID;
            break;
          case S_AUTOTUNE:
            break;
        }
        xOled = true;
      }
    }
  }

  switch(iPage)
  {
    case S_SETSPEED:
    case S_MAINSCREEN:
      iSpeed = Enc.read() / 4;
      break;
    case S_MENU:
      iItemMenu = Enc.read() / 4;
      if (iItemMenu >= 7)
      {
        Enc.write(0);
      }
      else if (iItemMenu < 0)
      {
        Enc.write(24);
      }
      break;
    case S_SETTEMP:
      dSetTemp = Enc.read() / 4;
      break;
    case S_PRESETTEMP:
      iItemMenu = Enc.read() / 4;
      if (iItemMenu >= 5)
      {
        Enc.write(0);
      }
      else if (iItemMenu < 0)
      {
        Enc.write(16);
      }
      break;
    case S_CHANGETEMPS:
      iItemMenu = Enc.read() / 4;
      if (iItemMenu >= 4)
      {
        Enc.write(0);
      }
      else if (iItemMenu < 0)
      {
        Enc.write(12);
      }
      break;
    case S_CHANGETEMP:
      iTempPreset[iItemMenu] = Enc.read() / 4;
      if(iTempPreset[iItemMenu] > MAXTEMP)
        Enc.write(MAXTEMP * 4);
      else if (iTempPreset[iItemMenu] < 0)
        Enc.write(0);
      break;
    case S_PRESETSPEED:
      iItemMenu = Enc.read() / 4;
      if (iItemMenu >= 8)
      {
        Enc.write(0);
      }
      else if (iItemMenu < 0)
      {
        Enc.write(28);
      }
      break;
    case S_CHANGESPEEDS:
      iItemMenu = Enc.read() / 4;
      if (iItemMenu >= 7)
      {
        Enc.write(0);
      }
      else if (iItemMenu < 0)
      {
        Enc.write(24);
      }
      break;
      break;
    case S_CHANGESPEED:
      iSpeedPreset[iItemMenu] = Enc.read() / 4;
      if(iSpeedPreset[iItemMenu] > MAXSPEED)
        Enc.write(MAXSPEED * 4);
      else if (iSpeedPreset[iItemMenu] < -MAXSPEED)
        Enc.write(-MAXSPEED * 4);
      break;
    case S_PID:
      iItemMenu = Enc.read() / 4;
      if (iItemMenu >= 5)
      {
        Enc.write(0);
      }
      else if (iItemMenu < 0)
      {
        Enc.write(16);
      }
      break;
    case S_KP:
      kp = double(Enc.read()) / double(400);
      break;
    case S_KI:
      ki = double(Enc.read()) / double(400);
      break;
    case S_KD:
      kd = double(Enc.read()) / double(400);
      break;
    case S_AUTOTUNE:
      break;
  }

  stepper.setSpeed(iSpeed);
  stepper.runSpeed();
  
  xEncBtnState = xEncBtn;
  analogWrite(LED, output);
}