#include <Servo.h>

int pattern1[] = { 94, 97, 94, 97, 96, 95, 96, 97, 96, 97, 96, 97, 96, 97, 96, 97, 96, 115, 97, 75, 97, 75, 97, 115, 96, 97, 96, 97, 96, 97, 96, 115, 97, 75, 97, 75, 97, 115, 96 };
int pattern2[] = { 89, 92, 89, 92, 90, 115, 90, 141, 90, 91, 90, 91, 90, 91, 90, 91, 90, 91, 90, 91, 90, 91, 90, 91, 90, 40, 91, 65, 91, 115, 91, 91, 90, 91, 90, 91, 90, 91, 90 };
int pattern3[] = { 69, 72, 69, 72, 70, 20, 70, 45, 70, 121, 70, 20, 70, 20, 70, 121, 70, 121, 70, 20, 70, 20, 70, 121, 70, 121, 70, 95, 70, 95, 70, 121, 70, 20, 70, 20, 70, 121, 70 };
int pattern4[] = { 89, 92, 89, 92, 90, 141, 90, 115, 90, 141, 90, 40, 90, 40, 90, 141, 90, 141, 90, 40, 90, 40, 90, 141, 90, 40, 91, 65, 91, 65, 91, 141, 90, 40, 90, 40, 90, 141, 90 };
int pattern5[] = { 90, 91, 90, 91, 90, 91, 89, 91, 89, 91, 89, 91, 89, 91, 89, 91, 89, 50, 91, 130, 89, 130, 89, 50, 91, 89, 91, 89, 91, 50, 91, 130, 89, 130, 89, 50 };
int pattern6[] = { 90, 91, 90, 91, 90, 75, 91, 60, 91, 90, 91, 89, 91, 89, 91, 89, 91, 89, 91, 89, 91, 89, 91, 89, 91, 120, 89, 105, 91, 105, 89, 91, 89, 91, 89 };
int pattern7[] = { 90, 91, 90, 91, 90, 91, 89, 91, 89, 50, 91, 130, 89, 130, 89, 50, 91, 89, 91, 89, 91, 89, 91, 89, 91, 89, 91, 89, 91, 89, 91, 89, 91, 89, 91, 89 };

int patternLength = sizeof(pattern1) / sizeof(int);
int currentStep = 0;


// Define the pins for the servos
int pitchPin = 2;
int rollPin = 3;
int yawPin = 4;
int leftAileronPin = 5;
int rightAileronPin = 6;
int rudderPin = 7;
int elevatorPin = 8;

// Define the servos
Servo pitchServo;
Servo rollServo;
Servo yawServo;
Servo leftAileronServo;
Servo rightAileronServo;
Servo rudderServo;
Servo elevatorServo;

// Define the pins for the joysticks and autopilot button
int pitchJoystickXPin = A0;
int pitchJoystickYPin = A1;
int yawJoystickXPin = A2;
int yawJoystickYPin = A3;
int autopilotButtonPin = 9;

// Define the variables to store the joystick values and autopilot button state
int pitchJoystickXValue = 0;
int pitchJoystickYValue = 0;
int yawJoystickXValue = 0;
int yawJoystickYValue = 0;
int autopilotButtonState = 0;

// Define the autopilot sequence variables
int autopilotPitchValue = 90;
int autopilotRollValue = 90;
int autopilotYawValue = 90;
int autopilotLeftAileronValue = 90;
int autopilotRightAileronValue = 90;
int autopilotRudderValue = 90;
int autopilotElevatorValue = 90;

void setup() {
  // Attach the servos to their pins
  pitchServo.attach(pitchPin);
  rollServo.attach(rollPin);
  yawServo.attach(yawPin);
  leftAileronServo.attach(leftAileronPin);
  rightAileronServo.attach(rightAileronPin);
  rudderServo.attach(rudderPin);
  elevatorServo.attach(elevatorPin);

  // Set the autopilot button pin as an input
  pinMode(autopilotButtonPin, INPUT);
}

void loop() {
  
  int pos1 = pattern1[currentStep];
  int pos2 = pattern2[currentStep];
  int pos3 = pattern3[currentStep];
  int pos4 = pattern4[currentStep];
  int pos5 = pattern5[currentStep];
  int pos6 = pattern6[currentStep];
  int pos7 = pattern7[currentStep];

  int currentPos1 = servo1.read();
  int currentPos2 = servo2.read();
  int currentPos3 = servo3.read();
  int currentPos4 = servo4.read();
  int currentPos5 = servo5.read();
  int currentPos6 = servo6.read();
  int currentPos7 = servo7.read();

  // Read the joystick values
  pitchJoystickXValue = analogRead(pitchJoystickXPin);
  pitchJoystickYValue = analogRead(pitchJoystickYPin);
  yawJoystickXValue = analogRead(yawJoystickXPin);
  yawJoystickYValue = analogRead(yawJoystickYPin);

  // Read the autopilot button state
  autopilotButtonState = digitalRead(autopilotButtonPin);

  // If the autopilot button is pressed, activate the autopilot sequence
  if (autopilotButtonState == HIGH) {

  for (int i = 0; i <= 10; i++) {
    int targetPos1 = currentPos1 + ((pos1 - currentPos1) * i / 10);
    int targetPos2 = currentPos2 + ((pos2 - currentPos2) * i / 10);
    int targetPos3 = currentPos3 + ((pos3 - currentPos3) * i / 10);
    int targetPos4 = currentPos4 + ((pos4 - currentPos4) * i / 10);
    int targetPos5 = currentPos5 + ((pos5 - currentPos5) * i / 10);
    int targetPos6 = currentPos6 + ((pos6 - currentPos6) * i / 10);
    int targetPos7 = currentPos7 + ((pos7 - currentPos7) * i / 10);

    servo1.write(targetPos1);
    servo2.write(targetPos2);
    servo3.write(targetPos3);
    servo4.write(targetPos4);
    servo5.write(targetPos5);
    servo6.write(targetPos6);
    servo7.write(targetPos7);

    delay(75);  // adjust this delay time as needed for smoother movement
  }

  }

  } else {
    // Map the joystick values to servo values
    int pitchServoValue = map(pitchJoystickYValue, 0, 1023, 0, 180);
    int rollServoValue = map(pitchJoystickXValue, 0, 1023, 0, 180);
int yawServoValue = map(yawJoystickXValue, 0, 1023, 0, 180);
int leftAileronServoValue = map(rollServoValue, 0, 180, 0, 90);
int rightAileronServoValue = map(rollServoValue, 0, 180, 180, 90);
int rudderServoValue = map(yawServoValue, 0, 180, 0, 180);
int elevatorServoValue = map(pitchServoValue, 0, 180, 180, 0);
// Write the servo values
pitchServo.write(pitchServoValue);
rollServo.write(rollServoValue);
yawServo.write(yawServoValue);
leftAileronServo.write(leftAileronServoValue);
rightAileronServo.write(rightAileronServoValue);
rudderServo.write(rudderServoValue);
elevatorServo.write(elevatorServoValue);
}

// Add a delay to avoid servo jitter
delay(10);

currentStep = (currentStep + 1) % patternLength;
}