#include <ESP32Servo.h>

// Define pin connections
const int joyXPin1 = 34;  // Joystick 1 X-axis (for servo 1 in/out) - Use ADC1 pins (32-39)
const int joyYPin1 = 35;  // Joystick 1 Y-axis (not used) - Use ADC1 pins (32-39)
const int joyXPin2 = 32;  // Joystick 2 X-axis (for servo 2 rotation) - Use ADC1 pins (32-39)
const int joyYPin2 = 33;  // Joystick 2 Y-axis (not used) - Use ADC1 pins (32-39)
const int servoInOutPin = 14;   // Servo 1 pin (in/out movement)
const int servoRotatePin = 15;  // Servo 2 pin (rotation)

// Servo objects
Servo servoInOut;
Servo servoRotate;

// Servo position limits and mapping ranges
const int servoInOutMin = 0;    // Minimum position for in/out servo
const int servoInOutMax = 180;  // Maximum position for in/out servo
const int servoRotateMin = 0;   // Minimum rotation for rotation servo
const int servoRotateMax = 180; // Maximum rotation for rotation servo
const int joyMin = 0;        // Minimum joystick reading (theoretical)
const int joyMax = 4095;     // Maximum joystick reading (12-bit ADC)

// Dead zone for joystick
const int deadZone = 150; // Adjust as needed, now based on 12-bit ADC

// Number of samples for averaging joystick readings
const int numAvgSamples = 5;

// Variables for averaging
int joyX1Samples[numAvgSamples];
int joyX2Samples[numAvgSamples];
int sampleIndex = 0;
bool samplesFilled = false;

void setup() {
  // Initialize servos using ESP32Servo
  ESP32PWM::allocateTimer(0); //important to allocate a timer to the esp32servo library, we are using the first of 4 timers here
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);
  servoInOut.setPeriodHertz(50); // Standard 50Hz servo
  servoRotate.setPeriodHertz(50); 
  servoInOut.attach(servoInOutPin, 500, 2400); // (pin, min pulse width, max pulse width in microseconds)
  servoRotate.attach(servoRotatePin, 500, 2400);

  // Initialize serial communication for debugging (optional)
  Serial.begin(115200); 
}

int readJoystickAvg(int joyPin, int samples[], int numSamples) {
  // Shift existing samples
  for (int i = numSamples - 1; i > 0; i--) {
    samples[i] = samples[i - 1];
  }

  // Read new sample
  samples[0] = analogRead(joyPin);

  // Calculate average
  long sum = 0;
  int count = samplesFilled ? numSamples : sampleIndex+1;
  for (int i = 0; i < count; i++) {
    sum += samples[i];
  }
  return (int)(sum / count);
}

void loop() {
  // Read joystick values with averaging
  int joyX1 = readJoystickAvg(joyXPin1, joyX1Samples, numAvgSamples);
  int joyX2 = readJoystickAvg(joyXPin2, joyX2Samples, numAvgSamples);

  // Update sample index and flag
  sampleIndex = (sampleIndex + 1) % numAvgSamples;
  if (sampleIndex == 0) {
    samplesFilled = true;
  }

  // --- Control in/out servo with joystick 1 X-axis ---
  
  if (abs(joyX1 - (joyMax/2)) > deadZone) { 
    int servoInOutPos = map(joyX1, joyMin, joyMax, servoInOutMax, servoInOutMin); 
    servoInOutPos = constrain(servoInOutPos, servoInOutMin, servoInOutMax);
    servoInOut.write(servoInOutPos);

    // Debugging
    Serial.print("Joystick 1 X: ");
    Serial.print(joyX1);
    Serial.print("\tServo In/Out: ");
    Serial.println(servoInOutPos);
  }

  // --- Control rotation servo with joystick 2 X-axis ---

  if (abs(joyX2 - (joyMax/2)) > deadZone) {
    int servoRotatePos = map(joyX2, joyMin, joyMax, servoRotateMin, servoRotateMax);
    servoRotatePos = constrain(servoRotatePos, servoRotateMin, servoRotateMax);
    servoRotate.write(servoRotatePos);

    // Debugging
    Serial.print("Joystick 2 X: ");
    Serial.print(joyX2);
    Serial.print("\tServo Rotate: ");
    Serial.println(servoRotatePos);
  }
}