// Import the FastPID library for PID control
#include <FastPID.h>
// Import the L298N library for motor control
#include <L298N.h>
// Import the L298NX2 library
#include <L298NX2.h>

// Define a Finger_Controller class
class Finger_Controller
{
  private:
    // L298N object for motor control
    L298N motor;
    // FastPID object for PID control
    FastPID myPID;
    // Target position for the motor
    int targetpos;
    // Pin for the potentiometer
    int potpin;

  public:
    // Finger_Controller constructor
    Finger_Controller(int EN, int IN1, int IN2, int pin)
      : motor(EN, IN1, IN2),myPID(2, 0.001, 0.1, 10, 16, true)
    {
      // Set up the pins for the motor
      pinMode(EN, OUTPUT);
      pinMode(IN1, OUTPUT);
      pinMode(IN2, OUTPUT);
      // Set the output range of the PID controller
      myPID.setOutputRange(-255, 255);
      // Initialize the target position
      targetpos = 512;
      // Initialize the potentiometer pin
      potpin = pin;
    }

    // Method to set the target position
    void set_target(int target)
    {
      targetpos = target;
    }

    // Method to update the motor position
    void update()
    {
      // Read the current position from the potentiometer
      int positionfeed = analogRead(potpin);
      // Get the PID output based on the current and target position
      int16_t output = myPID.step(targetpos, positionfeed);
      // Print debug information to the serial monitor
      Serial.print(targetpos);
      Serial.print(" ");
      Serial.print(positionfeed);
      Serial.print(" ");
      Serial.println(output);

      // If the output is positive, move the motor forward
      if (output > 0)
      {
        motor.setSpeed(output);
        motor.forward();
        delay(100);
      }
      // If the output is negative, move the motor backward
      else if (output < 0)
      {
        motor.setSpeed(abs(output));
        motor.backward();
        delay(100);
      }
      // If the output is zero, pause for 100ms
      else
        delay(100);

    }
};

// Create two Finger_Controller objects
Finger_Controller finger1(3, 2, 4, A0);
Finger_Controller finger2(6, 7, 8, A1);

void setup() {

  // Begin the serial communication
  Serial.begin(9600);

  // Set the initial target positions for the motors
  finger1.set_target(500);
  finger2.set_target(500);

}

void loop() {

  // Update the motor positions in the main loop
  finger1.update();
  finger2.update();

}