#include <Wire.h>
#include <SPI.h>

#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_ILI9341.h>

// potencjometr - "czujnik temperatury"
const int potPin = A0;

// wyjscie PWM (sterowanie "grzalka")
const int pwmPin = 5;

// przyciski do zmiany SP
const int btnUpPin   = 2;  // +0.1°C
const int btnDownPin = 3;  // -0.1°C

// TFT ILI9341 (SPI)
#define TFT_DC 9
#define TFT_CS 10
Adafruit_ILI9341 tft(TFT_CS, TFT_DC);

// OLED SSD1306 (I2C)
#define OLED_WIDTH 128
#define OLED_HEIGHT 64
#define OLED_ADDR 0x3C
Adafruit_SSD1306 oled(OLED_WIDTH, OLED_HEIGHT, &Wire, -1);

// PID
float setpoint = 0.0f;   // zadana "temperatura" [0..100]
float pv = 0.0f;          // wartosc mierzona
float errorPID = 0.0f;
float prevError = 0.0f;
float integral = 0.0f;
float control = 0.0f;     // sterowanie w [%] 0..100

// Nastawy PID
float Kp = 2.0f;
float Ki = 0.8f;
float Kd = 0.1f;

// Krok czasowy PID
const float dt = 0.05f;       // 50 ms

// Bufor do wykresu na OLED
float pvHistory[OLED_WIDTH];  // 128 probek

unsigned long lastUpdate = 0;
const unsigned long sampleTimeMs = 50; // 50 ms

// Prototypy
void handleSerialSetpoint();
void handleButtons();
void updateTFT(float errorPID, int pwmValue);
void updateOLEDGraph(float pvValue);
int mapPvToOLEDY(float pvVal);

void setup() {
  pinMode(pwmPin, OUTPUT);
  pinMode(potPin, INPUT);

  pinMode(btnUpPin,   INPUT_PULLUP);
  pinMode(btnDownPin, INPUT_PULLUP);

  Serial.begin(115200);
  Serial.println("Arduino Mega - PID + TFT + OLED");
  Serial.println("Podaj nowa wartosc zadana (0..100) i Enter.");
  Serial.println("Przyciski: D2 = +0.1C, D3 = -0.1C");

  // I2C
  Wire.begin();

  delay(100);                        // chwila na ustabilizowanie się ADC
  int rawStart = analogRead(potPin); // 0..1023
  pv = (float)rawStart * 100.0f / 1023.0f;
  setpoint = pv;                     // SP = aktualna temperatura
  integral = 0.0f;
  prevError = 0.0f;
  control  = 0.0f;
  Serial.print("Startowy SP ustawiony na PV: ");
  Serial.println(setpoint, 1);

  // OLED
  if (!oled.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    while (1) {}
  }
  oled.clearDisplay();
  oled.setTextSize(1);
  oled.setTextColor(SSD1306_WHITE);
  oled.setCursor(0, 0);
  oled.println("PV trend");
  oled.display();

  // Zeruj historie PV
  for (int i = 0; i < OLED_WIDTH; i++) {
    pvHistory[i] = setpoint;
  }

  // TFT
  tft.begin();
  tft.setRotation(1); // poziomo
  tft.fillScreen(ILI9341_BLACK);

  tft.setTextSize(2);
  tft.setTextColor(ILI9341_YELLOW, ILI9341_BLACK);
  tft.setCursor(10, 10);
  tft.print("PID Temperature");

  // Opisy pol
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE, ILI9341_BLACK);

  tft.setCursor(10, 40);
  tft.print("SP:");

  tft.setCursor(10, 60);
  tft.print("PV:");

  tft.setCursor(10, 80);
  tft.print("e :");

  tft.setCursor(10, 100);
  tft.print("u%:");

  tft.setCursor(180, 40);
  tft.print("PWM:");

  tft.setCursor(180, 70);
  tft.print("Kp:");
  tft.setCursor(180, 90);
  tft.print("Ki:");
  tft.setCursor(180, 110);
  tft.print("Kd:");
}

void loop() {
  unsigned long now = millis();

  // zmiana SP z Serial
  handleSerialSetpoint();
  // zmiana SP z przyciskow
  handleButtons();

  if (now - lastUpdate >= sampleTimeMs) {
    lastUpdate = now;

    // Pomiar "temperatury"
    int raw = analogRead(potPin); // 0-1023
    pv = (float)raw * 100.0f / 1023.0f; // 0-100

    // PID obliczenia
    errorPID = setpoint - pv;
    integral += errorPID * dt;
    float derivative = (errorPID - prevError) / dt;
    control = Kp * errorPID + Ki * integral + Kd * derivative;
    prevError = errorPID;

    // saturacja sterowania
    if (control < 0.0f) control = 0.0f;
    if (control > 100.0f) control = 100.0f;

    // PWM 0 - 255
    int pwmValue = (int)(control / 100.0f * 255.0f);
    if (pwmValue < 0) pwmValue = 0;
    if (pwmValue > 255) pwmValue = 255;

    analogWrite(pwmPin, pwmValue);

    // Aktualizacje wyswietlaczy
    updateTFT(errorPID, pwmValue);
    updateOLEDGraph(pv);
  }
}

