#include <PubSubClient.h>
#include <WiFi.h>

#include <Stepper.h>
Stepper myStepper(400, 27, 14, 12, 13);
float kesalahan;
float integralKesalahan;
float derivatifKesalahan;
float kesalahanLalu = 0;
float Kp, Ki, Kd;
float input;

const char* mqttServer    = "armadillo.rmq.cloudamqp.com";
const int mqttPort        = 1883;
const char* mqttUser      = "qodzxomh:qodzxomh";
const char* mqttPassword  = "OrRU5nkIkEjZ7lUmyQp_yEhT4uuofkmq";

WiFiClient espClient;
PubSubClient client(espClient);

int statspeed,statsuhu;
 
void setup() 
{
  
  myStepper.setSpeed(100);

  input = 25;
  Kp = 0;
  Ki = 0;
  Kd = 0;

  Serial.begin(115200); 
 
  WiFi.begin("Wokwi-GUEST", "", 6);
 
  while (WiFi.status() != WL_CONNECTED) 
  {
    delay(500);
    Serial.println("Connecting to WiFi..");
  }
  Serial.println("Connected to the WiFi network");
 
  client.setServer(mqttServer, mqttPort);
  client.setCallback(callback);
 
  while (!client.connected()) 
  {
    Serial.println("Connecting to MQTT...");
 
    if (client.connect("mqtt", mqttUser, mqttPassword )) 
    {
      Serial.println("connected");  
    } else
     {
      Serial.print("failed with state ");
      Serial.print(client.state());
      delay(2000);
    }
  }
  ///Topic name
  // statspeed = client.subscribe("esp/Speed");
  // statsuhu  = client.subscribe("esp/Suhu");
}
 
void callback(char* topic, byte* payload, unsigned int length) 
{
  Serial.print("Message arrived in topic: ");
  Serial.println(topic);
 
  Serial.print("Message:");
  for (int i = 0; i < length; i++) 
  {
    statspeed = (char)payload[i];
    Serial.print((char)payload[i]);
  }
  Serial.println();
  Serial.println("-----------------------");
}
 
void loop() 
{
  float sensor = analogRead(34);
  sensor = map(sensor,0,4095,20,45);

  float respon = hitungPID(input, sensor);

  if(respon <=0)
  {
    respon = 0;
  }

  if(respon >= 20)
  {
    respon = 20;
  }
  
  if(sensor <= 25)
  {
    Kp = 0;
    Kd = 0;
  }
  
  if(respon <= 19)
  {
    Kp = Kp - 0.01;
    Kd = Kd + 0.01;
  }

  if(respon >= 19)
  {
    Kp = Kp;
    Kd = Kd;
  }

  myStepper.step(respon);

  Serial.print(sensor);
  Serial.print("||");
  Serial.print(Kp);
  Serial.print(",");
  Serial.print(Ki);
  Serial.print(",");
  Serial.print(Kd);
  Serial.print("||");
  Serial.print(input);
  Serial.print(",");
  Serial.print(sensor);
  Serial.print(",");
  Serial.println(respon);
  delay(10);

  client.publish("esp/Suhu", String(sensor).c_str(), true);
  client.publish("esp/Speed",String(respon).c_str(), true);
  client.loop();
  
}

float hitungPID(float input, float sensor)
{
  kesalahan = input - sensor;
  integralKesalahan += kesalahan;
  derivatifKesalahan = kesalahan - kesalahanLalu;
  kesalahanLalu = kesalahan;
   
  return (Kp * kesalahan) + (Ki * integralKesalahan) + (Kd * derivatifKesalahan);
}