#include <PID_v1.h>
#include <SPI.h>
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"

#define TFT_CS     16
#define TFT_DC     5
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

#define LTBLUE    0xB6DF
#define LTTEAL    0xBF5F
#define LTGREEN   0xBFF7
#define LTCYAN    0xC7FF
#define LTRED     0xFD34 
#define LTMAGENTA 0xFD5F
#define LTYELLOW  0xFFF8
#define LTORANGE  0xFE73
#define LTPINK    0xFDDF
#define LTPURPLE  0xCCFF
#define LTGREY    0xE71C

#define BLUE      0x001F
#define TEAL      0x0438
#define GREEN     0x07E0
#define CYAN      0x07FF
#define RED       0xF800
#define MAGENTA   0xF81F
#define YELLOW    0xFFE0
#define ORANGE    0xFC00
#define PINK      0xF81F
#define PURPLE    0x8010
#define GREY      0xC618
#define WHITE     0xFFFF
#define BLACK     0x0000

#define DKBLUE    0x000D
#define DKTEAL    0x020C
#define DKGREEN   0x03E0
#define DKCYAN    0x03EF
#define DKRED     0x6000
#define DKMAGENTA 0x8008
#define DKYELLOW  0x8400
#define DKORANGE  0x8200
#define DKPINK    0x9009
#define DKPURPLE  0x4010
#define DKGREY    0x4A49
// this is the only external variable used by the graph
// it's a flat to draw the coordinate system only on the first pass
boolean display1 = true;
boolean display2 = true;
double ox , oy ;
double y;
int x, xmin, xmax;

const double dt = 0.01; // Time step
const double omega_n = 1.0; // Natural frequency
const double zeta = 1.0; // Damping ratio

double ya = 0.0; // Output
double y_dot = 0.0; // First derivative of y
double y_ddot = 0.0; // Second derivative of y

// Define PID constants
const double Kp = 1.0; // Proportional gain
const double Ki = 0.25; // Integral gain
const double Kd = 0.0; // Derivative gain

// Define PID variables
double input, output;
double setpoint = 0.0;

// Variables for PID control
double error, prevError = 0, integral = 0;


void setup() {
  Serial.begin(9600);
 Serial.begin(9600);
      x = 0; xmin = 0; xmax=60;


  tft.begin();
  tft.fillScreen(BLACK);
  tft.setRotation(1);
   delay(100);
 
}

void loop() {
  // Set input to 0.5
  input = 0.5;

  // Calculate error for PID control
  error = input-ya;

  // Update integral
  integral += error * dt;

  // Calculate PID output
  output = Kp * error + Ki * integral + Kd * (error - prevError) / dt;

  // Limit output to 0-1
  

  // Update input to the system (output of PID)


   // Compute the second derivative of y using the linear differential equation
    y_ddot = output - 2 * zeta * omega_n * y_dot - omega_n * omega_n * ya;

    // Update the first derivative of y
    y_dot += y_ddot * dt;

    // Update y
    ya += y_dot * dt;
    y=ya;

if (x < xmax ){
    x++;
  }else{
    xmin = xmax;
    xmax = xmax+60;
    display1 = true;
    tft.fillScreen(BLACK);
    delay(100);
  }

  if (x == 240){
    x = 0;
    xmin = 0;
    xmax = 60;
    display1 = true;
    tft.fillScreen(BLACK);
  }
  
  Graph(tft, x, y, 30, 210, 270, 180, xmin, xmax, 10, 0, 50, 10, "     V", " Time [s]", "Voltage [deg]", DKBLUE, RED, GREEN, WHITE, BLACK, display1); 
  Serial.print("Temp:");Serial.println(y); 
  delay(50);

    
}


void Graph(Adafruit_ILI9341 &d, double x, double y, double gx, double gy, double w, double h, double xlo, double xhi, double xinc, double ylo, double yhi, double yinc, String title, String xlabel, String ylabel, unsigned int gcolor, unsigned int acolor, unsigned int pcolor, unsigned int tcolor, unsigned int bcolor, boolean &redraw) {

  double ydiv, xdiv;
  // initialize old x and old y in order to draw the first point of the graph
  // but save the transformed value
  // note my transform funcition is the same as the map function, except the map uses long and we need doubles
  //static double ox = (x - xlo) * ( w) / (xhi - xlo) + gx;
  //static double oy = (y - ylo) * (gy - h - gy) / (yhi - ylo) + gy;
  double i;
  double temp;
  int rot, newrot;

  if (redraw == true) {

    redraw = false;
    ox = (x - xlo) * ( w) / (xhi - xlo) + gx;
    oy = (y - ylo) * (gy - h - gy) / (yhi - ylo) + gy;
    // draw y scale
    for ( i = ylo; i <= yhi; i += yinc) {
      // compute the transform
      temp =  (i - ylo) * (gy - h - gy) / (yhi - ylo) + gy;

      if (i == 0) {
        d.drawLine(gx, temp, gx + w, temp, acolor);
      }
      else {
        d.drawLine(gx, temp, gx + w, temp, gcolor);
      }

      d.setTextSize(1);
      d.setTextColor(tcolor, bcolor);
      d.setCursor(gx - 20, temp);
      // precision is default Arduino--this could really use some format control
      int ay = i;
      //d.println(i);
      d.println(ay);
    }
    // draw x scale
    for (i = xlo; i <= xhi; i += xinc) {

      // compute the transform

      temp =  (i - xlo) * ( w) / (xhi - xlo) + gx;
      if (i == 0) {
        d.drawLine(temp, gy, temp, gy - h, acolor);
      }
      else {
        d.drawLine(temp, gy, temp, gy - h, gcolor);
      }

      d.setTextSize(1);
      d.setTextColor(tcolor, bcolor);
      d.setCursor(temp - 5, gy + 10);
      // precision is default Arduino--this could really use some format control
      int ax= i;
      d.println(ax);
    }

    //now draw the labels
    d.setTextSize(2);
    d.setTextColor(tcolor, bcolor);
    d.setCursor(gx , gy - h - 30);
    d.println(title);

    d.setTextSize(1);
    d.setTextColor(acolor, bcolor);
    d.setCursor(gx , gy + 20);
    d.println(xlabel);

    d.setTextSize(1);
    d.setTextColor(acolor, bcolor);
    d.setCursor(gx - 30, gy - h - 10);
    d.println(ylabel);


  }

  //graph drawn now plot the data
  // the entire plotting code are these few lines...
  // recall that ox and oy are initialized as static above
  x =  (x - xlo) * ( w) / (xhi - xlo) + gx;
  y =  (y - ylo) * (gy - h - gy) / (yhi - ylo) + gy;
  d.drawLine(ox, oy, x, y, pcolor);
  d.drawLine(ox, oy + 1, x, y + 1, pcolor);
  d.drawLine(ox, oy - 1, x, y - 1, pcolor);
  ox = x;
  oy = y;

}