// Zmiana SP z Serial (np. 37.5 + Enter)
void handleSerialSetpoint() {
  if (Serial.available()) {
    float sp = Serial.parseFloat();
    if (sp >= 0.0f && sp <= 100.0f) {
      setpoint = sp;
      Serial.print("Nowa wartosc zadana: ");
      Serial.println(setpoint, 1);
    } else {
      Serial.println("SP spoza zakresu [0..100]");
    }
    while (Serial.available()) Serial.read();
  }
}

// Zmiana SP z przyciskow +0.1 / -0.1
void handleButtons() {
  static int lastUpState = HIGH;
  static int lastDownState = HIGH;

  int upState   = digitalRead(btnUpPin);
  int downState = digitalRead(btnDownPin);

  // reagujemy na zbocze opadajace (HIGH -> LOW)
  if (lastUpState == HIGH && upState == LOW) {
    setpoint += 0.1f;
    if (setpoint > 100.0f) setpoint = 100.0f;
    Serial.print("Nowa wartosc zadana: ");
    Serial.println(setpoint, 1);
  }

  if (lastDownState == HIGH && downState == LOW) {
    setpoint -= 0.1f;
    if (setpoint < 0.0f) setpoint = 0.0f;
    Serial.print("Nowa wartosc zadana: ");
    Serial.println(setpoint, 1);
  }

  lastUpState = upState;
  lastDownState = downState;
}

// TFT: wyswietlanie wszystkiego
void updateTFT(float errorPID, int pwmValue) {
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_CYAN, ILI9341_BLACK);

  // SP
  tft.fillRect(60, 40, 80, 16, ILI9341_BLACK);
  tft.setCursor(60, 40);
  tft.print(setpoint, 1);

  // PV
  tft.fillRect(60, 60, 80, 16, ILI9341_BLACK);
  tft.setCursor(60, 60);
  tft.print(pv, 1);

  // e
  tft.fillRect(60, 80, 120, 16, ILI9341_BLACK);
  tft.setCursor(60, 80);
  tft.print(errorPID, 2);

  // u%
  tft.fillRect(60, 100, 80, 16, ILI9341_BLACK);
  tft.setCursor(60, 100);
  tft.print(control, 1);

  // PWM
  tft.fillRect(240, 40, 70, 16, ILI9341_BLACK);
  tft.setCursor(240, 40);
  tft.print(pwmValue);

  // Kp, Ki, Kd
  tft.fillRect(220, 70, 90, 16, ILI9341_BLACK);
  tft.setCursor(220, 70);
  tft.print(Kp, 2);

  tft.fillRect(220, 90, 90, 16, ILI9341_BLACK);
  tft.setCursor(220, 90);
  tft.print(Ki, 2);

  tft.fillRect(220, 110, 90, 16, ILI9341_BLACK);
  tft.setCursor(220, 110);
  tft.print(Kd, 2);
}

// OLED: wykres PV w czasie
void updateOLEDGraph(float pvValue) {
  // przesuniecie wykresu
  for (int i = 0; i < OLED_WIDTH - 1; i++) {
    pvHistory[i] = pvHistory[i + 1];
  }
  pvHistory[OLED_WIDTH - 1] = pvValue;

  oled.clearDisplay();

  // opis
  oled.setTextSize(1);
  oled.setCursor(0, 0);
  oled.print("PV (");
  oled.print(pvValue, 1);
  oled.print("C)");

  // os X
  oled.drawLine(0, OLED_HEIGHT - 1, OLED_WIDTH - 1, OLED_HEIGHT - 1, SSD1306_WHITE);

  // wykres
  for (int x = 1; x < OLED_WIDTH; x++) {
    int y1 = mapPvToOLEDY(pvHistory[x - 1]);
    int y2 = mapPvToOLEDY(pvHistory[x]);
    oled.drawLine(x - 1, y1, x, y2, SSD1306_WHITE);
  }

  oled.display();
}

// PV 0 - 100 -> Y na OLED (od dolu do gory, troche marginesu)
int mapPvToOLEDY(float pvVal) {
  if (pvVal < 0.0f) pvVal = 0.0f;
  if (pvVal > 100.0f) pvVal = 100.0f;

  int yTop = 10;
  int yBottom = OLED_HEIGHT - 5;
  long y = map((long)(pvVal * 10), 0, 1000, yBottom, yTop);
  return (int)y;
}