#pragma once
#include <FastPID.h>
#include <L298NX2.h>

class Finger_Controller {
  public:
    // Constructor for Finger_Controller class
    Finger_Controller(bool finger_A, int potentiometer_pin)
      : finger_A(finger_A), potpin(potentiometer_pin), myPID(2, 0.001, 0.1, 10, 16, true)
    {
      myPID.setOutputRange(-255, 255);
      targetpos = 512;
    }

    // Copy constructor for Finger_Controller class
    Finger_Controller(const Finger_Controller &other)
      : finger_A(other.finger_A), motor_handler(other.motor_handler),
        targetpos(other.targetpos), potpin(other.potpin),
        debug_mode(other.debug_mode), myPID(1, 0.001, 0.1, 10, 16, true)
    {
      myPID.setOutputRange(-255, 255);
    }

    // Set the motor handler for the Finger_Controller
    void set_motor_handler(L298NX2* handler) {
      motor_handler = handler;
    }

    // Set the target position for the Finger_Controller
    void set_target(int target) {
      targetpos = target;
    }

    // Enable debug mode for the Finger_Controller
    void enableDebug() {
      debug_mode = true;
    }

    // Disable debug mode for the Finger_Controller
    void disableDebug() {
      debug_mode = false;
    }

    // Update the Finger_Controller
    void update() {
      int positionfeed = analogRead(potpin);
      int16_t output = myPID.step(targetpos, positionfeed);

      if (debug_mode) {
        Serial.print(targetpos);
        Serial.print(" ");
        Serial.print(positionfeed);
        Serial.print(" ");
        Serial.println(output);
      }
      int dt = 50;
      if (finger_A) {
        if (output > 0) {
          motor_handler->setSpeedA(output);
          motor_handler->forwardA();
          delay(dt);
        } else if (output < 0) {
          motor_handler->setSpeedA(abs(output));
          motor_handler->backwardA();
          delay(dt);
        } else
          delay(dt);
      }
      else {
        if (output > 0) {
          motor_handler->setSpeedB(output);
          motor_handler->forwardB();
          delay(dt);
        } else if (output < 0) {
          motor_handler->setSpeedB(abs(output));
          motor_handler->backwardB();
          delay(dt);
        } else
          delay(dt);
      }
    }

  private:
    bool finger_A;
    bool debug_mode;
    int targetpos;
    int potpin;
    L298NX2* motor_handler;
    FastPID myPID;
};

class Finger_Controller_X2 {
  public:
    // Constructor for Finger_Controller_X2 class
    Finger_Controller_X2(int EN_A, int IN1_A, int IN2_A, int potentiometer_pin_A, int EN_B, int IN1_B, int IN2_B, int potentiometer_pin_B)
      : finger_A(true, potentiometer_pin_A), finger_B(false, potentiometer_pin_B)
    {
      pinMode(EN_A, OUTPUT);
      pinMode(IN1_A, OUTPUT);
      pinMode(IN2_A, OUTPUT);
      pinMode(EN_B, OUTPUT);
      pinMode(IN1_B, OUTPUT);
      pinMode(IN2_B, OUTPUT);

      motors = new L298NX2(EN_A, IN1_A, IN2_A, EN_B, IN1_B, IN2_B);
      finger_A.set_motor_handler(motors);
      finger_B.set_motor_handler(motors);
    }

    Finger_Controller finger_A;
    Finger_Controller finger_B;

  private:
    L298NX2* motors;
};

// Create instances of Finger_Controller_X2 and Finger_Controller
Finger_Controller_X2 fingers(3, 2, 4, A0, 6, 7, 8, A1);
Finger_Controller finger1 = fingers.finger_A;
Finger_Controller finger2 = fingers.finger_B;

Finger_Controller_X2 fingers2(11, 12, 13, A2, 5, 10, 9, A3);
Finger_Controller finger3 = fingers2.finger_A;
Finger_Controller finger4 = fingers2.finger_B;

void setup() {

  //Serial.begin(9600);

}

void loop() {

  // Serial.print("1. ");
  //  finger1.update();
  //  Serial.print("2. ");
  //  finger2.update();

  // Set target positions for the fingers
  finger1.set_target(900);//4
  finger2.set_target(900);//2
  finger3.set_target(900);//1,5
  finger4.set_target(900);//3

  // Update the fingers for a certain number of iterations
  for (int i = 0; i < 30; i++)
  {
    finger1.update();
    finger2.update();
    finger3.update();
    finger4.update();
  }

  // Set new target positions for the fingers
  finger1.set_target(40);//4
  finger2.set_target(40);//2
  finger3.set_target(40);//1,5
  finger4.set_target(40);//3

  // Update the fingers again for a certain number of iterations
  for (int i = 0; i < 30; i++)
  {
    finger1.update();
    finger2.update();
    finger3.update();
    finger4.update();
  }

  // Repeat the above steps for different target positions

  /*
    finger1.set_target(40);
    finger2.set_target(40);
    finger3.set_target(40);
    finger4.set_target(40);

    for (int i = 0; i < 30; i++)
    {
    finger1.update();
    finger2.update();
    finger3.update();
    finger4.update();
    }
    finger1.set_target(1000);
    finger2.set_target(1000);
    finger3.set_target(1000);
    finger4.set_target(1000);

    for (int i = 0; i < 30; i++)
    {
    finger1.update();
    finger2.update();
    finger3.update();
    finger4.update();
    }
  */

}