#include <AccelStepper.h>

// Define stepper motor connections
#define MOTOR_INTERFACE_TYPE 1  // Using a driver with STEP and DIR pins
const int stepPin = 12;
const int dirPin = 14;

// Define potentiometer pins
const int posPotPin = 34;   // Position control
const int speedPin = 27;   // speed control
const int pPotPin = 35;     // Proportional control
const int iPotPin = 32;     // Integral control
const int dPotPin = 33;     // Derivative control

// Create stepper motor instance
AccelStepper motor(MOTOR_INTERFACE_TYPE, stepPin, dirPin);

// PID Control variables
float Kp, Ki, Kd;
float targetPosition = 0;
float currentPosition = 0;
float error = 0, lastError = 0, integral = 0, derivative = 0;
unsigned long lastTime = 0;
const float sampleTime = 0.1; // 100 milliseconds sample time

// Motor control variables
float maxSpeed = 2000.0;  // Maximum steps per second
int Accel = 5000;
float pidOutput = 0;

void setup() {
  Serial.begin(115200);
  
  // Configure motor parameters
  motor.setMaxSpeed(maxSpeed);
  motor.setAcceleration(Accel); // Optional acceleration setting
  
  // Initialize potentiometer pins
  pinMode(posPotPin, INPUT);
  pinMode(speedPin, INPUT);
  pinMode(pPotPin, INPUT);
  pinMode(iPotPin, INPUT);
  pinMode(dPotPin, INPUT);
}

void loop() {
  
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - lastTime) / 1000.0; // Convert to seconds

  // Read all potentiometers
  int posRaw = analogRead(posPotPin);
  int speedRaw = analogRead(speedPin);
  int pRaw = analogRead(pPotPin);
  int iRaw = analogRead(iPotPin);
  int dRaw = analogRead(dPotPin);

  // Map potentiometer values to appropriate ranges
  targetPosition = map((long)posRaw, 0, 4095, 0, 200);  // 0-2000 steps range
  maxSpeed = map((long)speedRaw, 0, 4095, 1, 5000);  // 1-5000 steps range
  Kp = map((float)pRaw, 0, 4095, 1, 20.0);     // Proportional range 1-20
  Ki = map((float)iRaw, 0, 4095, 0, 0.5);     // Integral range 0-0.5
  Kd = map((float)dRaw, 0, 4095, 0, 0.5);     // Derivative range 0-0.5

  // PID calculations at fixed intervals
  if (deltaTime >= sampleTime) {
    currentPosition = motor.currentPosition();
    error = targetPosition - currentPosition;

    // Calculate integral term with anti-windup
    integral += error * deltaTime;
    integral = constrain(integral, -100.0, 100.0); // Prevent integral windup

    // Calculate derivative term
    derivative = (error - lastError) / deltaTime;

    // Compute PID output
    pidOutput = (Kp * error) + (Ki * integral) + (Kd * derivative);
    
    // Constrain output to max speed
    pidOutput = constrain(pidOutput, -maxSpeed, maxSpeed);
    
    // Apply speed to motor
    motor.setSpeed(pidOutput);

    // Update previous values
    lastError = error;
    lastTime = currentTime;
  }

  // Execute motor movement
  motor.runSpeed();

  // Serial monitoring for debugging (optional)
  Serial.print("Target: ");
  Serial.print(targetPosition);
  Serial.print(" Current: ");
  Serial.print(currentPosition);
  Serial.print(" PID Out: ");
  Serial.print(pidOutput);
  Serial.print(" Kp: ");
  Serial.print(Kp);
  Serial.print(" Ki: ");
  Serial.print(Ki);
  Serial.print(" Kd: ");
  Serial.println(Kd);
}

// Helper function to map analog readings to float ranges
float map(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}