#include <PID_v1.h>

#define PIN_INPUT A0  // Assuming LSA08 sensor is connected to analog pin 0
#define PIN_OUTPUT 3

// Define Variables we'll be connecting to
double Setpoint = 500; // Setpoint value for the line sensor
double Input, Output;

// Define the aggressive and conservative Tuning Parameters
double aggKp = 4, aggKi = 0.2, aggKd = 1;
double consKp = 1, consKi = 0.05, consKd = 0.25;

// Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);

void setup()
{
  // Initialize the serial communication for debugging
  Serial.begin(9600);
  
  // Turn the PID on
  myPID.SetMode(AUTOMATIC);
}

void loop()
{
  // Read the sensor input
  Input = analogRead(PIN_INPUT);
  
  // Print the sensor input for debugging
  Serial.print("Sensor Input: ");
  // Serial.println(Input);
  
  double gap = abs(Setpoint - Input); // Distance away from setpoint
  if (gap < 10)
  {  
    // We're close to setpoint, use conservative tuning parameters
    myPID.SetTunings(consKp, consKi, consKd);
  }
  else
  {
    // We're far from setpoint, use aggressive tuning parameters
    myPID.SetTunings(aggKp, aggKi, aggKd);
  }

  // Compute PID output
  myPID.Compute();
  
  // Print the PID output for debugging
  Serial.print("PID Output: ");
  Serial.println(Output);
  
  // Map the PID output to the PWM range (0-255) and write it to the output pin
  analogWrite(PIN_OUTPUT, map(Output, 0, 255, 0, 1023));
  
  delay(100); // Delay for stability
}