#include <LiquidCrystal.h>
#include <SPI.h>
#include "Adafruit_MAX31855.h"

#define MAXDO   12
#define MAXCS   10
#define MAXCLK  13

#define rs  7
#define en  6
#define d4  5
#define d5  4
#define d6  3
#define d7  2

#define PWM_pin  11

LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
Adafruit_MAX31855 thermocouple(MAXCLK, MAXCS, MAXDO);
  
unsigned long aktualnyCzas = 0;
unsigned long zapamietanyCzas = 0;
unsigned long roznicaCzasu = 0;

float temperature_read = 0.0;
float set_temperature = 70;
float PID_error = 0;
float previous_error = 0;
float elapsedTime, TimePID, timePrev;
int PID_value = 0;
int output = 0;

float kp = 15;   float ki = 0.15;   float kd = 50;
int PID_p = 0;    int PID_i = 0;    int PID_d = 0;

void setup() {
  lcd.begin(16, 4);
  Serial.begin(9600);
  pinMode(PWM_pin,OUTPUT);
  TCCR2B = TCCR2B & B11111000 | 0x03;    // pin 3 and 11 PWM frequency of 980.39 Hz
  TimePID = millis(); 
}
void loop(){
  temperature_read = thermocouple.readCelsius();  
  analogWrite(PWM_pin,output);
  aktualnyCzas = millis();
  roznicaCzasu = aktualnyCzas - zapamietanyCzas;
  
  if (roznicaCzasu >= 1000UL) {
    zapamietanyCzas = aktualnyCzas;
    //Serial.println(aktualnyCzas);
    output = PID(temperature_read);
    lcd_display();
  }
}



double PID(float inp){
  PID_error = set_temperature - inp;
  PID_p = kp * PID_error;
  //Calculate the I value in a range on +-3
  if(-3 < PID_error && PID_error < 4)
  {
    PID_i += PID_error * elapsedTime;
  }

  //For derivative we need real time to calculate speed change rate
  timePrev = TimePID;                            // the previous time is stored before the actual time read
  TimePID = millis();                            // actual time read
  elapsedTime = (TimePID - timePrev) / 1000; 
  
  //Now we can calculate the D calue
  PID_d = kd*((PID_error - previous_error)/elapsedTime);
  
  //Final total PID value is the sum of P + I + D
  PID_value = PID_p + PID_i + PID_d;

  if(PID_value < 0)
  {    PID_value = 0;    }
  if(PID_value > 255)  
  {    PID_value = 255;  }
  previous_error = PID_error;     //Remember to store the previous error for next loop.
  return PID_value;
}
void lcd_display(){
  char buffer[16];
  
  lcd.clear();                        // 
  lcd.setCursor(0,0);                 //    
  lcd.print("Zadana: ");              //
  lcd.setCursor(8,0);                 //
  lcd.print(set_temperature);         //
  lcd.setCursor(14,0);                //
  lcd.print("C");                     //
  lcd.setCursor(0,1);                 //
  lcd.print("Odczyt: ");              //
  lcd.setCursor(8,1);                 //
  lcd.print(temperature_read);        //   Wyświetlanie pierwszych trzech linijek lcd
  lcd.setCursor(14,1);                //
  lcd.print("C");                     //
  lcd.setCursor(0,2);                 //
  lcd.print("PWM: ");                 //
  lcd.setCursor(5,2);                 //
  lcd.print(PID_value);               //
  lcd.setCursor(11,2);                //  
  lcd.print((100*PID_value)/255);     //
  lcd.setCursor(14,2);                //
  lcd.print("%");                     //
  
  sprintf(buffer, "P:%d", PID_p);     //
  lcd.setCursor(0,3);                 //
  lcd.print(buffer);                  //
  sprintf(buffer, "I:%d",PID_i);      //
  lcd.setCursor(5,3);                 //  Wyświetlanie ostatniej linijki lcd
  lcd.print(buffer);                  //  w ten sposób nie wyświetlają się
  sprintf(buffer, "D:%d", PID_d);     //  "krzaki" na wyswietlaczu
  lcd.setCursor(10,3);                //
  lcd.print(buffer);                  //
}