#include <Servo.h>

Servo servoX;
Servo servoY;

const int joystickXPin = A1;        // Joystick X-axis pin (inverted)
const int joystickYPin = A0;        // Joystick Y-axis pin

const int servoXPin = 9;            // Pin for servo X
const int servoYPin = 10;           // Pin for servo Y

const int servoXStartPosition = 80;    // Starting position for servo X in degrees (adjusted)
const int servoYStartPosition = 80;    // Starting position for servo Y in degrees (adjusted)

const int servoXMaxForwardDelta = 40;  // Maximum change in degrees for servo X when moving forward
const int servoXMaxBackwardDelta = 40; // Maximum change in degrees for servo X when moving backward
const int servoYMaxForwardDelta = 40;  // Maximum change in degrees for servo Y when moving forward
const int servoYMaxBackwardDelta = 40; // Maximum change in degrees for servo Y when moving backward

const float servoChangeRate = 0.05;    // Rate of change for servo position adjustment (adjust as needed)
const int responsivenessDelay = 20;     // Delay for responsiveness (adjust as needed)

int currentXPosition = servoXStartPosition;  // Current position for servo X
int currentYPosition = servoYStartPosition;  // Current position for servo Y

bool flipX = false; // Flag to indicate whether to flip X-axis input
bool flipY = false; // Flag to indicate whether to flip Y-axis input

void setup() {
  Serial.begin(9600); // Initialize serial communication
  pinMode(joystickXPin, INPUT);   // Set joystick X pin as input
  pinMode(joystickYPin, INPUT);   // Set joystick Y pin as input
  servoX.attach(servoXPin);       // Attach servo X to its pin
  servoY.attach(servoYPin);       // Attach servo Y to its pin
  
  // Set initial positions for servos
  servoX.write(servoXStartPosition);
  servoY.write(servoYStartPosition);
}

void loop() {
  int joystickXValue = analogRead(joystickXPin);  // Read joystick X value
  if (flipX) {
    joystickXValue = 1023 - joystickXValue; // Invert joystick X value if flipX is true
  }
  
  int joystickYValue = analogRead(joystickYPin);  // Read joystick Y value
  if (flipY) {
    joystickYValue = 1023 - joystickYValue; // Invert joystick Y value if flipY is true
  }

  // Map joystick values to servo positions
  int targetXPosition = servoXStartPosition + map(joystickXValue, 0, 1023, -servoXMaxBackwardDelta, servoXMaxForwardDelta);
  int targetYPosition = servoYStartPosition + map(joystickYValue, 0, 1023, -servoYMaxBackwardDelta, servoYMaxForwardDelta);

  // Calculate change in positions based on target positions
  int deltaX = targetXPosition - currentXPosition;
  int deltaY = targetYPosition - currentYPosition;

  // Adjust current positions based on change rate
  currentXPosition += deltaX * servoChangeRate;
  currentYPosition += deltaY * servoChangeRate;

  // Write mapped values to servos
  servoX.write(constrain(currentXPosition, servoXStartPosition - servoXMaxBackwardDelta, servoXStartPosition + servoXMaxForwardDelta));
  servoY.write(constrain(currentYPosition, servoYStartPosition - servoYMaxBackwardDelta, servoYStartPosition + servoYMaxForwardDelta));

  // Output data to console
  Serial.print("Joystick X: ");
  Serial.print(joystickXValue);
  Serial.print("\tJoystick Y: ");
  Serial.print(joystickYValue);
  Serial.print("\tTarget X Position: ");
  Serial.print(targetXPosition);
  Serial.print("\tTarget Y Position: ");
  Serial.print(targetYPosition);
  Serial.print("\tCurrent X Position: ");
  Serial.print(currentXPosition);
  Serial.print("\tCurrent Y Position: ");
  Serial.println(currentYPosition);

  delay(responsivenessDelay); // Adjust delay for responsiveness
}