#define BLYNK_TEMPLATE_ID "TMPL6_NeSrwZR"
#define BLYNK_TEMPLATE_NAME "SBR"
#define BLYNK_AUTH_TOKEN "YL3I1G5pwTiagFxBlMnTKt4rdY0kmbwl"
#define BLYNK_PRINT Serial

#include <WiFi.h>
#include <Wire.h>
#include <MPU6050_tockn.h>
#include <BlynkSimpleEsp32.h>
#include <PID_v1.h>

char auth[] = BLYNK_AUTH_TOKEN;
char ssid[] = "Wokwi-GUEST";
char pass[] = "";

MPU6050 mpu6050(Wire);

double Setpoint = 0;  // Setpoint for balancing
double Input, Output;

double Kp;
double Ki;
double Kd;

PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

// Motor control pins
int leftStepPin = 32;
int leftDirPin = 33;
int rightStepPin = 14;
int rightDirPin = 12;
int enable = 19;

// Motor control state
enum MotorState {
  MOTOR_IDLE,
  MOTOR_STEP_HIGH,
  MOTOR_STEP_LOW
};
MotorState motorState = MOTOR_IDLE;
unsigned long motorLastStepTime = 0;
const int motorStepInterval = 10000; // microseconds

// Define a maximum speed limit for the motor
const int maxMotorSpeed = 500; // Adjust this value based on your requirements

BLYNK_WRITE(V9) {
  Kp = param.asDouble();
}

BLYNK_WRITE(V10) {
  Ki = param.asDouble();
}

BLYNK_WRITE(V11) {
  Kd = param.asDouble();
}

void setup() {
  Serial.begin(9600);
  Blynk.begin(auth, ssid, pass);
  Wire.begin();
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);

  pinMode(leftStepPin, OUTPUT);
  pinMode(leftDirPin, OUTPUT);
  pinMode(rightStepPin, OUTPUT);
  pinMode(rightDirPin, OUTPUT);
  pinMode(enable, OUTPUT);
  digitalWrite(enable, LOW);

  myPID.SetOutputLimits(-500, 500);
  myPID.SetSampleTime(10);
  myPID.SetMode(AUTOMATIC);
}

void loop() {
  Blynk.run();

  mpu6050.update();
  double pitch = mpu6050.getAngleY(); // Tilt angle
  double gyroY = mpu6050.getGyroY(); // Gyro Y output

  // Sensor fusion for PID input
  Input = pitch; // Adjust the ratio based on testing

  myPID.SetTunings(Kp, Ki, Kd);
  myPID.Compute();

  // Non-blocking motor control
  unsigned long currentTime = micros();
  if (currentTime - motorLastStepTime >= motorStepInterval) {
    motorLastStepTime = currentTime;

    // Inversely scale the PID output for motor speed
    int motorSpeed = map(abs(Output*2), 0, 500, maxMotorSpeed, 0);

    // Limit the motor speed to the maximum defined
    motorSpeed = min(motorSpeed, maxMotorSpeed);

    // Threshold for considering the output as zero
    const double zeroThreshold = 5.0;

    // Check if the PID output is close enough to zero
    if (abs(Output) < zeroThreshold) {
      digitalWrite(enable, HIGH); // Stop the motors
    } else {
      digitalWrite(enable, LOW); // Enable the motors
      digitalWrite(leftDirPin, Output < 0 ? HIGH : LOW);
      digitalWrite(rightDirPin, Output > 0 ? HIGH : LOW);
      analogWrite(leftStepPin, motorSpeed);
      analogWrite(rightStepPin, motorSpeed);
    }
  }

  // Reduced frequency of Serial output for debugging
  static unsigned long lastPrintTime = 0;
  if (millis() - lastPrintTime > 500) {
    Serial.print("Pitch: "); Serial.print(pitch);
    Serial.print(" | Gyro Y: "); Serial.print(gyroY);
    Serial.print(" | PID Input: "); Serial.print(Input);
    Serial.print(" | Output: "); Serial.println(Output);
    lastPrintTime = millis();
  }
}